diff --git a/doc/Homing_5_Pose_MultiPoint_Weighted.md b/doc/Homing_5_Pose_MultiPoint_Weighted.md new file mode 100644 index 0000000..cd685d3 --- /dev/null +++ b/doc/Homing_5_Pose_MultiPoint_Weighted.md @@ -0,0 +1,247 @@ +# Homing 5 – Verbesserungspfade: Mehrpunkt-Residuen & Gewichtung + +> Reine Wegbeschreibung — **nichts davon ist umgesetzt**. Ergänzt +> [`Homing_5_Pose.md`](Homing_5_Pose.md) um drei mögliche Verbesserungen, die +> alle an derselben Stelle ansetzen: *was* ein Marker als Messung beiträgt und +> *wie stark* sie zählt. Stand: 2026-06-16, basiert auf Code-Durchsicht +> (`scripts/1_detect_aruco_observations.py`, `3b_corner_marker_poses.py`, +> `5_pose_estimation.py`), keine Code-Änderung. + +--- + +## Ausgangslage (aktueller Code) + +Pro Marker fließen heute genau **6 Residuen** in `5_pose_estimation.py`s +`residual_vector()` ein: 3× Position (`pos_mm`) + 3× Normale (skaliert mit +`normal_weight`, Default 100). Beide kommen aus `aruco_marker_poses.json`, +geschrieben von `3b_corner_marker_poses.py`: + +``` +3b: trianguliert 4 Ecken (DLT, je Ecke eigene Multi-View-Triangulation) + → position_m/mm = Schwerpunkt der 4 Ecken + → normal = SVD-Ebenen-Fit durch die 4 Ecken + → corners_m = die 4 Ecken selbst (steht im JSON, wird aber von + 5_pose_estimation.py nie gelesen) +``` + +Alle drei folgenden Punkte hängen an genau dieser Kette. + +--- + +## 1. Vier Eckpunkte statt Center+Normal + +**Frage des Nutzers:** „Mit vier Eckpunkten hätten wir weit mehr Statistik, +und nicht die zusammengefassten Werte, oder?" — ja. + +**Befund:** `corners_m` steht bereits in `aruco_marker_poses.json` +(`3b_corner_marker_poses.py:166`), wird aber von +`load_observations()` (`5_pose_estimation.py:110`) nicht gelesen. Center und +Normale sind beide bereits verlustbehaftete Zusammenfassungen der 4 Ecken +(Mittelwert bzw. SVD-Ebenen-Fit) — der eigentliche Fit in `residual_vector()` +sieht nur noch diese zusammengefasste Version, nicht die Rohdaten. + +**Idee:** Pro Marker 4×3=12 Eck-Residuen statt 3+3 Center/Normal-Residuen. +Vorteile: +- Mehr unabhängige Messpunkte → realistischere Statistik/Unsicherheit. +- Eine einzelne schlecht erkannte Ecke (Verdeckung, Blur) verzerrt nicht mehr + automatisch Center *und* Normale des ganzen Markers — der robuste + Huber-Verlust könnte direkt auf Eck-Ebene greifen statt auf Marker-Ebene. +- Lage *und* Orientierung kommen aus denselben 4 Punkten — keine separate, + zusätzlich verlustbehaftete Normalen-Schätzung nötig. + +**Nötig dafür:** +- `robot_fk.py`: neue Methode (analog `marker_world()`), die die 4 lokalen + Eckpunkte eines Markers (aus `marker["size"]` + ArUco-Eckreihenfolge/Spin- + Konvention, schon bekannt aus `corner_plane_normal()` und + `doc/Kalibrierung_Marker.md`) ins Weltsystem transformiert. +- `5_pose_estimation.py`: `load_observations()` um `corners_m` erweitern, + `residual_vector()` um einen dritten Modus neben den bestehenden + `"corner_pose"`/`"center_point"`-Werten von `marker_observation`. + +**Risiko:** mittel. Die Spin/Eckreihenfolge-Konvention muss exakt stimmen, +sonst systematischer Bias statt Verbesserung. Gut gegen Simulations-GT +(appRobotRendering) prüfbar, bevor an reale Daten. + +--- + +## 2. Einzelkamera-Marker einbeziehen statt verwerfen + +**Frage des Nutzers:** Komplett verworfene 1-Kamera-Marker seien vermutlich +schlechter als sie schwach gewichtet einzubeziehen — insbesondere relevant für +zukünftige Finger-Marker, wo oft zwei Kameras je einen *anderen* Marker sehen. + +**Befund — zwei Stellen verwerfen heute hart:** +- `3b_corner_marker_poses.py:142`: `if len(cam_ids) < args.minCams: continue` + (Default `--minCams 2`) — ein 1-Kamera-Marker bekommt **gar keinen Eintrag** + in `aruco_marker_poses.json`. Das passiert schon **vor** `5_pose_estimation.py`. +- `5_pose_estimation.py`s `load_observations(..., min_cams=2)` filtert + zusätzlich — meist wirkungslos, weil 3b den Marker oft schon gar nicht erst + schreibt. + +### Reprojektions-Residuum statt Single-View-PnP (bevorzugter Weg) + +**Ursprünglich angenommener Weg — Single-View-PnP — explizit nicht bevorzugt.** +`triangulate_multiview()` (3b) ist Multi-View-DLT — mit 1 Kamera unterbestimmt, +ein einzelnes Bild kann keinen 3D-Punkt triangulieren. Der naheliegende Ersatz +wäre Single-View-PnP (bekannte Kantenlänge `size` als Skalen-Anker) — das hat +aber die bekannte **Flip-Ambiguität** (zwei spiegelbildliche Lösungen aus +einer Ansicht, vgl. appRobotRendering `scene_reconstruct.py`). Nur als +**Fallback** vorgesehen ("besser als nichts"), explizit nicht der Hauptweg. + +**Bevorzugt: Reprojektion in den bestehenden globalen Fit, keine eigene +Marker-Pose.** Kamera A ist gegen das Board bereits voll posenbestimmt +(Stufe 2: `K`,`D`,`R`,`t` bekannt). Statt für einen 1-Kamera-Marker einen +eigenständigen (zwangsläufig mehrdeutigen) 3D-Punkt zu rekonstruieren, geht er +direkt als **Bild-Residuum** in denselben globalen Zustand `q` ein: + +``` +für den Gelenkzustand q (einzige Unbekannte — keine separate Marker-Pose): + P_k(q) = T_link(q) · p_k_lokal (4 Eckpunkte via FK) + û_k = project(K_A, D_A, R_A, t_A; P_k(q)) (Reprojektion in Kamera A) + residual_k = û_k − u_k_beobachtet (Pixel-Differenz je Ecke) +``` + +Keine Flip-Ambiguität, weil keine unabhängige Marker-Pose gesucht wird — nur +`q` (7 Variablen, durch alles andere im Modell schon mitbestimmt). Ein falscher +`q` projiziert einfach sichtbar daneben, statt eine zweite gültige Lösung zu +erzeugen. + +**„Kette von Triangulations-Optionen" (Nutzeridee):** Geteilte Variablen +(z. B. `e` bei FingerA/FingerB) werden in `analyze_chain()` schon heute in +**einen** Block zusammengefasst — sind beide Finger-Marker einzeln +triangulierbar (je ≥2 Kameras), bestimmen sie `e` schon jetzt gemeinsam. Neu +wäre: auch wenn FingerA nur von Kamera 1 und FingerB nur von Kamera 2 gesehen +wird (keiner einzeln triangulierbar), könnten zwei Reprojektions-Residuen +(beide Funktionen desselben `q`) den geteilten Freiheitsgrad **gemeinsam** +einschränken — ohne dass einer der beiden Marker für sich eine vollständige +Pose bräuchte. Passt zur vom Nutzer ausdrücklich erlaubten groben Genauigkeit +(±10°/±10 mm reicht für die Finger völlig). + +**Bezug zu Finger-Positionen:** `Hand`/`Palm`/`FingerA`/`FingerB` haben aktuell +**0 Marker** in `robot.json`. Sobald dort Marker ergänzt werden, dürften sie +wegen Größe/Beweglichkeit/Verdeckung oft nur von 1 Kamera gesehen werden — mit +der harten ≥2-Kamera-Regel blieben diese Gelenke dann trotz vorhandener Marker +oft `confidence:"none"`. Die Teilbaum-Logik in `observability()` hilft nur, +wenn irgendein Marker *im Teilbaum* ≥2 Kameras hat — nicht, wenn alle +Finger-Marker isoliert nur je 1 Kamera sehen. + +**Nötig dafür (deutlich größer als „Schwellwert ändern"):** +- 3b (oder ein neuer Zwischenschritt): 1-Kamera-Beobachtungen nicht verwerfen, + sondern mit Kamera-Referenz + rohen 2D-Eckpunkten aufnehmen. +- `5_pose_estimation.py`: `load_observations()`/`estimate_pose()` müssten + zusätzlich Kamerakalibrierung (`{cam}_camera_pose.json`, NPZ-Intrinsik) + einlesen können — heute reicht `aruco_marker_poses.json` allein. +- `residual_vector()`: zweiter Residuumstyp (Pixel statt mm) **gemeinsam** mit + den bestehenden mm-Residuen optimiert, mit eigenem Gewicht (analog + `normal_weight`, aber für „Pixel vs. Millimeter"). + +**Risiko:** primär die **Gewichtung zwischen den beiden Residuumstypen** — +falsch skaliert, dominiert einer den Fit und verschlechtert sogar die heute +schon gut funktionierenden ≥2-Kamera-Marker. Architektonisch größer als +Punkt 1/3, aber ohne die Flip-Problematik des ursprünglich angenommenen Wegs. + +--- + +## 3. Marker-Qualitäts-Gewichtung (Größe/Schärfe/Distanz/Kontrast) + +**Vom Nutzer bestätigt:** bewusst (provisorisch) entfernt — brachte in +Simulationen wenig, Option soll aber offen bleiben. + +**Befund — die Bausteine existieren bereits, sind aber nicht verbunden:** +- `1_detect_aruco_observations.py` berechnet pro Detektion bereits: + `area_px`, `sharpness` (Laplace-Varianz, `compute_sharpness()`), `contrast`/ + `dynamic_range` (`compute_contrast()`), `distance_to_border_px`, kombiniert + zu einem `confidence`-Score (`compute_confidence()`) — geschrieben ins + `quality`-Feld jeder Detektion in `{cam}_aruco_detection.json`. +- `robot.json` hat dafür ein fertiges Schema: `observation_weighting` + (`distance_weight`, `marker_size_weight`, `view_angle_weight`) und + `multiview_calculation` (`size_factor`, `sharpness_factor`, `border_factor`, + `homography_factor`, `spin_factor`, `weight_floor`, ...). +- **Aber:** Dieses Schema liest nur `3_multiview_bundle_adjustment_v4.py` — + ein Skript, das im Homing-Pfad **nicht** läuft (Homing nutzt + `3b_corner_marker_poses.py`). +- `3b_corner_marker_poses.py`s `load_cameras()` liest aus der Detection-JSON + nur `camera_matrix`, `distortion_coefficients`, `image_points_px` — das + `quality`-Feld wird **nie gelesen**. Es geht also schon hier verloren, bevor + es überhaupt zu `aruco_marker_poses.json` käme. + +**Idee (falls reaktiviert):** Die Qualitätsmaße existieren schon ganz am +Anfang der Pipeline — nur durchreichen nötig: 3b liest `quality`/`confidence` +aus der Detection-JSON und schreibt einen `weight`-Wert pro Marker in +`aruco_marker_poses.json`; `5_pose_estimation.py`s `residual_vector()` skaliert +das jeweilige Markerresiduum damit (analog zu `normal_weight`, aber pro Marker +statt global für alle). + +**Mögliche Erklärung, warum es in Simulation wenig brachte:** Der +appRobotRendering-Renderfehler-Boden (`markerOffsetMaxMm`, `sensorNoise`, … +aus `renderingInfo`) ist recht gleichförmig über alle Marker — wenig echte +Qualitätsunterschiede zum Gewichten. Bei echten Kameras (Beleuchtung, +Entfernung, Bewegungsunschärfe) könnte die Streuung größer und der Effekt +dadurch sichtbarer sein — das ist der Grund, die Option offen zu halten. + +**Nötig dafür:** klein und lokal begrenzt — nur 2 Stellen (3b: `quality` +durchreichen; `5_pose_estimation.py`: Gewicht im Residuum nutzen). +`1_detect_aruco_observations.py` und das `robot.json`-Schema müssen nicht +angefasst werden, die liegen schon bereit. + +**Risiko:** niedrig (reines Durchreichen + ein Faktor) — der Aufwand liegt im +erneuten Validieren (Simulation + reale Daten), nicht im Code selbst. + +--- + +## Zusammenhang der drei Punkte + +Alle drei ändern letztlich dasselbe: was als „eine Messung" zählt und wie +stark sie zählt. Sie sind unabhängig umsetzbar, aber am Ende würde man sie +zusammenführen wollen: + +``` +heute: residual = [Δposition, Δnormal × normal_weight] (6 Werte/Marker, nur ≥2-Kamera-Marker) +1: residual = [Δcorner_0 .. Δcorner_3] × marker_weight (12 Werte/Marker, weiterhin ≥2-Kamera) +2: + Reprojektions-Residuen für 1-Kamera-Marker (neuer Typ, eigene Gewichtung) +3: marker_weight zusätzlich nach Quality-Score aus Stufe 1 +``` + +**Geschätzte Reihenfolge nach Aufwand/Risiko** (keine Festlegung, nur +Einschätzung): 3 (niedrig, reines Durchreichen) → 1 (mittel, FK-Erweiterung, +gut simulationstestbar) → 2 (architektonisch am größten — neuer Residuumstyp ++ Kamerakalibrierung als zusätzlicher Input —, aber ohne Flip-Risiko, seit +Reprojektion statt PnP der bevorzugte Weg ist). + +--- + +## Umsetzungsplan (ToDo) + +Reihenfolge folgt der Risiko-Einschätzung oben: erst risikoarmes Durchreichen, +dann die beiden strukturellen Erweiterungen. Jeder Schritt ist einzeln +umsetzbar und (wo möglich) einzeln testbar, bevor der nächste beginnt. + +| # | Schritt | Testbar an | Bricht Bestehendes? | +|---|---|---|---| +| 1 | **(Punkt 3)** `quality`/`confidence` aus `{cam}_aruco_detection.json` bis nach `aruco_marker_poses.json` durchreichen (3b liest es, schreibt ein neues `weight`-Feld pro Marker) | Diff der Ausgabedatei: nur das neue Feld ist zusätzlich da, alles andere (Position, Normale, …) identisch zu vorher — reiner Additivitätstest | **Nein.** Rein additives Feld, kein Pflichtfeld, alte Leser ignorieren es | +| 2 | **(Punkt 3)** `residual_vector()` nutzt das neue Gewicht, hinter einem Schalter (`pose_estimation.use_marker_weight`, Default `false`) | A/B-Vergleich auf den appRobotRendering-Simulationsszenen (mit/ohne Schalter) gegen bekannte Grundwahrheit — genau der Test, der laut Nutzer beim ersten Versuch wenig brachte, jetzt wiederholbar | **Nein bei Default aus.** Mit `true`: Ergebnisse ändern sich gewollt — muss vor Produktiv-Default separat validiert werden | +| 3 | **(Punkt 1)** `robot_fk.py`: neue Methode liefert die 4 lokalen Eckpunkte eines Markers im Weltsystem (Baustein, noch ohne Anbindung) | Isoliert testbar, ganz ohne `5_pose_estimation.py`: gegen die wahren Eckpositionen aus `render_*.json` (Simulation liefert das schon) | **Nein.** Neue, bisher von niemandem aufgerufene Methode | +| 4 | **(Punkt 1)** Neuer `marker_observation`-Modus (z. B. `"corner_points"`) nutzt die 12 Eck-Residuen statt 6 Center/Normal-Residuen | Direkter Vorher/Nachher-Vergleich gegen dieselbe Validierungstabelle wie in `Homing_5_Pose.md` (10 Simulationsposen, bekannte GT) | **Nein als Opt-in** (Default bleibt `"corner_pose"`). Tuning-Punkt: `huber_delta_mm` ist auf die heutige Residuumsgröße kalibriert — mit 12 statt 6 Werten/Marker verschiebt sich die RMS-Größenordnung, müsste neu eingeordnet werden | +| 5 | **(Punkt 2)** 3b nimmt 1-Kamera-Beobachtungen mit auf (rohe 2D-Ecken + Kamera-Referenz), statt sie zu verwerfen | Output-Diff: nur neue Einträge für vorher fehlende Marker, bestehende ≥2-Kamera-Einträge unverändert | **Potenziell ja.** Jeder Code, der `aruco_marker_poses.json` liest und für *jeden* Markereintrag eine triangulierte `position_mm` erwartet (Viewer, `9_evaluateMarker.py`, CSV-Export) müsste Einträge ohne 3D-Position vertragen — vor diesem Schritt prüfen, welche Consumer das betrifft | +| 6 | **(Punkt 2)** `residual_vector()` um Reprojektions-Residuen erweitert; `load_observations()`/`estimate_pose()` lesen zusätzlich Kamerakalibrierung | Zuerst an Simulationsszenen, bei denen gut beobachtete Marker künstlich auf „nur 1 Kamera" reduziert werden (GT bekannt) — saubere Kontrolle, ob das Residuum tatsächlich hilft, bevor reale Finger-Marker überhaupt existieren | **Nein strukturell** (additiver Residuumstyp), aber Regressionsrisiko durch falsche mm/px-Gewichtung — vor Produktiv-Default gegen die bestehende Validierungstabelle gegenprüfen (wie Schritt 4) | +| 7 | Zusammenführen: ein gemeinsames Gewichtsschema (Quality × Kamera-Anzahl × Residuumstyp) statt drei separater Schalter | End-to-End gegen Simulationsbenchmark **und** die drei realen Fixtures aus `Homing_5_Pose.md` | **Nein**, wenn alle Vorstufen additive Defaults hatten | + +## Offene Punkte + +- [ ] Keiner der drei Punkte/Schritte ist priorisiert/entschieden — reine Optionen. +- [ ] Schritt 5 zuerst: welche Tools lesen `aruco_marker_poses.json` und setzen + eine triangulierte Position für jeden Eintrag voraus? (Viewer, Eval-Skripte) +- [ ] Für Schritt 3/4: Eckreihenfolge/Spin-Konvention zuerst exakt verifizieren, + bevor Residuen darauf aufbauen. +- [ ] Für Schritt 2: erneute Simulationsvalidierung, bevor eine Wiedereinführung + der Qualitätsgewichtung sinnvoll beurteilt werden kann. + +## Verweise + +- [`Homing_5_Pose.md`](Homing_5_Pose.md) — Hauptdokument zu `5_pose_estimation.py` +- `scripts/1_detect_aruco_observations.py` — Qualitätsmaße je Detektion +- `scripts/3b_corner_marker_poses.py` — Triangulation, Center/Normal-Aggregation, + ≥2-Kamera-Filter +- `scripts/3_multiview_bundle_adjustment_v4.py` — einziger Ort, der das + bestehende Gewichtungsschema (`observation_weighting`/`multiview_calculation`) + aktuell liest (nicht im Homing-Pfad) diff --git a/scripts/robot_1781069752019.json b/scripts/robot_1781069752019.json index fa5031d..1ddac2f 100644 --- a/scripts/robot_1781069752019.json +++ b/scripts/robot_1781069752019.json @@ -237,7 +237,9 @@ {"id": 102, "set": "A0", "position": [649.69, -223.0, -27.3], "normal": [0, 0, 1], "spin": 90}, {"id": 103, "set": "A0", "position": [105.71, -187.71, -27.3], "normal": [0, 0, 1], "spin": 90}, {"id": 104, "set": "A0", "position": [826.71, 239.16, -27.3], "normal": [0, 0, 1], "spin": 90}, - {"id": 105, "set": "A0", "position": [524.84, -266.25, -27.3], "normal": [0, 0, 1], "spin": 90} + {"id": 105, "set": "A0", "position": [524.84, -266.25, -27.3], "normal": [0, 0, 1], "spin": 90}, + {"id": 217, "set":"rail", "position": [732.5,-22.9,7.0], "normal":[0,0,1], "spin":90}, + {"id": 210, "set":"rail", "position":[122.7,-15.3,-0.2], "normal":[0,0,1]} ], "model": [ {