221 lines
12 KiB
Markdown
221 lines
12 KiB
Markdown
# 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.)
|