Multipoint Schritt 3

doc/Homing_5_Pose_MultiPoint_Weighted.md

@@ -1,11 +1,16 @@
 # Homing 5 – Verbesserungspfade: Mehrpunkt-Residuen & Gewichtung
 
-> Reine Wegbeschreibung — **nichts davon ist umgesetzt**. Ergänzt
+> Ergänzt [`Homing_5_Pose.md`](Homing_5_Pose.md) um drei mögliche
-> [`Homing_5_Pose.md`](Homing_5_Pose.md) um drei mögliche Verbesserungen, die
+> Verbesserungen, die alle an derselben Stelle ansetzen: *was* ein Marker als
-> alle an derselben Stelle ansetzen: *was* ein Marker als Messung beiträgt und
+> Messung beiträgt und *wie stark* sie zählt.
-> *wie stark* sie zählt. Stand: 2026-06-16, basiert auf Code-Durchsicht
+>
-> (`scripts/1_detect_aruco_observations.py`, `3b_corner_marker_poses.py`,
+> **Status (2026-06-25):** **Qualitäts-Gewichtung** (Doc-Punkt 3 / Schritte 1+2)
-> `5_pose_estimation.py`), keine Code-Änderung.
+> umgesetzt. **Mehrpunkt-Eckresiduen** (Doc-Punkt 1 / Schritte 3+4) umgesetzt
+> als Opt-in-Modus `marker_observation="corner_points"` — 12 Eck-Residuen für
+> Roboter-Links, 1 Center-Residuum für die (spin-unkalibrierten) Board/Rail-
+> Marker auf dem Root-Link. Endgültiges Tuning/Validierung gegen Simulation
+> steht aus (siehe Notiz unten). **Einzelkamera-Einbindung** (Doc-Punkt 2 /
+> Schritte 5+6) weiterhin **offen** — siehe Status-Spalte unten.
 
 ---
 
@@ -216,15 +221,21 @@ 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? |
+> **Status-Legende:** ✅ erledigt · ⬜ **offen**
-|---|---|---|---|
+>
-| 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 |
+> **Punkt-Zuordnung** (Doc-Abschnitt ↔ Chat-Nummerierung): Doc-Punkt 1 „Vier
-| 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 |
+> Eckpunkte" = Schritte 3+4 · Doc-Punkt 2 „Einzelkamera" = Schritte 5+6 ·
-| 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 |
+> Doc-Punkt 3 „Qualitäts-Gewichtung" = Schritte 1+2 (erledigt).
-| 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 | **Ja, konkret** — siehe Konsumenten-Recherche direkt unter der Tabelle. Mehrere Stellen brauchen einen Guard, **bevor** dieser Schritt scharf geschaltet wird |
+| # | Status | Schritt | Testbar an | Bricht Bestehendes? |
-| 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 |
+| 1 | ✅ erledigt (2026-06-17) | **(Doc-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 | ✅ erledigt (2026-06-17) | **(Doc-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 | ✅ erledigt (2026-06-25) | **(Doc-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 | 🟡 Code erledigt (2026-06-25), Tuning offen | **(Doc-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 | ⬜ **offen** (Vorarbeit/Guards ✅) | **(Doc-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 | **Ja, konkret** — siehe Konsumenten-Recherche direkt unter der Tabelle. Mehrere Stellen brauchen einen Guard, **bevor** dieser Schritt scharf geschaltet wird |
+| 6 | ⬜ **offen** | **(Doc-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 | ⬜ **offen** | 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 |
 
 ### Recherche zu Schritt 5: wer liest `aruco_marker_poses.json`? (2026-06-16)
 
@@ -259,6 +270,34 @@ Homing-Seite selbst (`editRobot.js`, `homing.js`) ist bereits robust.
 
 Alle anderen Konsumenten (`homing.js`, `editRobot.js` → `assignByZRange`/`alignSetToMeasured`, `scripts/9_evaluateMarker.py`) waren schon robust oder sind Offline-Benchmark-Code ohne Produktionsrelevanz.
 
+### Umsetzung Schritt 3+4 (Doc-Punkt 1) — Befunde (2026-06-25)
+
+- **Schritt 3** (`robot_fk.py`): `marker_corners_local/_world` + `all_markers_world`
+  liefern jetzt `corners_world` (4×3, in `corners_m`-Reihenfolge). Orientierung =
+  Spin um die Normale ∘ Minimal-Rotation [0,0,1]→Normale (exakt wie
+  boardViewer.html). Verifiziert in `test/test_robot_fk_corners.py`:
+  Selbst-Konsistenz (Center/Kanten/planar/Normalen-Rückgewinnung) **und** gegen
+  echte triangulierte Roboter-Ecken am Seed-Pose (~1 mm RMS, Identitäts-Paarung
+  schlägt jede Umordnung). Eckreihenfolge = `(+h,+h),(+h,-h),(-h,-h),(-h,+h)`
+  (= boardViewer-Zeiger (1,1,0)).
+- **Wichtiger Konventions-Befund:** Die `spin`-Werte sind nur für die
+  **Roboter-Marker** kalibriert/visuell verifiziert, **nicht** für die Board/
+  Rail-Marker (Set A0/rail, alle auf dem Root-Link `Board`). Deren Eckreihenfolge
+  liegt ~90° daneben. Lösung: `corner_points` nutzt 12 Eck-Residuen nur für
+  Roboter-Links (`corner_point_links`, Default = alle Nicht-Root-Links) und 1
+  Center-Residuum für die Board/Rail-Marker. Da `Board` Root ist (Residuum
+  konstant bzgl. der Gelenke), kostet das nichts an Information.
+- **Datenbefund (nicht Code):** 6 Marker des Board-Sets **A0 sind auf `Arm1`
+  fehlzugeordnet** (`[55,56,57,77,78,99]`), ~230 mm neben dem Modell. Sie
+  destabilisieren `corner_points` (12 statt 3 Residuen → falsches Minimum) und
+  ziehen auch `corner_pose` leicht. Auf bereinigten Markern konvergiert
+  `corner_points` ≈ `corner_pose`. → Marker-Zuordnung korrigieren (separate
+  Kalibrier-Aufgabe).
+- **Offen (Schritt 4 Tuning):** `huber_delta_mm` ist auf 6 Residuen/Marker
+  kalibriert; mit 12 verschiebt sich die RMS-Größenordnung. Sauberes A/B + Tuning
+  gegen appRobotRendering-Simulations-GT (klare Daten) steht aus, bevor der Modus
+  produktiv als Default taugt. CLI: `--marker-observation corner_points`.
+
 ## Offene Punkte
 
 - [ ] Keiner der drei Punkte/Schritte ist priorisiert/entschieden — reine Optionen.

scripts/5_pose_estimation.py

@@ -20,9 +20,15 @@ Four switchable methods (robot.json -> pose_estimation.method):
   global_ba         : all variables jointly, position + normal residuals, robust loss
   hybrid            : sequential_fk init -> global_ba refine   (default, most stable)
 
-Observation input:
+Observation input (robot.json -> pose_estimation.marker_observation):
-  marker_observation = "corner_pose"  -> aruco_marker_poses.json (pos + measured normal)
+  "corner_pose"   (default) -> aruco_marker_poses.json: 3 pos + 3 normal residuals/marker
-  marker_observation = "center_point" -> aruco_positions_*.json   (pos only)
+  "corner_points"           -> aruco_marker_poses.json: 12 corner residuals for
+                               robot-link markers (4 triangulated corners vs FK
+                               corners; no separate normal), 1 center residual for
+                               root-link (Board: floor/rail) markers whose spin is
+                               uncalibrated. Robust loss acts per corner. Opt-in;
+                               needs `corners_m`. Links via corner_point_links.
+  "center_point"            -> aruco_positions_*.json: position only
 
 Homing integration (appRobotHoming, see doc/Homing_5_Pose.md):
   --from-state <json>        seed/init state (flat {var: value}, or the
@@ -89,6 +95,14 @@ DEFAULT_CFG: Dict[str, Any] = {
     "min_cameras_per_marker": 2,
     "finger_block_joints": ["b", "c", "e"],
     "per_link_method": {},
+    # Nur im marker_observation="corner_points"-Modus relevant: welche Links die
+    # 4 Eck-Residuen nutzen. None/absent = alle Nicht-Root-Links (= der Roboter);
+    # der Root-Link (Board mit Boden-/Rail-Markern) nutzt ein Center-Residuum.
+    # Hintergrund: nur die Roboter-Marker-Spins sind kalibriert/verifiziert; die
+    # Board/Rail-Spins nicht — deren Eckreihenfolge wäre unzuverlässig. Board ist
+    # zudem Root (Residuum konstant bzgl. der Gelenke). Explizite Liste möglich,
+    # z.B. ["Arm1","Ellbow","Arm2","Hand","Palm","FingerA","FingerB"].
+    "corner_point_links": None,
     # One switch: if set to a link name (e.g. "Arm1"), that link's
     # jointToParent.origin Y/Z is fit together with the normal pose (same
     # global_ba solve) and the result is written back into robot.json
@@ -111,8 +125,12 @@ def load_pose_cfg(robot_data: Dict[str, Any]) -> Dict[str, Any]:
 def load_observations(path: str, use_normals: bool, min_cams: int = 2) -> Dict[int, Dict[str, Any]]:
     """
     Load marker observations. Accepts aruco_marker_poses.json (with measured
-    normal + num_cameras) or aruco_positions_*.json (position only).
+    normal + num_cameras + 4 triangulated corners) or aruco_positions_*.json
-    Returns: marker_id -> {pos_mm:(3,), normal:(3,)|None, link:str, n_cams:int}
+    (position only).
+    Returns: marker_id -> {pos_mm:(3,), normal:(3,)|None, corners_mm:(4,3)|None,
+                           link:str, n_cams:int, weight:float}
+    corners_mm (aus `corners_m`, m→mm) speist den marker_observation=
+    "corner_points"-Modus in residual_vector().
     """
     data = json.load(open(path, "r", encoding="utf-8"))
     out: Dict[int, Dict[str, Any]] = {}
@@ -135,7 +153,14 @@ def load_observations(path: str, use_normals: bool, min_cams: int = 2) -> Dict[i
             nn = np.linalg.norm(nv)
             if nn > 1e-9:
                 nrm = nv / nn
-        out[mid] = {"pos_mm": pos, "normal": nrm, "link": m.get("link", "?"), "n_cams": n_cams,
+        corners_mm = None
+        cm = m.get("corners_m")
+        if cm is not None:
+            arr = np.array(cm, dtype=float)
+            if arr.shape == (4, 3):
+                corners_mm = arr * 1000.0
+        out[mid] = {"pos_mm": pos, "normal": nrm, "corners_mm": corners_mm,
+                    "link": m.get("link", "?"), "n_cams": n_cams,
                     "weight": float(m.get("weight", 1.0))}
     return out
 
@@ -268,14 +293,64 @@ def model_markers(fk: RobotFK, state: Dict[str, float]) -> Dict[int, Dict[str, n
     return fk.all_markers_world(T)  # mid -> {world_mm, normal_world, link, local_mm}
 
 
+def _resolve_corner_links(fk: RobotFK, cfg: Dict[str, Any]) -> set:
+    """
+    Welche Links im "corner_points"-Modus die 4 Eck-Residuen nutzen.
+    Explizite Liste in cfg["corner_point_links"], sonst alle Nicht-Root-Links
+    (der Root-Link Board trägt die unkalibrierten Boden-/Rail-Marker und nutzt
+    ein Center-Residuum).
+    """
+    explicit = cfg.get("corner_point_links")
+    if isinstance(explicit, list) and explicit:
+        return set(explicit)
+    links = fk.links
+    roots = {ln for ln, ld in links.items()
+             if not ld.get("parent") or ld.get("parent") not in links}
+    return set(links.keys()) - roots
+
+
 def residual_vector(state: Dict[str, float], fk: RobotFK, obs: Dict[int, Dict[str, Any]],
                     marker_ids: List[int], cfg: Dict[str, Any]) -> np.ndarray:
-    """Position (mm) + optional normal (scaled) residuals over the given markers."""
+    """
+    Residuen über die gegebenen Marker. Modus via pose_estimation.marker_observation:
+
+      "corner_pose" (Default): 3 Position (mm) + optional 3 Normale
+                               (×normal_weight) je Marker — wie bisher.
+      "corner_points":         12 Eck-Residuen (4 Ecken × xyz, mm) je Marker auf
+                               einem Roboter-Link (corner_point_links), KEINE
+                               separate Normale (Orientierung steckt in den
+                               Ecken). Mehr unabhängige Messpunkte, robuster
+                               Verlust greift auf Eck-Ebene. Marker auf dem
+                               Root-Link (Board: Boden-/Rail-Marker mit
+                               unkalibriertem Spin) oder ohne `corners_mm`
+                               nutzen EIN Center-Residuum (3, "ein Punkt pro
+                               Marker") — gleiche mm-Skala → ein huber_delta_mm.
+    """
     model = model_markers(fk, state)
     res: List[float] = []
+    use_mw = bool(cfg.get("use_marker_weight", False))
+    obs_mode = str(cfg.get("marker_observation", "corner_pose")).lower()
+
+    if obs_mode == "corner_points":
+        corner_links = _resolve_corner_links(fk, cfg)
+        for mid in marker_ids:
+            if mid not in model or mid not in obs:
+                continue
+            mw = float(obs[mid].get("weight", 1.0)) if use_mw else 1.0
+            mm = model[mid]
+            oc = obs[mid].get("corners_mm")
+            mc = mm.get("corners_world")
+            if mm.get("link") in corner_links and oc is not None and mc is not None:
+                dc = (np.asarray(mc, float) - np.asarray(oc, float)) * mw   # (4,3)
+                res.extend(dc.reshape(-1).tolist())                         # 12 Werte
+            else:
+                dp = (np.asarray(mm["world_mm"], float) - obs[mid]["pos_mm"]) * mw
+                res.extend(dp.tolist())                                     # Center (1 Punkt)
+        return np.asarray(res, dtype=float)
+
+    # Default: Center (mm) + optionale Normale (skaliert)
     w_n = float(cfg.get("normal_weight", 30.0))
     use_n = bool(cfg.get("use_normals", True))
-    use_mw = bool(cfg.get("use_marker_weight", False))
     for mid in marker_ids:
         if mid not in model or mid not in obs:
             continue
@@ -646,6 +721,9 @@ def main() -> None:
     ap.add_argument("-robot", "--robot", required=True)
     ap.add_argument("-out", "--out", default=None)
     ap.add_argument("--method", default=None, help="override robot.json method")
+    ap.add_argument("--marker-observation", default=None, dest="marker_observation",
+                    help="override robot.json marker_observation "
+                         "(corner_pose | corner_points | center_point)")
     ap.add_argument("--from-state", default=None, metavar="JSON",
                     help="Seed/init state (flat {var:value} or {accumulated_state:{...}} as "
                          "written by 4b_revolute_angle.py). Used as x0 for global_ba/hybrid "
@@ -656,6 +734,8 @@ def main() -> None:
     cfg = load_pose_cfg(robot_data)
     if args.method:
         cfg["method"] = args.method
+    if args.marker_observation:
+        cfg["marker_observation"] = args.marker_observation
 
     fk = RobotFK(robot_data)
     obs = load_observations(args.markers, cfg.get("use_normals", True),

scripts/robot_fk.py

@@ -192,14 +192,109 @@ class RobotFK:
         """Transform a local marker position → world (mm)."""
         return transform_point(transforms.get(link_name, np.eye(4)), local_pos)
 
+    # ── Marker-Eckpunkte (Mehrpunkt-Residuen, doc/Homing_5_Pose_MultiPoint_Weighted.md Schritt 3) ──
+
+    @staticmethod
+    def _shortest_arc_R(normal: Sequence[float]) -> np.ndarray:
+        """
+        Rotationsmatrix [0,0,1] → normal (kürzester Bogen).
+
+        Repliziert THREE.Quaternion.setFromUnitVectors(vFrom=[0,0,1], vTo=n)
+        exakt (inkl. antiparallelem Sonderfall), damit die hier erzeugten
+        Ecken zur visuell verifizierten Orientierungs-Konvention in
+        boardViewer.html passen (qNormalLoc dort).
+        """
+        n = np.asarray(normal, dtype=float)
+        nn = float(np.linalg.norm(n))
+        n = n / nn if nn > 1e-12 else np.array([0.0, 0.0, 1.0])
+        # three.js: quat = (cross([0,0,1], n), dot([0,0,1], n) + 1), dann normalisieren.
+        r = float(n[2]) + 1.0
+        if r < 1e-12:
+            # antiparallel (n == [0,0,-1]): three.js else-Zweig für vFrom=[0,0,1] → (0,-1,0,0)
+            qx, qy, qz, qw = 0.0, -1.0, 0.0, 0.0
+        else:
+            qx, qy, qz, qw = -float(n[1]), float(n[0]), 0.0, r   # cross([0,0,1], n) = (-ny, nx, 0)
+        ql = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw)
+        qx, qy, qz, qw = qx / ql, qy / ql, qz / ql, qw / ql
+        return np.array([
+            [1 - 2 * (qy * qy + qz * qz), 2 * (qx * qy - qz * qw),     2 * (qx * qz + qy * qw)],
+            [2 * (qx * qy + qz * qw),     1 - 2 * (qx * qx + qz * qz), 2 * (qy * qz - qx * qw)],
+            [2 * (qx * qz - qy * qw),     2 * (qy * qz + qx * qw),     1 - 2 * (qx * qx + qy * qy)],
+        ])
+
+    @staticmethod
+    def _marker_plane_corners(half: float) -> np.ndarray:
+        """
+        Die 4 Eckpunkte in der Marker-Ebene (+Z = Normale), in DERSELBEN
+        Reihenfolge wie die von 3b_corner_marker_poses.py triangulierten
+        `corners_m` (ArUco 0..3, im Uhrzeigersinn von der Vorderseite gesehen).
+
+        Ecke 0 zeigt in Richtung (+h, +h) — dieselbe Konvention wie der visuell
+        kalibrierte Orientierungszeiger (1,1,0) in boardViewer.html. Damit passt
+        sie zu den manuell/visuell gesetzten `spin`-Werten der ARM-Marker, die
+        im Homing die Gelenkwinkel bestimmen (gegen echte corners_m am
+        Seed-Pose verifiziert, test_robot_fk_corners.py, RMS ~1 mm).
+
+        Hinweis: Die Board-Referenzmarker (Set A0) sind ~90° anders kalibriert,
+        ihre Eckreihenfolge passt unter dieser Konvention NICHT — egal, weil
+        Board der Root-Link ist: ihr Eck-Residuum ist konstant bzgl. der
+        Gelenkvariablen und beeinflusst die Schätzung nicht (der robuste
+        Huber-Verlust dämpft es als Ausreißer). Siehe Doc-Notiz.
+
+        Die Drehrichtung 0→1→2→3 ist so, dass 3b's `corner_plane_normal()`
+        (outward = -cross(e01,e02)) wieder +Z liefert — identisch zur
+        Beobachtungs-Konvention.
+        """
+        h = float(half)
+        return np.array([
+            [ h,  h, 0.0],   # 0
+            [ h, -h, 0.0],   # 1
+            [-h, -h, 0.0],   # 2
+            [-h,  h, 0.0],   # 3
+        ])
+
+    @classmethod
+    def marker_corners_local(cls,
+                             position: Sequence[float],
+                             normal: Sequence[float],
+                             size_mm: float,
+                             spin_deg: float = 0.0) -> np.ndarray:
+        """
+        Die 4 Marker-Ecken im LINK-lokalen Frame (mm), Reihenfolge wie `corners_m`.
+
+        Orientierung = Spin um die Normale ∘ Minimal-Rotation [0,0,1]→Normale,
+        exakt wie boardViewer.html (qSpinLoc.multiply(qNormalLoc)).
+        """
+        n = np.asarray(normal, dtype=float)
+        R = _rot_axis_angle(n, float(spin_deg)) @ cls._shortest_arc_R(n)
+        plane = cls._marker_plane_corners(float(size_mm) / 2.0)   # (4,3)
+        return np.asarray(position, dtype=float) + (R @ plane.T).T  # (4,3)
+
+    @classmethod
+    def marker_corners_world(cls,
+                             transforms: Dict[str, np.ndarray],
+                             link_name: str,
+                             position: Sequence[float],
+                             normal: Sequence[float],
+                             size_mm: float,
+                             spin_deg: float = 0.0) -> np.ndarray:
+        """Die 4 Marker-Ecken im Weltframe (mm), Reihenfolge wie `corners_m`."""
+        T = transforms.get(link_name, np.eye(4))
+        local = cls.marker_corners_local(position, normal, size_mm, spin_deg)
+        return np.array([transform_point(T, c) for c in local])
+
     def all_markers_world(self,
                           transforms: Dict[str, np.ndarray]
                           ) -> Dict[int, Dict[str, Any]]:
         """
         Returns
         -------
-        dict  marker_id -> {world_mm, local_mm, link, normal_world}
+        dict  marker_id -> {world_mm, local_mm, link, normal_world, corners_world}
+
+        corners_world: (4,3) Welt-mm in `corners_m`-Reihenfolge (für den
+        marker_observation="corner_points"-Modus in 5_pose_estimation.py).
         """
+        default_size = float((self.robot.get("markerDefaults", {}) or {}).get("size", 25.0))
         result: Dict[int, Dict[str, Any]] = {}
         for lname, ldata in self.links.items():
             T = transforms.get(lname, np.eye(4))
@@ -210,11 +305,15 @@ class RobotFK:
                     continue
                 loc = np.array(m["position"], dtype=float)
                 nor = np.array(m.get("normal", [0, 0, 1]), dtype=float)
+                size_mm = float(m.get("size", default_size))
+                spin_deg = float(m.get("spin", 0.0))
+                local_corners = self.marker_corners_local(loc, nor, size_mm, spin_deg)
                 result[mid] = {
                     "world_mm":    transform_point(T, loc),
                     "local_mm":    loc,
                     "link":        lname,
                     "normal_world": (R @ nor) / max(np.linalg.norm(R @ nor), 1e-12),
+                    "corners_world": np.array([transform_point(T, c) for c in local_corners]),
                 }
         return result
 

test/test_robot_fk_corners.py

@@ -0,0 +1,169 @@
+#!/usr/bin/env python3
+"""
+test_robot_fk_corners.py
+========================
+Isolierter Test für RobotFK.marker_corners_world/_local (doc/
+Homing_5_Pose_MultiPoint_Weighted.md, Schritt 3) — der Baustein für den
+marker_observation="corner_points"-Modus.
+
+Kein pytest nötig (das Repo testet sonst per Jest): plain asserts + main(),
+Aufruf:  python test/test_robot_fk_corners.py
+
+Geprüft wird die KONVENTION (Eckreihenfolge/Spin/Winding), denn ein Versatz
+dort gäbe systematischen Bias statt Verbesserung:
+
+  A) Selbst-Konsistenz je Marker (versch. Normalen/Spins):
+       - Schwerpunkt der 4 Ecken == Marker-Center
+       - alle 4 Kanten == size, planar
+       - die aus den Ecken via 3b-Konvention (corner_plane_normal) zurück-
+         gewonnene Normale == Marker-Normale  → Orientierung + Winding stimmen
+  B) Reale Daten: ROBOTER-Marker (Arm1/Ellbow/Arm2) am Seed-Pose eines echten
+     Captures — die FK-Ecken müssen zu den triangulierten `corners_m` passen,
+     UND die Identitäts-Paarung (Ecke i ↔ Ecke i) muss jede zyklische/
+     gespiegelte Umordnung schlagen → beweist Ecke-0 + Drehrichtung gegen echte
+     Triangulation. Nur Roboter-Marker, weil deren Spin kalibriert/verifiziert
+     ist; die Board/Rail-Marker (Root-Link) sind spin-unkalibriert und werden im
+     Fit bewusst per Center-Residuum behandelt (nicht über Ecken).
+"""
+import json
+import os
+import sys
+
+import numpy as np
+
+HERE = os.path.dirname(os.path.abspath(__file__))
+sys.path.insert(0, os.path.join(HERE, "..", "scripts"))
+from robot_fk import RobotFK  # noqa: E402
+
+ROBOT_JSON = os.path.join(HERE, "..", "scripts", "robot_1781069752019.json")
+CAP_DIR    = os.path.join(HERE, "homing", "20260616_133151")
+CAPTURE    = os.path.join(CAP_DIR, "aruco_marker_poses.json")
+SEED       = os.path.join(CAP_DIR, "state_Arm2.json")
+
+
+def recovered_normal(corners3d: np.ndarray) -> np.ndarray:
+    """3b_corner_marker_poses.py corner_plane_normal() — identisch nachgebaut."""
+    center = corners3d.mean(axis=0)
+    _, _, Vt = np.linalg.svd(corners3d - center)
+    n = Vt[-1]
+    cross = np.cross(corners3d[1] - corners3d[0], corners3d[2] - corners3d[0])
+    if np.dot(n, cross) > 0:
+        n = -n
+    nn = np.linalg.norm(n)
+    return n / nn if nn > 1e-12 else n
+
+
+def edge_lengths(c: np.ndarray):
+    return [float(np.linalg.norm(c[(i + 1) % 4] - c[i])) for i in range(4)]
+
+
+# ── A) Selbst-Konsistenz ────────────────────────────────────────────────────
+
+def test_self_consistency():
+    fk = RobotFK.from_file(ROBOT_JSON)
+    cases = [
+        ([0, 0, 1],   0.0),
+        ([0, 0, 1],  90.0),
+        ([0, 0, 1],  37.5),
+        ([0, -1, 0], 90.0),
+        ([-1, 0, 0],  0.0),
+        ([0, 0, -1], 12.0),          # antiparalleler Sonderfall der Minimal-Rotation
+        ([1, 1, 0.3], 215.0),        # schräge Normale + großer Spin
+    ]
+    size = 25.0
+    pos = np.array([100.0, -50.0, 30.0])
+    for normal, spin in cases:
+        local = RobotFK.marker_corners_local(pos, normal, size, spin)
+        assert local.shape == (4, 3), f"shape {local.shape}"
+        # Schwerpunkt == Center
+        d_center = np.linalg.norm(local.mean(axis=0) - pos)
+        assert d_center < 1e-9, f"centroid off by {d_center} (n={normal}, spin={spin})"
+        # Kanten == size, alle gleich (Quadrat)
+        for L in edge_lengths(local):
+            assert abs(L - size) < 1e-9, f"edge {L} != {size} (n={normal}, spin={spin})"
+        # planar: alle 4 in einer Ebene (Abstand zur SVD-Ebene ~0)
+        rel = local - local.mean(axis=0)
+        _, _, Vt = np.linalg.svd(rel)
+        planar = np.max(np.abs(rel @ Vt[-1]))
+        assert planar < 1e-9, f"not planar ({planar})"
+        # Normale aus den Ecken (3b-Konvention) == Marker-Normale
+        n_exp = np.asarray(normal, float) / np.linalg.norm(normal)
+        n_rec = recovered_normal(local)
+        dot = float(np.dot(n_exp, n_rec))
+        assert dot > 0.99999, (
+            f"recovered normal {n_rec} != {n_exp} (dot={dot:.6f}, spin={spin})")
+    print(f"[PASS] A) Selbst-Konsistenz: {len(cases)} Fälle "
+          "(Center, Kanten, planar, Normalen-Rückgewinnung).")
+
+
+# ── B) Reale Capture-Daten (Board-Marker) ───────────────────────────────────
+
+def best_pairing_error(model_c: np.ndarray, obs_c: np.ndarray):
+    """
+    RMS der Eck-Paarung nach Center-Abzug, für Identität und alle 8 Umordnungen
+    (4 zyklische × {vorwärts, rückwärts}). Gibt (rms_identity, rms_best, name_best).
+    """
+    m = model_c - model_c.mean(axis=0)
+    o = obs_c - obs_c.mean(axis=0)
+    best_rms, best_name, id_rms = None, None, None
+    for reverse in (False, True):
+        base = o[::-1] if reverse else o
+        for roll in range(4):
+            cand = np.roll(base, roll, axis=0)
+            rms = float(np.sqrt(np.mean(np.sum((m - cand) ** 2, axis=1))))
+            name = f"{'rev' if reverse else 'fwd'}+roll{roll}"
+            if best_rms is None or rms < best_rms:
+                best_rms, best_name = rms, name
+            if not reverse and roll == 0:
+                id_rms = rms
+    return id_rms, best_rms, best_name
+
+
+def test_against_real_capture():
+    if not (os.path.exists(CAPTURE) and os.path.exists(SEED)):
+        print(f"[SKIP] B) Capture/Seed fehlt: {CAP_DIR}")
+        return
+    fk = RobotFK.from_file(ROBOT_JSON)
+    seed = json.load(open(SEED, "r", encoding="utf-8")).get("accumulated_state", {})
+    transforms = fk.compute(seed)          # Roboter-Marker brauchen den Pose-Zustand
+    model = fk.all_markers_world(transforms)
+
+    # Root-Link (Board) bestimmen → davon wird NICHT über Ecken validiert.
+    roots = {ln for ln, ld in fk.links.items()
+             if not ld.get("parent") or ld.get("parent") not in fk.links}
+
+    data = json.load(open(CAPTURE, "r", encoding="utf-8"))
+    checked = 0
+    for m in data.get("markers", []):
+        mid = m["marker_id"]
+        if mid not in model or not m.get("corners_m"):
+            continue
+        if model[mid]["link"] in roots:                 # Board/Rail: bewusst Center-only
+            continue
+        obs_c = np.array(m["corners_m"], dtype=float) * 1000.0   # m → mm
+        model_c = np.asarray(model[mid]["corners_world"], float)
+        # Schlecht lokalisierte/fehlassoziierte Marker (Center weit daneben am
+        # Seed) überspringen — sie sagen nichts über die Eckreihenfolge aus.
+        if np.linalg.norm(model[mid]["world_mm"] - obs_c.mean(axis=0)) > 50.0:
+            continue
+        id_rms, best_rms, best_name = best_pairing_error(model_c, obs_c)
+        # 1) Form/Orientierung stimmt: Identitäts-RMS klein (Seed- + Triangulationsrauschen)
+        assert id_rms < 6.0, f"Roboter-Marker {mid}: Identitäts-RMS {id_rms:.2f}mm zu groß"
+        # 2) KEINE Umordnung ist besser als die Identität → Ecke-0 + Winding korrekt
+        assert best_name == "fwd+roll0", (
+            f"Roboter-Marker {mid}: '{best_name}' (RMS {best_rms:.2f}) schlägt Identität "
+            f"(RMS {id_rms:.2f}) → Eckreihenfolge falsch")
+        checked += 1
+    assert checked >= 4, f"zu wenige verwertbare Roboter-Marker geprüft ({checked})"
+    print(f"[PASS] B) Reale Capture-Daten: {checked} ROBOTER-Marker am Seed — "
+          "FK-Ecken passen zu corners_m, Identitäts-Paarung ist optimal.")
+
+
+def main():
+    test_self_consistency()
+    test_against_real_capture()
+    print("\n[OK] Alle Tests bestanden.")
+
+
+if __name__ == "__main__":
+    main()