# 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.)