# Pose-Erkennungs-Pipeline

Aus Mehrkamera-Fotos eines Roboters werden die Gelenkwinkel (`x,y,z,a,b,c,e`)
geschätzt. Kern: Jeder ArUco-Marker wird über seine **4 triangulierten Ecken**
als volle 6-DOF-Pose (Position **+ gemessene Normale**) rekonstruiert; daraus
werden die Gelenkwinkel über die Kinematik (`robot_fk.py`) bestimmt.

## Ergebnis (Stand: Nacht-Build)

Benchmark über 10 valide Sim-Posen (Scene4–9b, 11, 12; Scene10 war fehlerhaft
gerendert und ausgeschlossen), 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 und sind erledigt:
1. **npz-fy-Bug** in `render_robot.py` gefixt (war ~15 % Brennweitenfehler bei 16:9).
2. **Eck- statt Center-Triangulation** — liefert pro Marker eine Normale (~1,2° genau, validiert in `benchmark/stage0_corner_normals.py`).

## Pipeline (run/run_pipeline.bat)

```
1_detect_aruco_observations      Fotos  -> *_aruco_detection.json   (inkl. 4 Ecken)
2_estimate_camera_from_observations    -> *_camera_pose.json
3_multiview_bundle_adjustment_v4       -> aruco_positions_initial.json (Center; für Viewer/Vergleich)
3b_corner_marker_poses                 -> aruco_marker_poses.json   (Eck-Pose + Normale)
pose_estimation                        -> robot_state.json          (Gelenkwinkel + Konfidenz)
```

Aufruf: `.\run_pipeline.bat ..\data\simulation\Scene8`

## Pose-Estimation (pipeline/pose_estimation.py)

Parametrisiert über **Gelenk-Variablen** (nicht Links), damit Sonderfälle dieses
Roboters generisch funktionieren:
- Glieder ohne eigene Marker (Base/`x`, Hand/`b`, Palm/`c`) — über Kind-Marker bestimmt.
- Geteilte Variable (`e` für FingerA+FingerB) — eine Unbekannte.
- Verdeckte Mittelglieder — durch globales BA überbrückt.

### Vier Methoden (robot.json → `pose_estimation.method`)

| Methode | Idee | Benchmark (mean / worst) |
|---|---|---|
| `sequential_vector` | analytisch aus Marker-Paar-Vektoren | 0,32° / 1,72° |
| `sequential_fk` | blockweise 1D/Block-LS entlang der Kette | 0,43° / 1,84° |
| `global_ba` | alle Winkel gemeinsam, robuste Loss | 0,25° / 1,57° |
| **`hybrid`** (Default) | sequenzieller Init → globales BA | **0,25° / 1,57°** |

### robot.json-Konfiguration

```json
"pose_estimation": {
  "method": "hybrid",
  "marker_observation": "corner_pose",   // oder "center_point"
  "use_normals": true,
  "normal_weight": 100.0,                 // höher = stabiler bei wenigen Markern
  "robust_loss": "huber",
  "huber_delta_mm": 8.0,
  "max_iterations": 200,
  "min_cameras_per_marker": 2,
  "per_link_method": {}                   // optional: Methode pro Glied
}
```

`normal_weight=100` wurde empirisch gewählt: stabilisiert sicht-arme Posen
deutlich (Worst-Case-Winkel halbiert), kostet bei gut sichtbaren Posen nur ~0,2°.

### Konfidenz pro Gelenk

`robot_state.json → movements[v].confidence`: `high` (≥2 Marker/Variable im
Block), `medium` (≥1), `low` (unterbestimmt — misstrauen!), `none` (0 Marker).
Für die Kollisionssicherheit: Gelenke mit `low`/`none` nicht ungeprüft anfahren;
ggf. Homing/`a`-Drehung für mehr Sichtbarkeit (Multi-Pose).

## Benchmark- & Eval-Werkzeuge (benchmark/)

| Tool | Zweck |
|---|---|
| `stage0_corner_normals.py` | Eck-Normalen-Genauigkeit gegen GT (Go/No-Go) |
| `eval_pose.py` | geschätzte Gelenkwinkel vs. `pose.json` |
| `run_benchmark.py` | Scenes × Methoden → Matrix + Aggregat (CSV/JSON) |
| `pipeline/9_evaluateMarker.py` | Marker Position + Normale pro Glied vs. `render_*.json` |

Beispiele:
```
python benchmark/run_benchmark.py                          # alle bereiten Scenes, alle Methoden
python benchmark/run_benchmark.py --scenes 5 8 --methods hybrid
python benchmark/stage0_corner_normals.py --evalDir data/evaluations/Scene5 --gt data/simulation/Scene5/render_a.json
python benchmark/eval_pose.py data/evaluations/Scene5/robot_state.json data/simulation/Scene5/pose.json
```

## Viewer (run/robot_viewer.html)

Lade `aruco_marker_poses.json` als „aruco" → der **Observed-normals**-Pfeil zeigt
jetzt die **gemessene** Normale, eingefärbt nach Winkelabweichung zur Modell-
Normale (grün <2° / gelb 2–5° / rot >5°). Statistik-Panel zeigt Normal mean/max.

## Offene Punkte / Ideen

- **Adaptive `normal_weight`**: pro Marker nach Sicht-Qualität gewichten →
  best-case und worst-case gleichzeitig optimal (statt fester Kompromiss 100).
- **Multi-Pose-Fusion**: mehrere Aufnahmen bei verschiedenen `a`-Winkeln in
  *eine* globale BA werfen — für Posen, in denen die Hand sonst verdeckt ist.
- **Reale Kameras**: Schritt 1 nutzt aktuell für alle Bilder `render_a.npz`
  (Sim: alle Kameras gleich). Real bräuchte jede Kamera ihre eigene Intrinsik-npz.
- **`center_point`-Vergleich** im Benchmark (`--observation center_point`) braucht
  `aruco_positions_initial.json` pro Scene (Schritt 3).
```