Claude: Abweichungen bei 3 Kameras zählen

benchmark/eval_pose.py

@@ -9,6 +9,15 @@ Per-joint error:
   revolute (y,z,a,b,c): angular error in degrees, wrap-aware (179 vs -179 = 2deg)
   linear   (x,e):       error in millimetres
 
+With --robot, additionally computes FK-based position errors (mm) at the points
+in TRACK_POINTS — the euclidean distance between estimated and true world
+position of a point on the robot:
+  wrist_error_mm  : Hand origin   (depends only on arm joints x,y,z,a)
+  finger_error_mm : FingerA tip   (depends on the full chain x..e)
+A point whose chain contains an UNOBSERVABLE joint yields None (the true
+position is simply unknown) rather than a misleading value. n_unobservable
+counts how many of the 7 joints were unobservable.
+
 Prints a table and optionally writes a JSON summary. Returns nonzero if any
 observable joint exceeds a tolerance (for scripted regression checks).
 """
@@ -27,7 +36,14 @@ from robot_fk import RobotFK  # noqa: E402
 
 LINEAR = {"x", "e"}
 JOINTS = ["x", "y", "z", "a", "b", "c", "e"]
-FINGER_LINK = "FingerA"
+
+# Messpunkte entlang der Kette: name -> (link, lokaler Offset in mm).
+# wrist  = Hand-Ursprung (hängt nur von den Armgelenken x,y,z,a ab)
+# finger = FingerA-Skelettspitze (hängt von der ganzen Kette ab)
+TRACK_POINTS = {
+    "wrist":  ("Hand",    [0.0, 0.0, 0.0]),
+    "finger": ("FingerA", [0.0, -60.0, 0.0]),
+}
 
 
 def load_estimate(path: str) -> Dict[str, Dict[str, Any]]:
@@ -58,18 +74,45 @@ def joint_error(v: str, est: float, gt: float) -> float:
     return abs(((est - gt + 180.0) % 360.0) - 180.0)
 
 
-def finger_error_mm(est: Dict[str, Any], gt: Dict[str, float],
-                    robot_path: Optional[str]) -> Optional[float]:
-    """Euklidischer Abstand der FingerA-Position (FK) zwischen Schätzung und GT."""
-    if not robot_path:
+def _point_world(fk: RobotFK, vals: Dict[str, float],
+                 link: str, local: list) -> np.ndarray:
+    """Weltposition eines lokalen Punktes auf einem Link (mm)."""
+    T = fk.compute(vals)
+    h = np.array([local[0], local[1], local[2], 1.0])
+    return (T[link] @ h)[:3]
+
+
+def _dependent_joints(fk: RobotFK, gt_vals: Dict[str, float],
+                      link: str, local: list,
+                      eps: float = 5.0, thresh: float = 1e-6) -> set:
+    """Gelenke, die diesen Punkt bewegen — numerisch per Perturbation am GT-Zustand.
+
+    Robust für beliebige Robotermodelle: ein Gelenk zählt als abhängig, wenn
+    eine kleine Auslenkung den Punkt messbar verschiebt. Die eigene Drehung des
+    Handgelenks z.B. bewegt den Hand-Ursprung nicht und wird korrekt ausgelassen.
+    """
+    base = _point_world(fk, gt_vals, link, local)
+    deps = set()
+    for j in JOINTS:
+        v = dict(gt_vals)
+        v[j] = v[j] + eps
+        if float(np.linalg.norm(_point_world(fk, v, link, local) - base)) > thresh:
+            deps.add(j)
+    return deps
+
+
+def point_error_mm(fk: RobotFK, est_vals: Dict[str, float], gt_vals: Dict[str, float],
+                   observable: Dict[str, bool], link: str, local: list) -> Optional[float]:
+    """Euklidischer Positionsfehler eines Punktes (mm).
+
+    Gibt None zurück, wenn irgendein Gelenk, von dem der Punkt abhängt,
+    unbeobachtbar ist — dann ist die wahre Position schlicht unbekannt.
+    """
+    deps = _dependent_joints(fk, gt_vals, link, local)
+    if any(not observable.get(j, False) for j in deps):
         return None
-    fk = RobotFK.from_file(robot_path)
-    est_vals = {v: est[v]["value"] for v in JOINTS}
-    gt_vals  = {v: gt.get(v, 0.0) for v in JOINTS}
-    t_est = fk.compute(est_vals)
-    t_gt  = fk.compute(gt_vals)
-    p_est = t_est[FINGER_LINK][:3, 3]
-    p_gt  = t_gt[FINGER_LINK][:3, 3]
+    p_est = _point_world(fk, est_vals, link, local)
+    p_gt  = _point_world(fk, gt_vals, link, local)
     return float(np.linalg.norm(p_est - p_gt))
 
 
@@ -97,8 +140,19 @@ def evaluate(estimate_path: str, gt_path: str,
         "max_abs_deg": max(ang_errs) if ang_errs else None,
         "mean_abs_mm": (sum(lin_errs) / len(lin_errs)) if lin_errs else None,
         "max_abs_mm": max(lin_errs) if lin_errs else None,
-        "finger_error_mm": finger_error_mm(est, gt, robot_path),
+        "n_unobservable": sum(1 for v in JOINTS if not est.get(v, {}).get("observable", False)),
     }
+
+    # FK-basierte Positionsfehler (mm) je Messpunkt — nur wenn robot.json gegeben
+    if robot_path:
+        fk = RobotFK.from_file(robot_path)
+        est_vals = {v: est[v]["value"] for v in JOINTS}
+        gt_vals  = {v: gt.get(v, 0.0) for v in JOINTS}
+        observable = {v: bool(est.get(v, {}).get("observable", False)) for v in JOINTS}
+        for name, (link, local) in TRACK_POINTS.items():
+            summary[f"{name}_error_mm"] = point_error_mm(
+                fk, est_vals, gt_vals, observable, link, local)
+
     return {"rows": rows, "summary": summary}
 
 
@@ -128,9 +182,11 @@ def main() -> int:
     mm = f"{s['mean_abs_mm']:.2f}" if s["mean_abs_mm"] is not None else "-"
     xm = f"{s['max_abs_mm']:.2f}" if s["max_abs_mm"] is not None else "-"
     print(f"angles: mean {md}deg / max {xd}deg   |   linear: mean {mm}mm / max {xm}mm")
-    fe = s.get("finger_error_mm")
-    if fe is not None:
-        print(f"finger ({FINGER_LINK}) position error: {fe:.2f} mm")
+    print(f"unobservable joints: {s.get('n_unobservable', 0)}")
+    for name in TRACK_POINTS:
+        pe = s.get(f"{name}_error_mm")
+        txt = f"{pe:.2f} mm" if pe is not None else "n/a (Gelenk unbeobachtbar)"
+        print(f"{name:>6} position error: {txt}")
 
     if args.out:
         json.dump(res, open(args.out, "w", encoding="utf-8"), indent=2)