113 lines
4.9 KiB
Markdown
113 lines
4.9 KiB
Markdown
# 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).
|
||
```
|