440 lines
25 KiB
Markdown
440 lines
25 KiB
Markdown
# 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 der **eine Schalter**
|
||
> `pose_estimation.fit_origin_link` (siehe eigener Abschnitt unten), der einen
|
||
> Gelenk-Drehpunkt automatisch mitbestimmt und in `robot.json` übernimmt. **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.<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:
|
||
|
||
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 <json>` 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.<var>.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 (`pose_estimation.fit_origin_link`)
|
||
|
||
**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 (Marker-Mittelpunkte, keine Normalen). Diese
|
||
Y/Z-Werte sind laut Nutzer „etwas ungenau gemessen" — `5_pose_estimation.py` hat mit
|
||
Position+Normale und einem robusten Least-Squares-Löser bereits die Bausteine, um
|
||
denselben Drehpunkt aus den ohnehin vorhandenen Homing-Aufnahmen zu verfeinern.
|
||
|
||
**Ein Schalter, eine Stelle:** `robot.json` → `pose_estimation.fit_origin_link`
|
||
(aktuell `"Arm1"`). Wenn gesetzt, gibt `estimate_global_ba()` für diesen Link
|
||
`jointToParent.origin[1,2]` (Y,Z) als **2 zusätzliche Variablen derselben
|
||
Optimierung** frei (kein separater Lauf, keine Restore-Logik, keine eigene
|
||
Funktion) — die Gelenkvariable und der Drehpunkt werden **gemeinsam** bestimmt.
|
||
Bei Erfolg übernimmt `main()` das Ergebnis automatisch: `patch_robot_json_origin()`
|
||
schreibt nur die eine `"origin": [...]`-Zeile des Links in `robot.json` zurück
|
||
(Text-Patch, nicht `json.dump()` — der Rest der handgepflegten, kompakten Datei
|
||
bleibt unverändert). `null`/Feld weglassen = aus, keine Verhaltensänderung.
|
||
|
||
### Befund an echten Daten (drei reale Captures, sequenziell, 2026-06-16)
|
||
|
||
| Lauf | Fixture | Origin Y / Z danach | Δ zum Vorlauf |
|
||
|---|---|---|---|
|
||
| 1 | `20260616_133151` | 108,28 / 34,81 | +7,18 / −20,39 (ggü. ursprünglich 101,1 / 55,2) |
|
||
| 2 | `20260616_135403` | 108,84 / 34,89 | +0,57 / +0,08 |
|
||
| 3 | `20260616_120456` (unvollst. Seed) | 107,42 / 35,33 | −1,43 / +0,45 |
|
||
|
||
Drei unabhängige Aufnahmen (unterschiedliche Marker, unterschiedliche Posen) landen
|
||
im selben Bereich, und die Schritte werden **kleiner statt größer** — spricht für
|
||
Konvergenz, nicht für Rauschen/Drift. (Für die Doku danach wieder auf den
|
||
Ursprungswert `101.1, 55.2` zurückgesetzt — die Tabelle zeigt nur den Testlauf.)
|
||
**Konsequenz des „bei jedem Lauf automatisch":** der Wert wandert mit jeder neuen
|
||
Aufnahme leicht weiter, statt einmalig fixiert zu werden — gewünscht laut Nutzer,
|
||
aber gut zu wissen. Schalter auf `null` setzen, um das einzufrieren.
|
||
|
||
Ergänzt, ersetzt nicht: `doc/Kalibrierung.md` Schritt [4] (3-Pose-Kreis-Fit, nur
|
||
Marker-Mittelpunkte) bleibt die unabhängige Gegenmessung. Die Achs**richtung**
|
||
(`jointToParent.axis`) fitten beide Verfahren nicht.
|
||
|
||
---
|
||
|
||
## 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/<run>/aruco_marker_poses.json \
|
||
-robot scripts/robot_1781069752019.json \
|
||
--from-state data/homing/<run>/state_Arm2.json \
|
||
-out data/homing/<run>/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/<run>/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/<run>/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 `pose_estimation.fit_origin_link`** umgesetzt — ein
|
||
Konfig-Feld, direkt in `estimate_global_ba()` integriert (keine separate
|
||
Funktion/Report/CLI-Flag mehr), übernimmt das Ergebnis automatisch in
|
||
`robot.json` (`patch_robot_json_origin()`, Text-Patch). Generisch für jeden
|
||
Link, aktuell für `Arm1` aktiviert. Details: eigener Abschnitt oben.
|
||
|
||
**Noch offen:**
|
||
|
||
- [ ] **Adapter** `movements.<var>.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-Wert beobachten:** wandert bei jedem Lauf mit `fit_origin_link`
|
||
leicht weiter (s. Befund-Tabelle oben, wird kleiner/konvergiert bisher). Falls
|
||
das nicht erwünscht ist: Schalter nach dem Einschwingen auf `null` setzen, oder
|
||
gegen eine frische Verfahren-B-3-Pose-Messung gegenchecken.
|
||
- [ ] **`fit_origin_link` in der Kalibrierung-UI sichtbar machen** (`doc/Kalibrierung.md`
|
||
Schritt [4]) — aktuell nur in `robot.json` umschaltbar; Tab „Arm1 – Y" könnte
|
||
den aktuellen Wert/letzte Änderung neben Verfahren B anzeigen.
|
||
- [ ] **Mehrpose-Erweiterung** (mehrere `aruco_marker_poses.json` mit
|
||
gemeinsamem `origin`, je Pose ein eigener Gelenkwinkel, ein gemeinsamer Solve)
|
||
— würde die Winkel/Origin-Korrelation aus einer Einzelpose weiter reduzieren,
|
||
analog zur bestehenden 3-Pose-Aufnahme.
|
||
- [ ] `huber_delta_mm`/`normal_weight` ggf. gegen reale Marker-Genauigkeit
|
||
nachjustieren — reales Residuum (2,4–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 die Origin-Erweiterung in `estimate_global_ba()`
|
||
— aktuell nur manuell gegen die drei Fixtures verifiziert (s. oben);
|
||
appRobotHoming hat bisher keine Python-Testinfrastruktur (nur Jest/JS).
|
||
- [ ] 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").
|