Draft MultiPoint

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)

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": [
         {