# Homing 5 – Pose-Schätzung per Bundle-Adjustment (`hybrid`) > Technische Detail-Doku zu [`Homing.md`](Homing.md) — **Verfeinerungsschritt NACH > der 4b-Kette** ([`Homing_1_StepByStep.md`](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 (2026-06-16): **Code-Hooks umgesetzt und gegen drei echte Homing-Captures > verifiziert** — `--from-state` (Startwert aus 4b, mit Multi-Start-Schutz für alles, > was der Startwert nicht abdeckt), `null` statt `0` für unbeobachtbare Gelenke > (z. B. `Hand`/`Palm`/Finger, aktuell ohne Marker in `robot_1781069752019.json`), > `scipy` in `docker-compose.yaml` ergänzt, sowie neu der Kalibrier-Switch > `--calibrate-origin` (siehe eigener Abschnitt unten). **Noch offen:** Anbindung in > `homingOrchestrator.js`/`server.js`/Frontend (siehe Integrationsschritte — > Umfang/Fehlerfall/Robotersteuerung-Politik dafür sind noch nicht festgelegt). --- ## 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..{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: 1. `sequential_vector` — analytische Winkel aus Marker-Paar-Vektoren (schnell, braucht ≥2 Marker/Gelenk) 2. `sequential_fk` — blockweiser nichtlinearer Fit entlang der Kette, vorherige Variablen eingefroren, Multi-Start `{0,60,…,300}°` gegen lokale Minima 3. `global_ba` — **einziges** Bündelausgleichsproblem über **alle 7 Variablen gleichzeitig** (`scipy.optimize.least_squares`, Huber-Loss) 4. **`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 **Ursprünglicher Befund (vor dem Code-Hook vom 2026-06-16):** `estimate_pose()` rief für `global_ba`/`hybrid` immer selbst `estimate_sequential_fk()` als „billigen, robusten Init" auf — es gab keinen Parameter, um stattdessen einen extern vorgegebenen Startwert einzuspeisen, obwohl `estimate_global_ba()` selbst intern bereits ein `x0`-Dict entgegennahm (`:272-273`). **Das ist jetzt behoben** (`seed`-Parameter auf `estimate_pose()`/`estimate_sequential_fk()`, CLI `--from-state`) — die folgende Beschreibung des Multi-Start-Mechanismus gilt weiterhin unverändert für **alles, was der Seed nicht abdeckt** (bzw. für den reinen Kaltstart ohne `--from-state`): `estimate_sequential_fk()` initialisiert jede nicht geseedete 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** (`:348-356`). 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 / Status:** `5_pose_estimation.py` sollte in appRobotHoming **nicht kalt** laufen, sondern mit dem `accumulated_state` der 4b-Kette als Startwert. **Umgesetzt:** `--from-state ` lädt einen flachen oder `{"accumulated_state": {...}}`-verpackten Zustand; `estimate_sequential_fk()` überspringt nur Blöcke, die **vollständig** im Startwert enthalten sind, und wendet auf alles andere weiterhin seinen normalen Multi-Start an — auch bei einem **unvollständigen** Seed (z. B. nur `x,y` aus einem abgebrochenen 4b-Lauf) bleiben `z`/`a` also Multi-Start-geschützt, statt ungeschützt bei `0` zu starten. Verifiziert an drei echten Captures (s. „Validierung an echten appRobotHoming-Daten" unten). ### 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".) ### Validierung an echten appRobotHoming-Daten (2026-06-16) Drei echte Captures aus `test/homing/` (nicht simuliert; vom Nutzer bereitgestellt, inkl. eines bereits aktualisierten 4b-Laufs): | Fixture | 4b kam bis | Arm1-Marker gesehen | Ellbow-Marker gesehen | |---|---|---|---| | `20260616_120456` | Arm1 (Ellbow nicht gespeichert) | 197, 243 | 129, 132 | | `20260616_133151` | Arm2 | 198, 229 | 129, 132 | | `20260616_135403` | Arm2 | 197, 243 | 129, 132, 121 | Geprüft für jede Fixture (`python scripts/5_pose_estimation.py … --from-state state_Arm2.json`, bzw. `state_Arm1.json` für die unvollständige erste Fixture): - **Kein Crash**, trotz `Hand`/`Palm`/`FingerA`/`FingerB` aktuell ganz ohne Marker in `robot_1781069752019.json` (`"markers": []` an allen vieren) — `b`, `c`, `e` kommen als `confidence:"none"`, `"value": null` heraus, exakt wie gefordert ("Hand als unbekannt stehen lassen"). - **Kalt vs. geseedet liefert dieselben Werte** (z. B. `133151`: `x=193.96mm, y=25.74°, z=-28.00°, a=-0.81°` in beiden Fällen) — der Seed verändert das Ergebnis nicht, wenn der Kaltstart bereits im richtigen Minimum lag; er schützt nur die Fälle, in denen das nicht so ist. - **Unvollständiger Seed** (`120456`, nur `x,y` aus `state_Arm1.json`, `z`/`a` fehlen): liefert dieselben Werte wie der volle Kaltstart — und durchläuft jetzt nachweislich den Multi-Start-Pfad für `z`/`a` (Code-Pfad geprüft, nicht nur Zufall des Ergebnisses). - Residuum über alle Marker: **4,3–4,5 mm RMS** (deutlich über der Simulationsvalidierung oben — erwartbar, reale Marker/Kameras sind verrauschter als der appRobotRendering-Renderfehler-Boden; noch keine `huber_delta_mm`/`normal_weight`-Nachjustierung vorgenommen). --- ## 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 im appRobotHoming-Container~~ — **behoben (2026-06-16):** `docker-compose.yaml` installierte nur `opencv-python-headless numpy`; ohne `scipy` greift `HAVE_SCIPY=False` und `estimate_sequential_fk`/`estimate_global_ba` fallen lautlos auf den Nullzustand zurück (nur eine `[WARN]`-Zeile, kein Fehler-Exit) — ein stiller Fehlmodus. `scipy` ist jetzt in der `pip3 install`-Zeile ergänzt (kein separater Image-Build nötig — `pip3 install` läuft laut `command:` bei jedem Containerstart neu). **Noch nicht** auf einem laufenden Container wirksam geprüft — wirkt erst nach dessen nächstem Neustart. - **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..value`). Eine kleine Adapterfunktion ist nötig, kein Drop-in-Ersatz. - ~~Unbeobachtbare Gelenke werden als `0.0` ausgegeben, nicht als `null`~~ — **behoben (2026-06-16):** `main()`s Output-Writer schreibt jetzt `"value": null`, wenn `observable:false`, statt der internen `0.0` (die intern bleibt, weil die FK für die *anderen* Gelenke einen Zahlenwert braucht — nur der *Output*-Vertrag ändert sich). Verifiziert an allen drei Fixtures (`Hand`, `Palm`, `e` → `null`). Gilt nur für `5_pose_estimation.py` selbst — der Adapter zu `/api/homing/send-state` (nächster Punkt) muss `null` weiterhin korrekt durchreichen, nicht wieder in `0` zurückverwandeln. - **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 `--from-state` und 4b-Startwert — als Regressionstest für genau diese Einschränkung (beide Aufrufe stehen unter „Aufruf (Stand-alone, zum Testen)"). - **`finger_block_joints`/`per_link_method`** stehen schon (leer) in der robot.json — vorbereitete, aber im Skript bisher ungenutzte Erweiterungspunkte aus appRobotRendering. --- ## Kalibrier-Switch: Gelenk-Origin (`--calibrate-origin`) **Motivation:** `doc/Kalibrierung.md` Schritt [4] bestimmt `links.Arm1.jointToParent.origin[1,2]` (Y/Z des Schultergelenk-Drehpunkts) geometrisch aus einer **dedizierten 3-Pose-Aufnahme** (Verfahren B: Kreis-Umkreismittelpunkt durch 3 Positionen je Marker, nur Marker-**Mittelpunkte**, keine Normalen — Details dort). Diese Y/Z-Werte sind laut Nutzer „etwas ungenau gemessen". `5_pose_estimation.py` hat mit `residual_vector()` (Position **und** Normale, robuste Verlustfunktion, generischer Least-Squares-Löser) bereits die Bausteine, um densel­ben Drehpunkt **aus den ohnehin vorhandenen Homing-Aufnahmen** zu verfeinern, statt eine eigene Aufnahme-Session zu brauchen. ### Ansatz Statt nur die Gelenkvariable `q` zu fitten, werden für **einen** angegebenen Link zusätzlich 2 Komponenten seines `jointToParent.origin` freigegeben: ``` Normalfall: min_q Σ_marker ρ(‖r(q)‖) (3 Freiheitsgrade weniger) --calibrate-origin: min_{q_link, origin_y, origin_z} Σ_marker∈link ρ(‖r(q_link, origin_y, origin_z)‖) ``` Implementiert in `estimate_origin_calibration()` (neu, `scripts/5_pose_estimation.py`): mutiert `fk.links[]["jointToParent"]["origin"][1,2]` **transient** während des Solves (jeder `fk.compute()`-Aufruf liest `origin` frisch aus dem Dict, siehe `robot_fk.py:compute()` — kein Caching, daher funktioniert die direkte Mutation ohne Änderung an `robot_fk.py`) und stellt den Originalwert danach **immer** wieder her — das Skript bleibt ein reines Report-Tool, **`robot.json` wird nie geschrieben**. Multi-Start `{0,60,…,300}°` für die eigene Gelenkvariable, wenn revolut (wie bei den anderen Verfahren). Alle anderen Gelenke bleiben fix (aus `--from-state`, sonst `0`) — Vorbedingung wie beim bestehenden Verfahren: die übrige Kette (insbesondere `x`) muss schon vertrauenswürdig sein. ### Aufruf ```bash python scripts/5_pose_estimation.py data/homing//aruco_marker_poses.json \ -robot scripts/robot_1781069752019.json \ --from-state data/homing//state_Arm2.json \ --calibrate-origin Arm1 # -> schreibt Arm1_origin_calibration.json (Report), robot.json unverändert ``` Funktioniert generisch für jeden Link mit eigenen Markern (an `Ellbow` mit `--calibrate-origin Ellbow` getestet) — keine Arm1-spezifische Hardcodierung. ### Befund an echten Daten (2026-06-16, vorläufig) Auf zwei **unabhängigen** Fixtures mit **unterschiedlichen** sichtbaren Arm1-Markern ergibt sich eine konsistente Korrektur: | Fixture | gesehene Marker | Δ Origin Y | Δ Origin Z | Residuum RMS | |---|---|---|---|---| | `20260616_133151` | 198, 229 | **+6,46 mm** | **−19,97 mm** | 1,19 mm | | `20260616_135403` | 197, 243 | **+7,33 mm** | **−18,49 mm** | 1,19 mm | Beide Läufe sehen **andere** Markerpaare und kommen trotzdem auf nahezu denselben Versatz (~+7 mm / ~−19 mm) — das ist kein Zufallsrauschen eines einzelnen Markers, sondern ein konsistenter Hinweis, dass der aktuell in `robot_1781069752019.json` hinterlegte Wert (`[110, 101.1, 55.2]`) tatsächlich um ungefähr diesen Betrag daneben liegt. **Noch nicht** unabhängig gegen das geometrische Verfahren B (3-Pose-Aufnahme) gegengeprüft — siehe Offene Punkte. ### Einschränkungen / Unterschiede zum bestehenden Verfahren | | Verfahren B (`yAxisCompute.js`, bestehend) | `--calibrate-origin` (neu) | |---|---|---| | Aufnahmen nötig | 3 Posen, ≥15° Drehung dazwischen | **1** Pose (mehr optional, noch nicht implementiert) | | Signal | Marker-Mittelpunkt über 3 Zeitpunkte | Position **+ Normale**, robuste Verlustfunktion | | Fehlerabschätzung | Residuum εᵢ je Marker (Kreis-Abweichung) | `residual_rms` über alle Link-Marker | | Achsrichtung | wird mitbestimmt (Kreuzprodukt/Ebenen-Normale) | wird **nicht** gefittet — nur `origin`, `axis` bleibt aus robot.json | | Identifizierbarkeit | durch Drehung explizit entkoppelt von Winkel | aus 1 Pose: Winkel/Origin-Korrelation theoretisch möglich, durch mehrere Marker + Normalen an verschiedenen Hebelarmen empirisch entkoppelt (s. Befund oben) — **nicht formal bewiesen** | | Schreibt robot.json | ja, über „Joint-Origin Y/Z übernehmen" | nein — nur Report, gleiche Übernahme-Aktion nutzbar | Die Achs**richtung** (`jointToParent.axis`) fitten beide Verfahren nicht — das bleibt vorerst bei Verfahren B, falls sie ebenfalls ungenau ist. --- ## Aufruf (Stand-alone, zum Testen) **Empfohlen — mit Startwert aus der 4b-Kette** (z. B. dem letzten vorhandenen `state_*.json`; unvollständig ist ok, fehlende Variablen bleiben Multi-Start-geschützt): ```bash python scripts/5_pose_estimation.py data/homing//aruco_marker_poses.json \ -robot scripts/robot_1781069752019.json \ --from-state data/homing//state_Arm2.json \ -out data/homing//robot_state.json ``` **Kalt** (kein `--from-state`) — funktioniert weiterhin identisch wie vor diesem Code-Hook, aber ohne den oben beschriebenen Schutz für gekoppelte Blöcke; nützlich, um das Kaltstart-/Lokales-Minimum-Verhalten aus „Wichtige Einschränkung" gezielt zu reproduzieren/regressionszutesten: ```bash python scripts/5_pose_estimation.py data/homing//aruco_marker_poses.json \ -robot scripts/robot_1781069752019.json # Verfahren erzwingen, z.B. zum gezielten Vergleich einzelner Methoden: python scripts/5_pose_estimation.py data/homing//aruco_marker_poses.json \ -robot scripts/robot_1781069752019.json --method global_ba ``` Gegen die echten Testdaten in `test/homing/*/` ausprobiert — siehe „Validierung an echten appRobotHoming-Daten" oben. --- ## Integrationsschritte (Offene Punkte) **Erledigt (2026-06-16):** - [x] **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. - [x] **`scipy` in `docker-compose.yaml` ergänzt** (`pip3 install … numpy scipy`). - [x] **Code-Hook `--from-state`:** `load_seed_state()` (akzeptiert flach oder `{accumulated_state:{...}}`) + `estimate_sequential_fk(..., seed=...)` überspringt nur vollständig geseedete Blöcke, alles andere bleibt Multi-Start-geschützt. `estimate_pose(..., seed=...)` reicht das durch. Verifiziert an 3 echten Fixtures (s. „Validierung an echten appRobotHoming-Daten"). - [x] **Robustheit gegen fehlende Marker:** `Hand`/`Palm`/`FingerA`/`FingerB` (aktuell `"markers": []`) laufen ohne Crash durch, Output zeigt `null`/ `confidence:"none"` statt erfundener `0`. `main()`s Output-Writer mappt `observable:false → value:null` (intern bleibt `0.0` für die FK-Rechnung der anderen Gelenke — nur der Output-Vertrag ändert sich). - [x] **Kalibrier-Switch `--calibrate-origin `** umgesetzt (`estimate_origin_calibration()`) — generisch für jeden Link mit eigenen Markern, getestet an `Arm1` und `Ellbow`. Schreibt nie `robot.json`, nur einen `*_origin_calibration.json`-Report. Details: eigener Abschnitt oben. **Noch offen:** - [ ] **Adapter** `movements..value` → flaches `{x,…,e}`-State-Objekt für `POST /api/homing/send-state`; `null` muss `null` bleiben (nicht zurück zu `0`). - [ ] **Anbindung in `homingOrchestrator.js`** (neuer Schritt nach der 4b-Schleife, SSE-Events) — **Umfang/Fehlerfall/Sende-Politik an die Robotersteuerung sind noch nicht festgelegt** (offene Rückfrage vom 2026-06-16, noch unbeantwortet: Minimal-Fix vs. Voll-Integration; Abbruch vs. Fallback bei Fehler; Senden vs. nur Anzeigen). Diese drei Entscheidungen zuerst klären, dann verdrahten. - [ ] **Arm1-Origin-Befund anwenden oder verwerfen:** Δ(Y,Z) ≈ (+7, −19) mm ist auf zwei unabhängigen Fixtures konsistent (s. Abschnitt „Kalibrier-Switch"). Vor dem Übernehmen: (a) mit mehr Fixtures/Posen erhärten, (b) wenn möglich gegen eine frische Verfahren-B-3-Pose-Messung gegenchecken, (c) erst dann via Kalibrierung-Tab „Joint-Origin Y/Z übernehmen" übernehmen. - [ ] **`--calibrate-origin` an die Kalibrierung-UI anbinden** (`doc/Kalibrierung.md` Schritt [4]) — aktuell nur CLI/Report; Tab „Arm1 – Y" könnte beide Verfahren (Geometrisch/Verfahren B und `--calibrate-origin`) nebeneinander anzeigen. - [ ] **Mehrpose-Erweiterung für `--calibrate-origin`** (mehrere `aruco_marker_poses.json` + gemeinsames `origin`, je Pose ein eigener Gelenkwinkel) — würde die Winkel/Origin-Korrelationsschwäche aus einer Einzelpose weiter reduzieren, analog zur bestehenden 3-Pose-Aufnahme. - [ ] `huber_delta_mm`/`normal_weight` ggf. gegen reale Marker-Genauigkeit nachjustieren — reales Residuum (4,3–4,5 mm RMS) liegt deutlich über der Simulation; Defaults sind unverändert aus appRobotRendering übernommen. - [ ] Python-Tests (`pytest`) für `load_seed_state()`, den Block-Skip in `estimate_sequential_fk()` und `estimate_origin_calibration()` — aktuell nur manuell gegen die drei Fixtures verifiziert (s. oben); appRobotHoming hat bisher keine Python-Testinfrastruktur (nur Jest/JS), das wäre die erste. - [ ] Eintrag in `Homing.md`-Tabelle (Doku-Übersicht) ergänzen, sobald `homingOrchestrator.js` verdrahtet ist. --- ## Verweise - Allgemeiner Ablauf: [`Homing.md`](Homing.md) - Vorheriger Schritt (Kamera/Triangulation, liefert den gemeinsamen Input): [`Homing_0_Camera.md`](Homing_0_Camera.md) - Vorstufe (4b-Kette, liefert den hier benötigten Startwert): [`Homing_1_StepByStep.md`](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").