appRobotRender/README.md

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.

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:

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

cd run
.\run_pipeline.bat ..\data\simulation\Scene8

Einzelne Stufen (zum Experimentieren):

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:

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):

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.