#!/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()