233 lines
11 KiB
Markdown
233 lines
11 KiB
Markdown
# 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
|
||
```
|
||
|
||
Nur eine Teilmenge der Kameras verwenden (für Experimente / die Kamera-Studie):
|
||
```bash
|
||
python pipeline/run_pipeline.py data/simulation/Scene8 --cameras a,c,f
|
||
```
|
||
|
||
**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 --robot data/robot/robot.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
|
||
```
|
||
Mit `--robot` ergänzt `eval_pose.py` FK-basierte Positionsfehler in mm (Handgelenk
|
||
und Finger getrennt; unbeobachtbare Punkte werden als unbekannt gewertet).
|
||
|
||
**Kamera-Anzahl-Studie** (Wie viele Kameras braucht es?):
|
||
```bash
|
||
run\run_camera_study.bat # alle Szenen, k = 3..alle
|
||
python benchmark/camera_count/analyze.py # Tabelle + Boxplot (mm)
|
||
```
|
||
Details: [`benchmark/camera_count/README.md`](benchmark/camera_count/README.md).
|
||
|
||
**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`, `run_pipeline.py` (mit `--cameras`) |
|
||
| `benchmark/` | `stage0_corner_normals.py`, `eval_pose.py`, `run_benchmark.py` |
|
||
| `benchmark/camera_count/` | Kamera-Anzahl-Studie (`run_camera_study.py`, `analyze.py`, eigene README) |
|
||
| `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.
|