vor dem Start von callibrate
This commit is contained in:
chk
File diff suppressed because it is too large
Load Diff
220
doc/callibrate_scene_roadmap.md
Normal file
220
doc/callibrate_scene_roadmap.md
Normal file
@@ -0,0 +1,220 @@
|
||||
# Roadmap: Szenen-Kalibrierung der Board-/Loose-Marker (`callibrate_scene`)
|
||||
|
||||
Status: **Vorschlag / zur Abstimmung**
|
||||
Ort der Arbeit: `pipeline/` (nicht `approbot-pipeline/`, das bleibt die eingefrorene Kopie)
|
||||
Datum: 2026-06-04
|
||||
|
||||
> Eingearbeitete Entscheidungen:
|
||||
> 1. **Gelenkzustand UNBEKANNT** → wird mitgeschätzt (kein FK-Welt-Anker a priori).
|
||||
> 2. **Set-Definition direkt in `robot.json`** über ein optionales `"set"`-Feld je Marker.
|
||||
> Marker bleiben im bisherigen Format an ihrem Link. Gleiches `set` = ein starr zusammenhängendes
|
||||
> Objekt mit **fixen relativen Bezügen**. **Kein `set` = loser Marker** (fix, aber Lage z.Zt. unbekannt).
|
||||
> 3. **Welt = Roboter (Konvention 2)**, Roboter steht *nicht* bei 0/0/0.
|
||||
> 4. **Primär eine Aufnahme (7 Bilder)**, *ohne* zusätzliche Base-Marker; mehrere Posen als Fallback.
|
||||
> 5. **Ausgabe vorerst `robot.calibrated.json`** (Debugging); später in-place nach `robot.json`.
|
||||
|
||||
---
|
||||
|
||||
## 0. Machbarkeit — Kurzurteil
|
||||
|
||||
**Ja, machbar** — die anspruchsvollere Variante, weil der Gelenkzustand mitgeschätzt wird und es
|
||||
keinen a-priori Welt-Anker gibt (weder Board noch Arm). Vorgehen:
|
||||
|
||||
> **Erst ankerlos rekonstruieren** (metrisch, aus der bekannten Marker-Größe), **dann den Roboter
|
||||
> in die Rekonstruktion einpassen** — der eingepasste Roboter definiert die Welt — **dann die Sets
|
||||
> einpassen** und die Marker-Positionen aktualisieren.
|
||||
|
||||
Bausteine im Bestand: Per-Marker-PnP (`solve_single_marker_pose`), Eck-Triangulation
|
||||
(`3b_corner_marker_poses.py`), Bündelausgleichung (`3_multiview_bundle_adjustment_v5...`),
|
||||
FK + θ-Schätzung (`pose_estimation.py`, `robot_fk.py`), Kabsch-Fit (`rigid_transform_no_scale`).
|
||||
|
||||
**Ein Beobachtbarkeits-Knackpunkt** für „eine Aufnahme genügt ohne Base-Marker" steht in §7 — er ist
|
||||
beherrschbar, aber bewusst zu entscheiden.
|
||||
|
||||
---
|
||||
|
||||
## 1. Problem & Zielbild
|
||||
|
||||
**Realität:** Höhe/Orientierung zwischen Board und Arm sind ungenau. Marker ~20–105 liegen fix,
|
||||
aber unbekannt: teils Board-Platte, teils darunterliegendes A0-Blatt, teils einzeln aufgeklebt.
|
||||
|
||||
**Vertrauenswürdig:** die *interne Geometrie* der Roboter-Links und die *relativen Bezüge innerhalb
|
||||
jedes Sets* (beide in `robot.json` hinterlegt), sowie die *Marker-Kantenlänge* (25 mm → Maßstab).
|
||||
**Unbekannt:** Gelenkwinkel, Kamera-Posen, Platzierung jedes Sets, Lage jedes losen Markers.
|
||||
|
||||
**Ziel:** Nach der Kalibrierung hat jedes Set (als starres Objekt korrekt platziert) und jeder lose
|
||||
Marker (einzeln vermessen) eine korrekte Pose in einem roboter-verankerten Weltsystem.
|
||||
|
||||
### Marker-Klassen (aus dem `set`-Feld abgeleitet)
|
||||
|
||||
| Klasse | Erkennung | bekannt | zu schätzen |
|
||||
|---|---|---|---|
|
||||
| **Arm-Marker** (Roboter) | liegen an Arm-Links (Arm1…Finger) | Lage je Link | — definieren via Fit die Welt |
|
||||
| **Set-Marker** (starr) | `"set": "A0"`, `"set":"Brett"`, … | interne Relativlage (fix) | 6-DoF-Platzierung je Set |
|
||||
| **Lose Marker** | **kein** `set`-Feld | nur „fix vorhanden" | je Marker eigene 6-DoF-Pose |
|
||||
|
||||
---
|
||||
|
||||
## 2. Set-Definition: `set`-Feld am Marker (kein Strukturumbau)
|
||||
|
||||
Marker bleiben **wie bisher** in der `markers`-Liste ihres Links. Ein optionales `"set"` gruppiert sie:
|
||||
|
||||
```jsonc
|
||||
"Board": {
|
||||
"parent": null, "mountPosition": [0,0,0], "mountRotation": [0,0,0],
|
||||
"markers": [
|
||||
{ "id": 210, "position": [ 20,-20,0.3], "normal": [0,0,1], "set": "A0" },
|
||||
{ "id": 211, "position": [250,-10,0.3], "normal": [0,0,1], "set": "A0" },
|
||||
{ "id": 215, "position": [250,-90,0.3], "normal": [0,0,1], "set": "Brett" },
|
||||
{ "id": 208, "position": [350,-90,0.3], "normal": [0,0,1], "set": "Brett" },
|
||||
{ "id": 205, "position": [750,-90,0.3], "normal": [0,0,1] } // kein set -> lose
|
||||
]
|
||||
}
|
||||
```
|
||||
|
||||
- **Gleiches `set` ⇒ ein starres Objekt.** Die relativen Bezüge der Marker im Set gelten als **fix**;
|
||||
die Kalibrierung bestimmt nur die 6-DoF-Platzierung des ganzen Sets und schreibt die daraus
|
||||
resultierenden Positionen zurück (Format unverändert, relative Anordnung erhalten).
|
||||
- **Kein `set` ⇒ loser Marker.** Wird einzeln (Position + Normale + ggf. Spin) vermessen.
|
||||
- **Arm-Marker** brauchen kein `set`: ihr Link ist bereits ein starrer Körper und dient als
|
||||
Roboter-Referenz (sie werden *nicht* kalibriert, sondern definieren die Welt).
|
||||
- **Auto-Discovery** (Projekt-Konvention): Sets ergeben sich aus den `set`-Werten, nichts hartkodiert.
|
||||
|
||||
Hinweis: `robot_fk.py` / `all_markers_world()` bleiben unverändert — das `set`-Feld ist reine
|
||||
Zusatzinfo, die nur der Kalibrier-Treiber auswertet.
|
||||
|
||||
---
|
||||
|
||||
## 3. Algorithmus (Gelenkzustand unbekannt)
|
||||
|
||||
### Phase A — Ankerlose, metrische Rekonstruktion
|
||||
1. **Detektion** (Schritt 1) → Ecken je Kamera.
|
||||
2. **Per-Marker-PnP** je Kamera aus der bekannten Marker-Größe (`SOLVEPNP_IPPE_SQUARE`) → volle
|
||||
Marker-Pose *relativ zur Kamera*. Kein Welt-Anker nötig.
|
||||
3. **Relativer Posen-Graph:** gemeinsam gesehene Marker verknüpfen Kamerapaare → Init aller Kamera-
|
||||
und Marker-Posen in einem *beliebigen* Szenen-Frame `S`.
|
||||
4. **Globale Bündelausgleichung** (scipy `least_squares`, Huber): verfeinert alle Kamera- und
|
||||
Marker-Posen über die Reprojektion aller Ecken. Maßstab fix durch Marker-Größe.
|
||||
|
||||
→ konsistente, metrische 3D-Szene (Arm- **und** Set-/Loose-Marker) in `S`.
|
||||
|
||||
### Phase B — Roboter einpassen = Welt definieren
|
||||
5. Arm-Marker per ID → Link zuordnen. **Fit** von Gelenkwinkeln θ **und** der Platzierung der
|
||||
FK-Wurzel in `S`, sodass `FK(θ)` der Arm-Marker die rekonstruierten Arm-Positionen trifft
|
||||
(erweitert `pose_estimation.py` um eine freie Wurzel-Platzierung statt fixer Identität).
|
||||
6. In das Weltsystem rücktransformieren. Welt-Ursprung = FK-Wurzel-Frame (= heutiges „Board"-Frame),
|
||||
Roboter sitzt mit dem fertigen θ darin — *nicht* bei 0/0/0 (§6).
|
||||
|
||||
### Phase C — Set-Fit & Rückschreiben
|
||||
7. **Set-Marker (pro `set`):** Kabsch (`rigid_transform_no_scale`) bildet die **fixe interne Lage**
|
||||
auf die rekonstruierten Welt-Positionen ab → 6-DoF-Set-Platzierung → aktualisierte Positionen
|
||||
(= Platzierung ∘ interne Lage). Auch nicht gesehene Set-Marker erhalten so eine Position, sofern
|
||||
das Set über ≥3 nicht-kollineare Marker bestimmt ist.
|
||||
**Lose Marker:** triangulierte Pose direkt übernehmen.
|
||||
8. **Rückschreiben** nach `robot.calibrated.json` (Marker-Format unverändert, `set`-Felder erhalten)
|
||||
+ `calibration_report.json` (je Set die explizite Verschiebung/Verdrehung + RMS; je Marker Status).
|
||||
Nicht beobachtbare Größen → **`null`** (nie 0).
|
||||
|
||||
### Fallback — mehrere Posen (statische Kameras)
|
||||
Mehrere Gelenkzustände bei festen Kameras: Kamera-Posen + Set-/Loose-Posen + Wurzel-Platzierung sind
|
||||
**geteilte** Unbekannte, je Pose ein eigener θ-Satz. Löst die §7-Schwächen vollständig auf.
|
||||
|
||||
---
|
||||
|
||||
## 4. Eingaben & Ausgaben
|
||||
|
||||
**Eingaben:** `robot.json` (Arm-Geometrie + `set`-Felder + fixe interne Set-Lagen);
|
||||
Szenen-Ordner mit `render_*.png` **oder** vorhandene `*_aruco_detection.json`.
|
||||
(Gelenkzustand wird NICHT benötigt.)
|
||||
|
||||
**Ausgaben:** `robot.calibrated.json` (aktualisierte Marker-Positionen, Format wie bisher);
|
||||
`calibration_report.json` (je Set: Verschiebung/Verdrehung, RMS, #Kameras/#Marker, Status;
|
||||
je losem Marker: Pose oder `null`). Optional Viewer-Overlay Soll↔kalibriert.
|
||||
|
||||
---
|
||||
|
||||
## 5. Neue / geänderte Dateien
|
||||
|
||||
| Datei | Art | Inhalt |
|
||||
|---|---|---|
|
||||
| `pipeline/calibrate_scene.py` | **neu** | Treiber: Auto-Discovery Kameras+Sets, Phase A→B→C, schreibt `robot.calibrated.json`+Report |
|
||||
| `pipeline/scene_reconstruct.py` | **neu** | Phase A: Per-Marker-PnP, Posen-Graph, globale BA (ankerlos) |
|
||||
| `pipeline/robot_register.py` | **neu** | Phase B: Fit θ + freie Wurzel-Platzierung (nutzt `robot_fk`) |
|
||||
| `pipeline/marker_sets.py` | **neu** | liest `set`-Felder aus `robot.json`; Klassifizierung Arm/Set/Lose |
|
||||
| `3b_corner_marker_poses.py` | **erweitern** | volle Marker-**Rotation** (Normale + Spin) aus 4 Ecken |
|
||||
| `pose_estimation.py` | **erweitern** | optionale freie Wurzel-Platzierung (für Phase B wiederverwendbar) |
|
||||
|
||||
`2_estimate_camera_from_observations.py` / `robot_fk.py`: voraussichtlich **unverändert**.
|
||||
|
||||
---
|
||||
|
||||
## 6. Weltursprung (Konvention 2, Roboter nicht bei 0/0/0)
|
||||
|
||||
- Ursprung = FK-Wurzel-Frame (heute „Board"). Der Roboter sitzt mit seinem modellierten Versatz
|
||||
(`Base.jointToParent.origin` + Slider `x`) darin → **nicht** bei 0/0/0. Das deckt den Wunsch ab.
|
||||
- „Welt durch Roboter definiert" wird dadurch realisiert, dass die **Kalibrierung am Roboter
|
||||
verankert** (Fit θ + Wurzel-Platzierung über Arm-Marker), statt den Board-Markern zu vertrauen.
|
||||
Die Board-Positionen werden konsistent *neu* abgeleitet.
|
||||
- Der Kinematik-Baum bleibt unverändert. (Optionaler späterer Umbau „Base = Wurzel" möglich, aber
|
||||
für die Kalibrierung nicht nötig.)
|
||||
|
||||
---
|
||||
|
||||
## 7. Beobachtbarkeit — Einzelaufnahme ohne Base-Marker (wichtig)
|
||||
|
||||
Verifiziert an `robot.json`: `Base`, `Hand`, `Palm` haben **keine** Marker; erster markierter Link
|
||||
ist `Arm1`. Daraus folgt für EINE Pose mit unbekannten Gelenkwinkeln:
|
||||
|
||||
- **Slider `x` und `Joint1 y` sind nicht von der absoluten Roboter-Platzierung trennbar** (2-DoF-
|
||||
Gauge-Freiheit): eine Verschiebung entlang der Schiene ≙ Änderung von `x`; eine Drehung um die
|
||||
`Joint1`-Achse ≙ Änderung von `y`. Die Set-/Loose-Marker erben diese 2 Freiheitsgrade.
|
||||
- **Gut bestimmt aus einer Pose:** `z, a, b, c, e` und damit die gesamte Szene *relativ*.
|
||||
|
||||
Da Base-Marker mechanisch unerwünscht sind, der empfohlene Weg:
|
||||
|
||||
- **Gauge per Konvention fixieren** (Default für Einzelaufnahme): `x`,`y` auf die gefittete
|
||||
Konfiguration / nominale Schienen-Null setzen und die Welt so definieren. Ergebnis ist
|
||||
**in sich konsistent** → für künftige Pose-Schätzung (Board als Anker) voll nutzbar; lediglich
|
||||
der *absolute* Schienen-Nullpunkt und die `Joint1`-Null sind dann Konvention, kein Messwert.
|
||||
- **Mehrere Posen** (Fallback), wenn die absolute Basis-Lage / absolute `x`,`y` wirklich gebraucht
|
||||
werden — das löst die 2 Freiheitsgrade vollständig auf.
|
||||
- *(optional, falls je möglich:* ein einzelner Base-/Schlitten-Marker würde Einzelaufnahme voll
|
||||
beobachtbar machen — derzeit zurückgestellt.)*
|
||||
|
||||
QA: Reprojektions-RMS je Kamera; Set-Fit-Residuum (mm); Co-Visibility-Graph zusammenhängend?;
|
||||
≥3 nicht-kollineare Marker je Set; ≥2 Kameras je losem Marker (sonst Status `partial`/`unobserved`).
|
||||
|
||||
---
|
||||
|
||||
## 8. Validierung (Sim zuerst)
|
||||
|
||||
`pose.json` liefert in der Simulation GT-Gelenkwinkel **und** Kamera-Pos/Targets:
|
||||
1. Bekannte Sets künstlich verschieben/verdrehen → kalibrieren → Rück-Transform gegen Soll.
|
||||
2. Gefittete θ gegen GT-θ; gefittete Kamera-Posen gegen GT.
|
||||
3. Einzelaufnahme- vs. Mehrfach-Posen-Genauigkeit quantifizieren (belegt §7).
|
||||
4. Erst danach `data/recorded/`-Szenen.
|
||||
|
||||
---
|
||||
|
||||
## 9. Phasen / Meilensteine
|
||||
|
||||
- **P0 — `set`-Felder & Parser:** `set`-Felder in `robot.json` ergänzen; `marker_sets.py`
|
||||
(Arm/Set/Lose-Klassifizierung); FK-Welt-Positionen unverändert verifizieren.
|
||||
- **P1 — Ankerlose Rekonstruktion (Phase A):** Per-Marker-PnP + Posen-Graph + globale BA; gegen
|
||||
GT-Kamera-Posen (Sim) prüfen.
|
||||
- **P2 — Roboter-Registrierung (Phase B):** Fit θ + freie Wurzel-Platzierung; gegen GT-θ; §7-Gauge.
|
||||
- **P3 — Set-Fit & Rückschreiben (Phase C):** Kabsch + Loose → `robot.calibrated.json` + Report;
|
||||
Sim-Validierung mit künstlichem Offset.
|
||||
- **P4 — Mehrfach-Posen-Fallback.**
|
||||
- **P5 — Reale Szenen + Viewer-Overlay; danach in-place nach `robot.json`.**
|
||||
|
||||
---
|
||||
|
||||
## 10. Verbleibende kleinere Punkte
|
||||
|
||||
1. **Gauge-Konvention für Einzelaufnahme** (§7): `x`,`y` = gefittet, oder Schiene/`Joint1` auf
|
||||
nominal? (Beeinflusst nur den absoluten Nullpunkt, nicht die Set-Relativlagen.)
|
||||
2. **Set-Namensraum:** sind `set`-Namen global eindeutig oder pro Link? (Vorschlag: global, z.B.
|
||||
„A0", „Brett" — ein Set = ein physisches Objekt.)
|
||||
3. **Lose-Marker-Orientierung:** reicht Position + Normale, oder wird der Spin (Drehung um die
|
||||
Normale) gebraucht? (Bestimmt die nötige Genauigkeit von Phase C / 3b.)
|
||||
Reference in New Issue
Block a user