170 lines
7.4 KiB
Python
170 lines
7.4 KiB
Python
#!/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()
|