# Roboter-Pose-Erkennung aus Mehrkamera-ArUco-Aufnahmen

Aus synchronen Fotos eines Roboterarms von mehreren kalibrierten Kameras werden
die sieben Gelenkstellungen (`x, y, z, a, b, c, e`) rekonstruiert. Auf dem
Roboter und einer fixen Bodenplatte (*Board*) sitzen ArUco-Marker. Aus den
**vier Eckpunkten** jedes Markers wird per Mehrsicht-Triangulation eine volle
6-DOF-Markerpose (Position **und** Flächennormale) bestimmt; daraus werden über
das Vorwärtskinematik-Modell die Gelenkwinkel geschätzt.

> Die ausführliche Fassung mit allen Herleitungen liegt als LaTeX/PDF unter
> [`doc/pipeline.tex`](doc/pipeline.tex).

## Ergebnis

Benchmark über 10 valide Sim-Posen (Grundwahrheit bekannt), Methode **hybrid**:

| Metrik | Wert |
|---|---|
| Winkelfehler Mittel | **0,25°** |
| Winkelfehler Std (Stabilität) | 0,13° |
| Winkelfehler schlechtester | 1,57° |
| Positionsfehler Mittel | 0,10 mm |

Zwei Vorbedingungen waren entscheidend: (1) korrekte Kamera-Intrinsik (ein
früherer ~15 %-Brennweitenfehler bei 16:9 in der npz-Erzeugung wurde behoben);
(2) Eck- statt Center-Triangulation — liefert pro Marker eine Normale (~1,2°
genau, validiert in `benchmark/stage0_corner_normals.py`).

## Grundidee

Ein ArUco-Marker ist **kein Punkt**, sondern ein ebenes Quadrat bekannter
Kantenlänge mit vier detektierten Ecken. Trianguliert man diese vier Ecken über
mehrere Kameras, liefert **jeder einzelne Marker** neben der Position auch eine
gemessene Orientierung. Damit genügt ein einziger sichtbarer Marker, um den
Winkel des Gliedes zu bestimmen, auf dem er sitzt.

## Pipeline-Architektur

Aufruf: `run/run_pipeline.bat`

| Stufe | Skript | Ausgabe |
|---|---|---|
| 1 | `1_detect_aruco_observations.py` | `*_aruco_detection.json` (inkl. 4 Ecken) |
| 2 | `2_estimate_camera_from_observations.py` | `*_camera_pose.json` |
| 3 | `3_multiview_bundle_adjustment_v4.py` | `aruco_positions_initial.json` (Center, für Viewer) |
| 3b | `3b_corner_marker_poses.py` | `aruco_marker_poses.json` (Eck-Pose + Normale) |
| 4 | `pose_estimation.py` | `robot_state.json` (Gelenkwinkel + Konfidenz) |

### Stufe 1 — ArUco-Detektion
Pro Bild liefert OpenCV je Marker die vier Eckpunkte (im Uhrzeigersinn) plus
Qualitätsmaße (Fläche, Schärfe, Kontrast, Randabstand). Die Ecken sind die
primären Messungen; der Mittelpunkt ist nur ihr Mittelwert.

### Stufe 2 — Kamera-Pose
Die Board-Marker haben bekannte Weltpositionen (aus `robot.json`). Aus ihren
beobachteten Mittelpunkten wird die Kamerapose geschätzt: `solvePnP`-Initial,
dann Levenberg-Marquardt auf den normierten Reprojektionsresiduen
`min_θ Σ ‖ũ_i − π̃(R(ω)X_i + t)‖²` mit `θ = (ω, t)` (Rodrigues-Rotation).
*Hinweis:* Da die Board-Marker koplanar sind, ist die Tiefe schlecht
konditioniert — korrekte Intrinsik ist Pflicht.

### Stufe 3b — Eck-Triangulation + Normale
Jede der 4 Ecken wird per **DLT** aus allen sie sehenden Kameras trianguliert
(SVD des gestapelten Gleichungssystems mit `P_c = [R_c | t_c]` in normierten
Koordinaten). Die Markerposition ist der Schwerpunkt der 4 triangulierten Ecken;
die **Normale** ist der kleinste Singulärvektor der Ausgleichsebene durch die
Ecken, vorzeichenrichtig über die ArUco-Eckreihenfolge.

### Stufe 4 — Pose-Estimation
Siehe Abschnitt *Schätzverfahren*.

## Vorwärtskinematik (`robot_fk.py`)

Topologisch sortiert, je Link `T_l = T_parent · Mount · Gelenkursprung ·
Q(q_l)`, mit `Q = Rot(Achse, q)` (revolut) bzw. `Trans(Achse·q)` (linear).
Markerpose im Weltsystem: `p_m = T_l · p_local`, `n_m = R(T_l) · n_local`.

Kinematische Kette (7 DOF):

| Link | Eltern | Typ | Var | Achse | Marker |
|---|---|---|---|---|---|
| Board | — | Wurzel | — | — | viele |
| Base | Board | linear | `x` | [1,0,0] | **0** |
| Arm1 | Base | revolut | `y` | [-1,0,0] | 4 |
| Ellbow | Arm1 | revolut | `z` | [-1,0,0] | 7 |
| Arm2 | Ellbow | revolut | `a` | [0,-1,0] | 8 |
| Hand | Arm2 | revolut | `b` | [1,0,0] | **0** |
| Palm | Hand | revolut | `c` | [0,-1,0] | **0** |
| FingerA | Palm | linear | `e` | [1,0,0] | 3 |
| FingerB | Palm | linear | `e` | [-1,0,0] | 3 |

Besonderheiten, die die Engine generisch löst: **Base/Hand/Palm tragen keine
eigenen Marker** (nur über Kind-Marker beobachtbar), und **beide Finger teilen
`e`** (eine Unbekannte, gegengleiche Achsen).

## Schätzverfahren

Beobachtungen sind die Eckposen `{(Y_m, n_m)}`. Alle Verfahren parametrisieren
über die **Gelenkvariablen** `q`. Das Markerresiduum kombiniert Position und
Normale: `r_m(q) = [ p_m(q) − Y_m ; w_n·(n_m(q) − n_m^obs) ]` mit Gewicht `w_n`
(`normal_weight`). Für die sequenziellen Verfahren wird die Kette in Blöcke
zerlegt (markerlose Gelenke werden mit dem nächsten markertragenden gekoppelt):
`{x,y}`, `{z}`, `{a}`, `{b,c,e}`.

| Methode | Idee | Benchmark mean / worst |
|---|---|---|
| `sequential_vector` | analytischer Winkel aus ⊥-projizierten Marker-Paar-Vektoren, zirkuläres Mittel | 0,32° / 1,72° |
| `sequential_fk` | blockweiser nichtlinearer Ausgleich entlang der Kette, Multi-Start für große Winkel | 0,43° / 1,84° |
| `global_ba` | **alle** Gelenkwinkel gemeinsam, Huber-robust; überbrückt markerlose Glieder | 0,25° / 1,57° |
| **`hybrid`** (Default) | sequenzieller Init → globales Refinement | **0,25° / 1,57°** |

- **A `sequential_vector`** — pro revolutem Gelenk mit ≥2 Markern: Drehwinkel aus
  dem vorzeichenbehafteten Winkel zwischen Modell- und Beobachtungsvektor
  (senkrecht zur Gelenkachse projiziert), gewichtetes Zirkularmittel über
  Markerpaare. Sehr schnell, keine lokalen Minima; braucht ≥2 Marker/Gelenk.
- **B `sequential_fk`** — pro Block ein robustes Least-Squares mit fixierten
  Vorgängern; Multi-Start über `{0,60,…,300}°` für die führende Drehachse.
  Funktioniert schon mit einem Marker (dank Normalenresiduum).
- **C `global_ba`** — ein einziges robustes Least-Squares über alle 7 Variablen.
  Die Fingermarker bestimmen rückwärts auch `b`/`c` (markerlose Glieder).
  Startwert aus B vermeidet lokale Minima.
- **D `hybrid`** — B-Init + C-Refinement; durchgehend am genauesten und stabilsten.

## Beobachtbarkeit & Konfidenz

Pro Gelenk wird aus dem Verhältnis sichtbarer Marker zu Block-Variablen eine
Konfidenz abgeleitet und in `robot_state.json` geschrieben:
`high` (≥2/Var), `medium` (≥1), `low` (unterbestimmt), `none` (0 Marker).
**Sicherheitsrelevant:** Gelenke mit `low`/`none` nicht ungeprüft anfahren — bei
verdeckter Hand z. B. Achse `a` drehen und neu aufnehmen (Multi-Pose).

## Konfiguration (`robot.json`)

Eine Datei beschreibt Kinematik, Markerpositionen, Rendering und Algorithmik.
Wichtigste Abschnitte: `units`, `vision_config` (ArUco-Dict, `MarkerSize`),
`links` (die Kette), `renderingInfo` (Render-Defaults), `robot_test_poses` /
`test_camera_positions|targets` (Szenengenerator) und `pose_estimation`:

```json
"pose_estimation": {
  "method": "hybrid",                   // sequential_vector | sequential_fk | global_ba | hybrid
  "marker_observation": "corner_pose",  // oder "center_point"
  "use_normals": true,
  "normal_weight": 100.0,               // Gewicht der Normalenresiduen
  "robust_loss": "huber",
  "huber_delta_mm": 8.0,                // Ausreißer-Schwelle (mm)
  "max_iterations": 200,
  "min_cameras_per_marker": 2,
  "per_link_method": {}                 // optional: Verfahren je Glied
}
```

**Wichtige Optionen:**
- `method` — wählt das Schätzverfahren (A–D).
- `normal_weight` — zentraler Stabilitätsparameter. Höher stabilisiert sicht-arme
  Posen (Worst-Case-Winkel halbiert), zu hoch (≳300) lässt die Orientierung die
  Position überstimmen. `100` ist ein robuster Kompromiss.
- `huber_delta_mm` — ab welchem Residuum (mm) ein Marker als Ausreißer gedämpft wird.
- `marker_observation` — `corner_pose` (empfohlen) nutzt die Eckposen aus 3b,
  `center_point` nur Mittelpunkte aus `aruco_positions_initial.json`.

## Aufruf

**Gesamte Pipeline** (Bilder → Gelenkwinkel):
```bat
cd run
.\run_pipeline.bat ..\data\simulation\Scene8
```

**Einzelne Stufen** (zum Experimentieren):
```bash
python pipeline/3b_corner_marker_poses.py --evalDir data/evaluations/Scene8 --robot data/robot/robot.json
python pipeline/pose_estimation.py data/evaluations/Scene8/aruco_marker_poses.json \
       -robot data/robot/robot.json --method hybrid -out data/evaluations/Scene8/robot_state.json
```

**Validierung & Benchmark:**
```bash
python benchmark/eval_pose.py data/evaluations/Scene8/robot_state.json data/simulation/Scene8/pose.json
python benchmark/run_benchmark.py --scenes 4 5 6 7 8 9 9a 9b 11 12
python benchmark/stage0_corner_normals.py --evalDir data/evaluations/Scene5 --gt data/simulation/Scene5/render_a.json
python pipeline/9_evaluateMarker.py data/evaluations/Scene8/aruco_marker_poses.json data/simulation/Scene8/render_a.json
```

**Szenengenerator** (Blender → Bilder + npz + Grundwahrheit):
```bat
cd run
.\run_sceneGenerator.bat --poses 8        REM nur Pose 8
```

**Viewer:** `run/robot_viewer.html` im Browser öffnen, `robot.json` und
`aruco_marker_poses.json` laden. Der *Observed-normals*-Pfeil zeigt die gemessene
Normale, eingefärbt nach Abweichung zur Modellnormale (grün <2° / gelb 2–5° / rot >5°).

## Verzeichnisse

| Pfad | Inhalt |
|---|---|
| `pipeline/` | Pipeline-Stufen 1–4, `robot_fk.py`, `pose_estimation.py` |
| `benchmark/` | `stage0_corner_normals.py`, `eval_pose.py`, `run_benchmark.py` |
| `run/` | `.bat`-Starter, `robot_viewer.html` |
| `setup/generateSets/` | Blender-Szenengenerator (`render_Loop.py`, `render_robot.py`) |
| `data/robot/robot.json` | Modell + Konfiguration |
| `data/simulation/SceneX/` | gerenderte Bilder, npz, Grundwahrheit (`render_*.json`, `pose.json`) |
| `data/evaluations/SceneX/` | Pipeline-Zwischenergebnisse + `robot_state.json` |
| `doc/pipeline.tex` | ausführliche LaTeX/PDF-Dokumentation |

## Grenzen & Ausblick

- **Intrinsik:** Koplanare Board-Marker → schlecht konditionierte Tiefe; korrekte
  npz-Intrinsik ist Voraussetzung.
- **Sicht-arme Posen:** zu wenige Marker → `confidence=low`; Abhilfe per Multi-Pose
  (Achse `a` drehen).
- **Adaptive `normal_weight`:** pro Marker nach Sichtqualität skalieren — könnte
  Best- und Worst-Case zugleich optimieren.
- **Reale Kameras:** Stufe 1 nutzt aktuell dieselbe npz für alle Bilder
  (Simulation); reale Aufbauten brauchen je Kamera eine eigene Kalibrierung.