From 9501d54d5c3b71fb01aee8c77feedffe9a3aee13 Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Tue, 2 Jun 2026 06:08:50 +0200 Subject: [PATCH] Claude: Doku --- POSE_PIPELINE.md | 112 ------------------------ README.md | 217 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 217 insertions(+), 112 deletions(-) delete mode 100644 POSE_PIPELINE.md diff --git a/POSE_PIPELINE.md b/POSE_PIPELINE.md deleted file mode 100644 index cc6f9a8..0000000 --- a/POSE_PIPELINE.md +++ /dev/null @@ -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). -``` diff --git a/README.md b/README.md index e69de29..2b3dd5e 100644 --- a/README.md +++ b/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.