Homing 5 – Pose-Schätzung per Bundle-Adjustment (`hybrid`)

Technische Detail-Doku zu Homing.md — Verfeinerungsschritt NACH der 4b-Kette (Homing_1_StepByStep.md), nicht deren Ersatz: 5_pose_estimation.py braucht den accumulated_state von 4b als Startwert. Ohne guten Startwert läuft die interne Optimierung mangels eigener verlässlicher Initialisierung leicht in ein lokales Minimum (siehe „Wichtige Einschränkung" unten). Status: Skript liegt bereits in scripts/5_pose_estimation.py, noch nicht in homingOrchestrator.js/server.js verdrahtet — und braucht noch einen kleinen Code-Hook, um den 4b-Startwert überhaupt entgegennehmen zu können (siehe Offene Punkte).

Herkunft

scripts/5_pose_estimation.py ist 1:1 (byte-identisch, per Diff verifiziert) aus dem Schwesterprojekt appRobotRendering übernommen (pipeline/pose_estimation.py, dort Stufe 4 der allgemeinen Pose-Pipeline). Mitgewandert und ebenfalls identisch: scripts/robot_fk.py. Dort ist das Verfahren an zehn simulierten Szenen mit bekannter Grundwahrheit validiert (doc/pipeline.tex im Rendering-Projekt) — diese Zahlen unten sind Simulationsergebnisse aus appRobotRendering, keine Messung an appRobotHoming/echter Hardware.

scripts/robot_1781069752019.json enthält bereits den passenden pose_estimation-Konfigurationsblock (identisch zu den Rendering-Defaults: method: hybrid, normal_weight: 100, huber_delta_mm: 8.0, …) — die Datenseite ist also schon vorbereitet, nur die Prozess-Verdrahtung fehlt noch.

Einordnung in den Homing-Ablauf

1_detect_aruco_observations.py  ┐
2_estimate_camera_from_observations.py  │  = "Board-Pipeline" (Homing_0_Camera.md)
3b_corner_marker_poses.py       ┘
        │
        ▼  aruco_marker_poses.json
        │
        ▼
4b_revolute_angle.py × N  (sequenziell root→tip, über --from-state verkettet)
        │  Homing_1_StepByStep.md — liefert pro Gelenk eine Schätzung
        │  (Primär/Fallback-1/Fallback-2), abhängig von Sichtbarkeit
        ▼
accumulated_state {x,y,z,a,b,c,e}   ← Startwert, NICHT optional überspringbar
        │
        ▼
5_pose_estimation.py  (method=global_ba, accumulated_state als Startwert x0)
        │  dieses Dokument — EIN gemeinsamer Bündelausgleich über alle 7
        │  Variablen gleichzeitig, verfeinert/korrigiert den 4b-Zustand global
        ▼
robot_state.json { movements: {…}, residual_rms, … }

Wichtig: 5_pose_estimation.py ist kein Ersatz für die 4b-Kette, sondern ein Verfeinerungsschritt danach, der deren accumulated_state als Startwert braucht. Lässt man die 4b-Kette weg, fehlt dieser Startwert — die interne Optimierung initialisiert dann faktisch bei 0 für jede Variable, und bei einer beim Einschalten unbekannten Roboterpose ist das ein guter Weg in ein lokales Minimum (Mechanismus s. „Wichtige Einschränkung" unten).

Stufe	Eingabe	Aufruf	Ausgabe
4b-Kette (liefert den Startwert)	`aruco_marker_poses.json` + extern geschätztes `x_mm`	N Prozesse, je Link einer, `--from-state` verkettet	`accumulated_state` (flach: `x,y,z,a,b,c,e`)
`5_pose_estimation.py` (verfeinert global)	`aruco_marker_poses.json` + `accumulated_state` aus 4b als Startwert	1 Prozess	`robot_state.json` → `movements.<var>.{value,unit,observable,confidence,n_markers}`

Wie es funktioniert (kurz)

Das Skript parametrisiert über Gelenkvariablen (nicht Links) und liest pro Marker Position und gemessene Normale aus aruco_marker_poses.json (3b-Ausgabe). Vier austauschbare Verfahren (robot.json → pose_estimation.method), hybrid ist Standard und kombiniert die letzten beiden:

sequential_vector — analytische Winkel aus Marker-Paar-Vektoren (schnell, braucht ≥2 Marker/Gelenk)
sequential_fk — blockweiser nichtlinearer Fit entlang der Kette, vorherige Variablen eingefroren, Multi-Start {0,60,…,300}° gegen lokale Minima
global_ba — einziges Bündelausgleichsproblem über alle 7 Variablen gleichzeitig (scipy.optimize.least_squares, Huber-Loss)
hybrid = 2 als Startwert → 3 als Verfeinerung

Die Blockbildung in analyze_chain() ist generisch aus der FK-Topologie abgeleitet (keine festen Link-Namen) — passt damit zur Projekt-Konvention „Scripts müssen Szenen/Ketten automatisch erkennen, nichts hartkodieren". Für dieses Robot-Modell ergibt sich u. a. der Block {x, y}: Base (Variable x) hat keine eigenen Marker ("markers": [] in robot_1781069752019.json) und wird automatisch mit Arm1 (Variable y, 5 Marker) zu einem gemeinsamen Least-Squares-Fit zusammengefasst.

Jedes Ergebnis kommt mit einer Konfidenz pro Variable (high/medium/low/none, abgeleitet aus sichtbaren Markern je Block) — analog zur 4b-Kette, aber pro Block statt pro Einzel-Fallback-Stufe.

Wichtige Einschränkung: Startwert und lokale Minima

estimate_pose() ruft für global_ba/hybrid immer zuerst selbst estimate_sequential_fk() als „billigen, robusten Init" auf (scripts/5_pose_estimation.py:471-476) — es gibt aktuell keinen Parameter, um stattdessen einen extern vorgegebenen Startwert (z. B. den accumulated_state aus 4b) einzuspeisen, obwohl estimate_global_ba() selbst intern bereits ein x0-Dict entgegennimmt (:236-237).

estimate_sequential_fk() initialisiert jede Variable bei 0.0 und rastert den Multi-Start {0,60,120,180,240,300}° nur über die erste Variable eines Blocks (bvars[0]) — und auch das nur, wenn diese selbst revolute ist (:296-304). Für dieses Robotermodell heißt das konkret:

Block {x, y} (Base markerlos → mit Arm1 zusammengefasst): bvars[0] ist x (linear) → lead_type != "revolute" → kein Multi-Start. y (Schultergelenk, Arm1) wird in einem einzigen Lauf ab 0° gefittet.
Block {b, c, e} (Hand/Palm markerlos → mit den Fingermarkern zusammengefasst): nur b bekommt den 6-Punkte-Raster; c und e starten in jedem der 6 Läufe fix bei 0.
Einzelvariablen-Blöcke wie Ellbow ({z}) oder Arm2 ({a}) bekommen den vollen Raster auf sich selbst — dort ist das Risiko deutlich kleiner.

Liegt die echte Pose in y, c oder e weit von 0 entfernt (beim Homing nach dem Einschalten der Normalfall, nicht die Ausnahme), kann schon die sequential_fk-Vorstufe in einem falschen lokalen Minimum landen — die anschließende global_ba-Verfeinerung poliert dieses falsche Minimum dann nur noch, statt es zu verlassen. Das deckt sich mit dem in der Validierungstabelle unten sichtbaren großen Abstand zwischen Mittelwert (0,253°) und Schlechtestfall (1,568°) bei sonst niedriger Streuung (0,134°) — ein Muster, das zu „meist gut, gelegentlich falsches Minimum" passt.

Konsequenz: 5_pose_estimation.py sollte in appRobotHoming nicht kalt laufen, sondern mit dem accumulated_state der 4b-Kette als Startwert (Details und der dafür nötige Code-Hook: Abschnitt „Integrationsschritte").

Validierung im Rendering-Projekt (Simulation, 10 Posen, bekannte GT)

Verfahren	Winkel Ø [°]	Winkel schlechtest. [°]	Position Ø [mm]	Position schlechtest. [mm]
`sequential_vector`	0,315	1,717	0,144	0,712
`sequential_fk`	0,434	1,838	0,158	0,851
`global_ba`	0,253	1,568	0,103	0,390
`hybrid`	0,253	1,568	0,103	0,390

(Quelle: appRobotRendering/doc/pipeline.tex, Abschnitt „Validierung und Ergebnisse".)

Vorteile

Bestes/stabilstes Verfahren im Rendering-Benchmark (s. Tabelle oben) — unter allen vier Methoden der niedrigste Mittel- und Worst-Case-Fehler.
Überbrückt markerlose Gelenke automatisch. Hand (Variable b) und Palm (c) tragen keine eigenen Marker — global_ba zieht die Information aus den Fingermarkern rückwärts durch die Kette. Die 4b-Kette braucht dafür explizit einen Fallback pro Gelenk; hier passiert es als Nebenprodukt der gemeinsamen Optimierung.
Fittet x und y gemeinsam aus denselben Arm1-Markern (Block {x,y}, weil Base markerlos ist) — konsistenter als zwei getrennte Schätzungen. Ersetzt estimateXFromMarkers() aber nicht: dieser Block ist genau einer der beiden, die ohne guten Startwert anfällig für ein lokales Minimum sind (s. „Wichtige Einschränkung" unten) — die gemeinsame Schätzung ist also ein Mehrwert nach einer 4b-Vorschätzung, kein Grund, diese zu überspringen.
Funktioniert mit nur 1 sichtbarem Marker pro Gelenk, weil das Residuum Position und Normale nutzt (Gl. in residual_vector()) — die 4b-Primärmethode braucht dafür mindestens 2.
Ist die automatisierte Form der bereits manuell durchgeführten Gegenrechnung. Der Befund vom 2026-06-16 in Homing_1_StepByStep.md (Ellbow: Fallback-2 lag 35–40° neben einer von Hand gerechneten Least-Squares-Kontrolle über Ellbow- und Arm2-Marker) ist exakt das, was global_ba/hybrid automatisch und für alle Gelenke gleichzeitig macht. Ein Lauf hätte den Fallback-2-Fehler vermutlich direkt erkennbar gemacht.
Robuste Verlustfunktion (Huber) dämpft einzelne Ausreißer-Marker (Fehldetektion, Verdeckung) automatisch, statt dass ein einzelner schlechter Marker das ganze Gelenk verfälscht.
Multi-Start über mehrere Startwinkel hilft dort, wo er greift (Blöcke mit genau einer Variable, z. B. Ellbow/z, Arm2/a) — für Homing wertvoller als für Kalibrierung, weil beim Einschalten die Pose komplett unbekannt ist. Greift aber nicht bei den gekoppelten Blöcken {x,y} und {b,c,e} (s. u.) — genau dort ist ein externer Startwert aus 4b nötig.

Nachteile

Kein verlässlicher eigener Kaltstart — Startwert von außen zwingend nötig. Wie im Abschnitt „Wichtige Einschränkung" hergeleitet: der interne Multi-Start deckt nur Einzelvariablen-Blöcke ab, nicht die gekoppelten Blöcke {x,y} und {b,c,e}. Allein aufgerufen (ohne accumulated_state aus 4b) ist 5_pose_estimation.py daher beim Homing real gefährdet, in einem lokalen Minimum zu landen, statt die echte Pose zu finden — kein Rand-, sondern ein Kernfall, weil die Pose beim Einschalten grundsätzlich unbekannt ist.
scipy fehlt aktuell im appRobotHoming-Container. docker-compose.yaml installiert nur opencv-python-headless numpy (pip3 install --quiet --no-cache-dir opencv-python-headless numpy). Ohne scipy greift HAVE_SCIPY=False: estimate_sequential_fk lässt jeden Block auf 0.0 stehen, estimate_global_ba gibt den (dann ebenfalls nullwertigen) Startwert unverändert zurück — kein Fehler, nur eine [WARN]-Zeile auf stdout. Das ist ein stiller Fehlmodus: muss vor dem ersten Einsatz behoben werden (scipy zur pip3 install-Zeile ergänzen).
Zwei nichtlineare Least-Squares-Läufe statt eines geschlossenen Ausdrucks — langsamer als sequential_vector und langsamer als ein einzelner 4b_revolute_angle.py-Aufruf. Für „schnell, vollautomatisch" (Anspruch aus Homing.md) noch nicht auf echter Hardware gemessen.
Kein progressives Zwischenergebnis. Die 4b-Kette liefert nach jedem Link ein SSE-analysis-Event und aktualisiert den Board-Viewer live („progressiver Update je erkanntem Gelenk", Homing.md → Implementierung). estimate_pose() gibt nur den fertigen Endzustand zurück — für dieselbe UX müsste man zusätzlich die internen Zwischenstände von estimate_sequential_fk() exponieren.
Verliert die dokumentierte Fallback-Diagnostik. Homing_1_StepByStep.md protokolliert pro Gelenk, welche Stufe gegriffen hat (method: primary / fallback_1 / fallback_2). 5_pose_estimation.py liefert nur eine Block-Konfidenz (high/medium/low/none), nicht welche Heuristik intern gewirkt hat — weniger Transparenz beim Debuggen einzelner Gelenke.
Ausgabeformat passt nicht direkt auf /api/state. Der Endpunkt erwartet ein flaches {x,y,z,a,b,c,e} (accumulated_state, siehe server/server.js → POST /api/homing/send-state), 5_pose_estimation.py schreibt verschachtelt (movements.<var>.value). Eine kleine Adapterfunktion ist nötig, kein Drop-in-Ersatz.
Unbeobachtbare Gelenke werden als 0.0 ausgegeben, nicht als null (Konfidenz none/observable:false steht nur als Metadatum daneben). Das widerspricht der sonst im Projektverbund befolgten Konvention „Unbekannt bleibt null, nie erfundene 0". Eine Integration muss observable:false aktiv auf null ummappen, bevor der Zustand weitergereicht wird — sonst wandert eine stille 0°/0mm in Richtung Robotersteuerung.
Noch nicht an echten Kamerabildern/Markern validiert. Die Zahlen oben sind Simulation aus appRobotRendering (saubere FK-Marker-Positionen, definierter Renderfehler-Rauschboden). Reale Marker-Ungenauigkeiten (s. Kalibrierung_Marker.md) und reale Kameranoise könnten andere huber_delta_mm/ normal_weight-Werte als die übernommenen Defaults verlangen.

Besonderheiten

Reiner, unveränderter Import-Stand — momentan git-?? (untracked), noch nicht in homingOrchestrator.js/server.js referenziert (nur 4b_revolute_angle.py ist dort als SCRIPT_4B verdrahtet).
Schema-Kompatibilität zur lokalen 3b_corner_marker_poses.py bereits geprüft: Feldnamen marker_id, position_mm/position_m, normal, num_cameras stimmen 1:1 — load_observations() braucht keine Anpassung.
Namens-Kollision mit 5_camera_z_refine.py — zwei Skripte teilen sich das Präfix 5_. Entspricht der Konvention aus appRobotRendering, wo mehrere Dateien sich ein Stufen-Präfix teilen (z. B. 3_*, 4_*); kein Bug, aber beim Lesen der scripts/-Liste leicht zu verwechseln.
Die pose_estimation.method-Option erlaubt gezieltes A/B-Testen ohne Codeänderung: --method sequential_vector|sequential_fk|global_ba|hybrid per CLI-Override, oder dauerhaft über robot_1781069752019.json → pose_estimation.method. Nützlich, um den Effekt des Startwerts zu isolieren: einmal kalt (zeigt das Problem aus „Wichtige Einschränkung"), einmal mit 4b-Startwert (sobald der Code-Hook existiert) — als Regressionstest für genau diese Einschränkung.
finger_block_joints/per_link_method stehen schon (leer) in der robot.json — vorbereitete, aber im Skript bisher ungenutzte Erweiterungspunkte aus appRobotRendering.

Aufruf (Stand-alone, zum Testen)

⚠️ Diese Aufrufe laufen kalt (kein externer Startwert — der Code-Hook dafür existiert noch nicht, s. Integrationsschritte). Geeignet, um das Kaltstart-/ Lokales-Minimum-Verhalten aus „Wichtige Einschränkung" zu beobachten und zu reproduzieren — nicht der vorgesehene Produktionspfad.

python scripts/5_pose_estimation.py data/homing/<run>/aruco_marker_poses.json \
       -robot scripts/robot_1781069752019.json \
       -out   data/homing/<run>/robot_state.json
# Verfahren erzwingen, z.B. zum gezielten Vergleich einzelner Methoden:
python scripts/5_pose_estimation.py data/homing/<run>/aruco_marker_poses.json \
       -robot scripts/robot_1781069752019.json --method global_ba

Integrationsschritte (Offene Punkte)

scipy in docker-compose.yaml ergänzen (pip3 install … Zeile) — ohne das läuft hybrid lautlos auf Nullzustand.
Architektur entschieden: 4b-Kette läuft zuerst und liefert den accumulated_state als Startwert; 5_pose_estimation.py läuft danach als globaler Verfeinerungsschritt darüber. Kein Ersatz, keine parallele Alternative — siehe „Wichtige Einschränkung" oben.
Code-Hook in 5_pose_estimation.py ergänzen: aktuell gibt es keinen Weg, einen externen Startwert hineinzugeben. Vorschlag: CLI-Flag analog zu 4b (--from-state accumulated_state.json), das den 4b-Zustand als x0 direkt an estimate_global_ba() durchreicht (Parameter existiert dort bereits, :236-237) und so den internen estimate_sequential_fk()-Kaltstart in estimate_pose() (:471-476) umgeht bzw. nur als Fallback nutzt, falls kein externer Startwert übergeben wird.
Adapter movements.<var>.value → flaches {x,…,e}-State-Objekt für POST /api/homing/send-state; dabei observable:false → null ummappen.
Anbindung in homingOrchestrator.js (neuer Schritt, analog runBoardPipeline/ 4b-Loop) + SSE-Event(s) für Fortschritt (auch ohne echtes Zwischenergebnis, z. B. ein step-Event „läuft" / „fertig").
Erste echte Messung: hybrid-Ergebnis gegen 4b-Kette auf demselben data/homing/<run>/aruco_marker_poses.json vergleichen (insbesondere am Ellbow-Fall aus Homing_1_StepByStep.md).
huber_delta_mm/normal_weight ggf. gegen reale Marker-Genauigkeit nachjustieren (Defaults sind aus appRobotRendering-Simulation übernommen).
Eintrag in Homing.md-Tabelle (Doku-Übersicht) ergänzen, sobald verdrahtet.

Verweise

Allgemeiner Ablauf: Homing.md
Vorheriger Schritt (Kamera/Triangulation, liefert den gemeinsamen Input): Homing_0_Camera.md
Vorstufe (4b-Kette, liefert den hier benötigten Startwert): Homing_1_StepByStep.md
Ursprung & Validierung: Projekt appRobotRendering, pipeline/pose_estimation.py + doc/pipeline.tex (Abschnitte „Pose-Estimation: Vier Schätzverfahren" und „Validierung und Ergebnisse").

17 KiB Raw Blame History Unescape Escape

Homing 5 – Pose-Schätzung per Bundle-Adjustment (hybrid)