Files

chk 773e32c51c Scene Roadmap

2026-06-10 07:13:19 +02:00

19 KiB

Raw Permalink Blame History

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:

Gelenkzustand UNBEKANNT → wird mitgeschätzt (kein FK-Welt-Anker a priori).

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

Welt = Roboter (Konvention 2), Roboter steht nicht bei 0/0/0.

Primär eine Aufnahme (7 Bilder), ohne zusätzliche Base-Marker; mehrere Posen als Fallback.

Ausgabe vorerst robot.calibrated.json (Debugging); später in-place nach robot.json.

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.

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:

"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

Detektion (Schritt 1) → Ecken je Kamera.
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.
Relativer Posen-Graph: gemeinsam gesehene Marker verknüpfen Kamerapaare → Init aller Kamera- und Marker-Posen in einem beliebigen Szenen-Frame S.
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

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

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.
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:

Bekannte Sets künstlich verschieben/verdrehen → kalibrieren → Rück-Transform gegen Soll.
Gefittete θ gegen GT-θ; gefittete Kamera-Posen gegen GT.
Einzelaufnahme- vs. Mehrfach-Posen-Genauigkeit quantifizieren (belegt §7).
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

Gauge-Konvention für Einzelaufnahme (§7): x,y = gefittet, oder Schiene/Joint1 auf nominal? (Beeinflusst nur den absoluten Nullpunkt, nicht die Set-Relativlagen.)
Set-Namensraum: sind set-Namen global eindeutig oder pro Link? (Vorschlag: global, z.B. „A0", „Brett" — ein Set = ein physisches Objekt.)
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

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.

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

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.
Loser Marker: eigene unbekannte Pose (R_m, t_m), einzeln aus den triangulierten Ecken (Position = Eckmittel, Orientierung aus Eckebene + Eckreihenfolge).
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

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)