Multipoint Schritt 3

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()