diff --git a/doc/Homing_5_Pose_MultiPoint_Weighted.md b/doc/Homing_5_Pose_MultiPoint_Weighted.md index 1e38473..0a5973e 100644 --- a/doc/Homing_5_Pose_MultiPoint_Weighted.md +++ b/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 -> [`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. +> 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. +> +> **Status (2026-06-25):** **Qualitäts-Gewichtung** (Doc-Punkt 3 / Schritte 1+2) +> 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? | -|---|---|---|---| -| 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 | **Ja, konkret** — siehe Konsumenten-Recherche direkt unter der Tabelle. Mehrere Stellen brauchen einen Guard, **bevor** dieser Schritt scharf geschaltet wird | -| 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 | +> **Status-Legende:** ✅ erledigt · ⬜ **offen** +> +> **Punkt-Zuordnung** (Doc-Abschnitt ↔ Chat-Nummerierung): Doc-Punkt 1 „Vier +> Eckpunkte" = Schritte 3+4 · Doc-Punkt 2 „Einzelkamera" = Schritte 5+6 · +> Doc-Punkt 3 „Qualitäts-Gewichtung" = Schritte 1+2 (erledigt). + +| # | Status | Schritt | Testbar an | Bricht Bestehendes? | +|---|---|---|---|---| +| 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. diff --git a/scripts/5_pose_estimation.py b/scripts/5_pose_estimation.py index 822faf6..e6b9216 100644 --- a/scripts/5_pose_estimation.py +++ b/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: - marker_observation = "corner_pose" -> aruco_marker_poses.json (pos + measured normal) - marker_observation = "center_point" -> aruco_positions_*.json (pos only) +Observation input (robot.json -> pose_estimation.marker_observation): + "corner_pose" (default) -> aruco_marker_poses.json: 3 pos + 3 normal residuals/marker + "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 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). - Returns: marker_id -> {pos_mm:(3,), normal:(3,)|None, link:str, n_cams:int} + normal + num_cameras + 4 triangulated corners) or aruco_positions_*.json + (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), diff --git a/scripts/__pycache__/5_pose_estimation.cpython-311.pyc b/scripts/__pycache__/5_pose_estimation.cpython-311.pyc index eaf2566..34c9dbe 100644 Binary files a/scripts/__pycache__/5_pose_estimation.cpython-311.pyc and b/scripts/__pycache__/5_pose_estimation.cpython-311.pyc differ diff --git a/scripts/__pycache__/robot_fk.cpython-311.pyc b/scripts/__pycache__/robot_fk.cpython-311.pyc index 0830b4a..419ce82 100644 Binary files a/scripts/__pycache__/robot_fk.cpython-311.pyc and b/scripts/__pycache__/robot_fk.cpython-311.pyc differ diff --git a/scripts/robot_fk.py b/scripts/robot_fk.py index 2f9a376..4a3108a 100644 --- a/scripts/robot_fk.py +++ b/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 diff --git a/test/test_robot_fk_corners.py b/test/test_robot_fk_corners.py new file mode 100644 index 0000000..bec443a --- /dev/null +++ b/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()