Claude: Doku

POSE_PIPELINE.md

@@ -1,112 +0,0 @@
-# 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).
-```

README.md

@@ -0,0 +1,217 @@
+# 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.