# 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`.
> 6. **Code generisch & `robot.json`-unabhängig.** `robot.json` ist nur ein Beispiel und wird später
>    gegen ein anderes geprüft. KEINE festen Marker-IDs, Link-/Set-Namen, Achsen oder Gelenk-Variablen
>    im Code (Auto-Discovery aus den Daten). Daten-spezifisches (z.B. die `set`-Zuordnung) gehört in
>    `robot.json`, nicht in den Code.
> 7. **Aktuelle Marker-Positionen = brauchbare Startwerte.** Die relativen Bezüge innerhalb jedes Sets
>    gelten als korrekt → direkte Grundlage für den Kabsch-Fit und die BA-Initialisierung.

---

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

---

## 11. Umsetzungs-Log: Mathematik & Anwendung

Pro abgeschlossener Phase: *was* gemacht wurde, *welche Mathematik* dahinter steckt, *wie* man es
anwendet. (Wird mit jeder Phase fortgeschrieben.)

### P0 — Marker-Klassifizierung & `set`-Tags — ✅ erledigt (2026-06-04)

**Was gemacht**
- Neu: `pipeline/marker_sets.py` — liest ein beliebiges `robot.json`, klassifiziert jeden Marker in
  `arm` / `set` / `loose` und gibt einen Report aus. Vollständig generisch (keine festen Namen/IDs).
- Daten: `data/robot/robot.json` mit `set`-Tags versehen — `Brett` (9 Board-Oberflächen-Marker,
  z≈0.3) und `A0` (60 Papier-Marker, z≈−27.3). Chirurgisch eingefügt (kompaktes Custom-Format der
  Datei bleibt erhalten, nur +1 Zeile), FK numerisch exakt invariant.

**Mathematik / Logik**

1) *Statisch (Welt) vs. beweglich (Roboter):* Ein Link `L` ist beweglich, wenn auf dem Pfad
   `L → Wurzel` mindestens ein Gelenk vom Typ revolute/linear liegt:
   ```
   movable(L) = ∃ A ∈ chain(L→root):  type(jointToParent(A)) ∈ {revolute, linear}
   ```
   Die Wurzel und nur über `fixed` angebundene Links sind statisch (= Welt). Generisch, da nur
   Gelenk-Typen geprüft werden — keine Link-Namen.

2) *Rollen* eines Markers `m` auf Link `L`:
   ```
   role(m) = arm       falls movable(L)                    → Welt-Referenz, NICHT kalibriert
           = set(s)    falls ¬movable(L) ∧ m.set = s       → starres Objekt, 6-DoF kalibrieren
           = loose     falls ¬movable(L) ∧ m hat kein set  → einzeln vermessen
   ```

3) *Modell eines starren Sets `S`* (die eigentliche Kalibriergröße der Phase C):
   Marker `i` hat eine **bekannte, fixe** lokale Lage `p_iˡᵒᵏ` (die vertrauten relativen Bezüge).
   Das Set hat eine **unbekannte** starre Platzierung `(R_S, t_S) ∈ SE(3)`:
   ```
   p_iʷᵉˡᵗ = R_S · p_iˡᵒᵏ + t_S          R_S ∈ SO(3) (Verdrehung), t_S ∈ ℝ³ (Verschiebung)
   ```
   `(R_S, t_S)` = die gesuchten 6 DoF je Set. Schätzung aus gemessenen Welt-Positionen `q_i`
   per **Kabsch / orthogonalem Procrustes** (ohne Skalierung):
   ```
   min_{R∈SO(3), t}  Σ_i ‖ R·p_iˡᵒᵏ + t − q_i ‖²
     p̄=mean(pˡᵒᵏ), q̄=mean(q),  H = Σ (p_iˡᵒᵏ−p̄)(q_i−q̄)ᵀ,  U Σ Vᵀ = svd(H)
     R = V·diag(1,1,det(V Uᵀ))·Uᵀ,   t = q̄ − R·p̄
   ```
   (vorhanden als `rigid_transform_no_scale`). Weil die aktuellen `robot.json`-Positionen brauchbare
   Startwerte sind und die relativen Bezüge stimmen, gilt `p_iˡᵒᵏ` = aktuelle Set-Positionen
   (ggf. zentriert) und `(R_S,t_S) ≈ Identität` als Initialwert.

4) *Loser Marker:* eigene unbekannte Pose `(R_m, t_m)`, einzeln aus den triangulierten Ecken
   (Position = Eckmittel, Orientierung aus Eckebene + Eckreihenfolge).

5) *FK-Invarianz:* `set` ist reine Metadaten; `p^welt = T_Link(θ)·p^lok` bleibt unberührt
   (verifiziert: max |Δ| = 0).

**Anwendung**
```
# Report (Mensch):
python pipeline/marker_sets.py -robot data/robot/robot.json
# Report (Maschine):
python pipeline/marker_sets.py -robot data/robot/robot.json --json
```
```python
from marker_sets import load_robot, classify_markers, get_sets, get_loose_markers, get_arm_markers
data  = load_robot("data/robot/robot.json")
sets  = get_sets(data)            # {"A0":[MarkerInfo,...], "Brett":[...]}
loose = get_loose_markers(data)   # [MarkerInfo,...]
arm   = get_arm_markers(data)     # {id: MarkerInfo}
```
*Neues `robot.json` vorbereiten:* an jeden Marker eines starren Objekts `"set": "<name>"` ergänzen
(Name frei wählbar, ein Set = ein physisches Objekt); lose Marker ohne `set`; Arm-Marker (an
beweglichen Links) brauchen keinen Eintrag. Der Code liest die Sets dann automatisch.

### P1 — Ankerlose, metrische Rekonstruktion (Phase A) — ✅ erledigt (2026-06-04)

**Was gemacht**
- Neu: `pipeline/scene_reconstruct.py` — rekonstruiert aus den ArUco-Eckbeobachtungen ALLER Kameras
  die Kamera- und Marker-Posen in einem gemeinsamen Frame `S`, **ohne** Welt-Anker, **ohne**
  `robot.json` (nur Marker-Kantenlänge nötig). Vollständig generisch.

**Mathematik**

Konventionen: `E_c` = world(S)→Kamera c, `G_m` = Marker-lokal→world(S), also `M_{c←m} = E_c · G_m`.

1) *Per-Marker-PnP (tentativ):* für jedes (Kamera c, Marker m) löst IPPE_SQUARE die Pose eines
   Quadrats bekannter Größe. **Problem:** ein planares Quadrat hat eine **2-fache Flip-Ambiguität**
   → naive Verkettung liefert ~⅓ gespiegelte Knoten (empirisch verifiziert).

2) *Flip-robuste Initialisierung* (Referenzkamera `c0`, `E_{c0}=I`), iterativ:
   - **Kamera-Pose per RANSAC-PnP** gegen die bereits platzierten 3D-Ecken
     `X_S = G_m · corner_local`. RANSAC verwirft gespiegelte Marker als Ausreißer.
   - **Marker-Ecken triangulieren** (lineares DLT über alle platzierten Kameras) — Triangulation
     ist **flip-frei**; daraus Marker-Pose via Kabsch `local→tri`. Korrigiert Flips aus Schritt 1.
   - Marker mit nur 1 Kamera sind nicht triangulierbar → als `insufficient_views` markiert
     (unbeobachtbar, **nicht** rekonstruiert; Konvention „unbekannt = null").

3) *Globale Bündelausgleichung* (`scipy.least_squares`, robuste Huber-Loss, **dünnbesetzte**
   Jacobi): minimiert die Reprojektion aller Ecken
   ```
   min_{E_c, G_m}  Σ_{(c,m)} Σ_{k=1..4} ρ( ‖ π(K_c, D_c; E_c·G_m·corner_k) − u_{c,m,k} ‖ )
   ```
   Gauge: Referenzkamera `E_{c0}=I` fix (entfernt die 6-DoF-Starrkörperfreiheit).

4) *Similarity-Gauge / Skala:* Ankerlose SfM bestimmt die Struktur nur bis auf eine **7-DoF-
   Ähnlichkeit** (Rotation, Translation, **Skala**). Der absolute Maßstab kommt hier provisorisch aus
   der angenommenen Markergröße — empirisch ~0.93× gegenüber der echten Welt (konsistent über alle
   Szenen, also ein systematischer Markergrößen-/Rand-Versatz). **Die echte Skala + Lage fixiert
   erst Phase B über die bekannte mm-Geometrie des Roboters** (robuster als die Markergröße). Die
   *Form* ist bereits korrekt.

**Validierung (Sim, gegen FK-Ground-Truth)**
- Reprojektion median **0.7–1.6 px** über Scene5/6/8/10/11/12 — besser als die bestehende,
  board-verankerte Pipeline (3.2–4.9 px), weil keine falschen Marker-Positionen angenommen werden.
- Form (nach Similarity-Ausrichtung): Residuum **median ~3–7 mm** = Sensor-Rauschboden der Renders
  (`markerOffsetMaxMm: 4` + Rauschen); Übereinstimmung mit der bestehenden Triangulation **1.9 mm**.
- Skalenfaktor konsistent **0.92–0.94** (→ Phase B).

**Anwendung**
```
python pipeline/scene_reconstruct.py --evalDir data/evaluations/Scene8
# -> <evalDir>/scene_reconstruction.json  (Kamera- & Marker-Posen in S, Reproj-Statistik,
#    Liste insufficient_view_markers)
```
```python
import scene_reconstruct as sr
res = sr.reconstruct("data/evaluations/Scene8")   # dict; res["cameras"], res["markers"]
```
Voraussetzung: `*_aruco_detection.json` je Kamera (Pipeline-Schritt 1). Marker-Kantenlänge wird
aus den Detektionen gelesen (Fallback `--markerSize`).