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:

npz-fy-Bug in render_robot.py gefixt (war ~15 % Brennweitenfehler bei 16:9).
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

"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).

4.9 KiB Raw Blame History Unescape Escape