merge vom thinkcentre > reconstruct

benchmark/camera_count/README.md

@@ -0,0 +1,148 @@
+# Kamera-Anzahl-Studie
+
+Untersucht, wie sich die Pose-Schätzgenauigkeit verändert, wenn weniger Kameras
+verwendet werden. Hauptergebnis ist eine Kurve **k Kameras → Positionsfehler in mm**
+an zwei Punkten des Roboters (Handgelenk und Finger).
+
+Hintergrundfrage und Entscheidungslogik: [`doc/camera_number_roadmap.md`](../../doc/camera_number_roadmap.md)
+
+---
+
+## Voraussetzungen
+
+Pro Szene müssen vorhanden sein:
+
+| Datei | Woher |
+|---|---|
+| `data/simulation/SceneX/render_*.png` | Blender-Renderer |
+| `data/simulation/SceneX/render_*.npz` | Kamera-Intrinsik |
+| `data/simulation/SceneX/pose.json` | Ground-Truth-Pose |
+| `data/robot/robot.json` | Robotermodell |
+
+Neue Szenen werden **automatisch erkannt** — kein Skript anpassen nötig.
+Für die Plots zusätzlich `matplotlib` (`pip install matplotlib`); ohne läuft
+alles, nur das PNG entfällt.
+
+---
+
+## Metriken
+
+Pro Kamera-Subset werden zwei Positionsfehler per Vorwärtskinematik berechnet —
+der euklidische Abstand zwischen geschätzter und wahrer Position eines Punktes:
+
+| Metrik | Punkt | Hängt ab von | Bedeutung |
+|---|---|---|---|
+| `wrist_error_mm` | Hand-Ursprung | Armgelenke `x,y,z,a` | Wie genau ist der Arm bis zum Handgelenk? |
+| `finger_error_mm` | FingerA-Skelettspitze | volle Kette `x..e` | Wie genau ist die ganze Pose inkl. Hand/Finger? |
+
+**Unbeobachtbare Gelenke → leerer Wert (n/a).** Sieht ein Subset z. B. die Hand
+nicht, ist `b/c/e` unbeobachtbar — dann ist die *wahre* Fingerposition schlicht
+unbekannt und `finger_error_mm` bleibt **leer** (statt mit einer falschen 0
+gefüllt zu werden). Das Handgelenk hängt nur von den Armgelenken ab und ist
+deshalb meist auch mit wenigen Kameras bestimmbar.
+
+Welche Gelenke einen Punkt bewegen, wird **numerisch** ermittelt (kleine
+Auslenkung je Gelenk) — funktioniert daher auch, wenn sich das Robotermodell ändert.
+
+Zusätzliche Spalten:
+- `n_unobservable` — Anzahl der 7 Gelenke, die in diesem Subset unbeobachtbar waren.
+- `mean_abs_deg` / `max_abs_deg` — Gelenkwinkelfehler (nur **beobachtbare** Gelenke).
+- `mean_abs_mm` / `max_abs_mm` — Lineargelenkfehler (`x`, `e`).
+
+---
+
+## Schritt 1 — Studie laufen lassen
+
+```bat
+run\run_camera_study.bat
+```
+
+Das wertet alle verfügbaren Szenen aus, k = 3 bis (alle verfügbaren Kameras),
+10 zufällige Subsets pro k.
+
+**Optionen:**
+
+```bat
+REM Nur bestimmte Szenen
+run\run_camera_study.bat --scenes Scene7 Scene9
+
+REM Kamerabereich einschränken
+run\run_camera_study.bat --k-min 3 --k-max 5
+
+REM Weniger Samples — schneller, weniger repräsentativ
+run\run_camera_study.bat --samples 3
+
+REM Pipeline für vorhandene Subsets erneut laufen lassen
+run\run_camera_study.bat --force
+
+REM VOLLSTÄNDIGER Neustart: alles löschen und neu rechnen
+run\run_camera_study.bat --clean
+
+REM Nur eine re-gerenderte Szene komplett neu rechnen (Rest bleibt erhalten)
+run\run_camera_study.bat --clean --scenes Scene10
+```
+
+**Drei Stufen der Wiederholung:**
+
+| Flag | Wirkung |
+|---|---|
+| *(kein Flag)* | Vorhandene `robot_state.json` werden wiederverwendet, nur Auswertung neu. Schnell. |
+| `--force` | Pipeline läuft erneut. Für geänderte Pipeline-Logik. |
+| `--clean` | Löscht Zwischenergebnisse vorher. **Nötig nach Re-Rendering.** |
+
+**Merge:** Ein Lauf über einzelne Szenen aktualisiert nur deren Zeilen in
+`camera_study.json/.csv` — die übrigen Szenen bleiben erhalten.
+
+---
+
+## Schritt 2 — Auswertung
+
+```bash
+python benchmark/camera_count/analyze.py                    # finger_error_mm (Standard)
+python benchmark/camera_count/analyze.py --metric wrist_error_mm
+python benchmark/camera_count/analyze.py --metric mean_abs_deg
+```
+
+Beispielausgabe:
+
+```
+Kamera-Anzahl vs. finger_error_mm
+   k |     n |   n/a |   Mittel |   Median |      Std |      Min |      Max
+--------------------------------------------------------------------------
+   3 |     4 |     6 |    1.775 |    1.583 |    0.986 |    0.710 |    3.225
+   4 |    10 |     0 |    1.955 |    2.034 |    0.696 |    0.503 |    3.317
+   ...
+   7 |     1 |     0 |    1.251 |    1.251 |    0.000 |    1.251 |    1.251
+
+  n/a = Subsets, bei denen dieser Punkt unbeobachtbar war (nicht eingerechnet).
+```
+
+Bei k=3: **6 von 10 Subsets sehen den Finger gar nicht** (n/a=6). Das ist die
+eigentliche Aussage — nicht „der Fehler ist klein", sondern „die meisten
+3-Kamera-Anordnungen verlieren die Hand komplett".
+
+Verfügbare Metriken: `finger_error_mm` (Standard), `wrist_error_mm`,
+`mean_abs_deg`, `max_abs_deg`, `mean_abs_mm`, `max_abs_mm`.
+
+---
+
+## Ausgabedateien
+
+| Pfad | Inhalt |
+|---|---|
+| `benchmark/camera_count/results/camera_study.json` | Alle Einzelergebnisse |
+| `benchmark/camera_count/results/camera_study.csv` | Dasselbe als CSV (inkl. `wrist_error_mm`, `finger_error_mm`, `n_unobservable`) |
+| `benchmark/camera_count/results/camera_count_<metric>.png` | Boxplot je Metrik |
+| `data/camera_study/SceneX/k3_abc/` | Pipeline-Zwischenergebnisse pro Subset |
+
+Die Zwischenergebnisse in `data/camera_study/` sind groß — nicht committen.
+
+---
+
+## Ergebnis interpretieren
+
+1. **Wird die Hand überhaupt gesehen?** → `n/a`-Spalte bei `finger_error_mm`.
+2. **Ab welchem k wird der Fehler stabil?** → Knick in der Kurve.
+3. **Welche Kombination ist bei kleinem k am besten?** → beste/schlechteste Subsets.
+4. **Arm vs. Hand getrennt:** `wrist_error_mm` ist meist schon mit 3 Kameras gut;
+   der kritische Teil ist die Hand/Finger-Orientierung.

benchmark/camera_count/analyze.py

@@ -0,0 +1,152 @@
+#!/usr/bin/env python3
+"""
+benchmark/camera_count/analyze.py
+===================================
+Liest camera_study.json und erstellt:
+  - Konsolentabelle: k → Mittelwert / Median / Std / Min / Max des Fehlers
+    (inkl. n/a-Spalte: Subsets, bei denen der Punkt unbeobachtbar war)
+  - Beste und schlechteste Kamera-Subsets je k
+  - Boxplot: Anzahl Kameras vs. Fehler (PNG)
+
+Metriken:
+  finger_error_mm (Standard) — Fingerposition in mm (volle Kette x..e).
+                                Leer, wenn Hand/Palm/Finger unbeobachtbar.
+  wrist_error_mm             — Handgelenkposition in mm (nur Armgelenke x,y,z,a).
+  mean_abs_deg / max_abs_deg — Gelenkwinkelfehler in Grad (nur beobachtbare Gelenke).
+  mean_abs_mm / max_abs_mm   — Lineargelenkfehler (x, e) in mm.
+
+Aufruf:
+  python benchmark/camera_count/analyze.py
+  python benchmark/camera_count/analyze.py --metric wrist_error_mm
+  python benchmark/camera_count/analyze.py --metric mean_abs_deg
+"""
+from __future__ import annotations
+
+import argparse
+import json
+import statistics
+from pathlib import Path
+
+RESULTS_DIR = Path(__file__).resolve().parent / "results"
+
+
+def load(path: str) -> list[dict]:
+    data = json.loads(Path(path).read_text(encoding="utf-8"))
+    if not data:
+        print("[ERROR] Keine Ergebnisse in der Datei.")
+    return data
+
+
+def group_by_k(data: list[dict], metric: str) -> tuple[dict[int, list[float]], dict[int, int]]:
+    """Werte je k (None übersprungen) plus Anzahl der n/a-Fälle je k."""
+    by_k: dict[int, list[float]] = {}
+    na_by_k: dict[int, int] = {}
+    for row in data:
+        k = row["k"]
+        v = row.get(metric)
+        if v is None:
+            na_by_k[k] = na_by_k.get(k, 0) + 1
+            continue
+        by_k.setdefault(k, []).append(v)
+    return by_k, na_by_k
+
+
+def print_table(by_k: dict[int, list[float]], na_by_k: dict[int, int], metric: str) -> None:
+    print(f"\nKamera-Anzahl vs. {metric}")
+    print(f"{'k':>4} | {'n':>5} | {'n/a':>5} | {'Mittel':>8} | {'Median':>8} | "
+          f"{'Std':>8} | {'Min':>8} | {'Max':>8}")
+    print("-" * 74)
+    all_ks = sorted(set(by_k) | set(na_by_k))
+    for k in all_ks:
+        na = na_by_k.get(k, 0)
+        vals = by_k.get(k, [])
+        if not vals:
+            print(f"{k:>4} | {0:>5} | {na:>5} | {'—':>8} | {'—':>8} | "
+                  f"{'—':>8} | {'—':>8} | {'—':>8}")
+            continue
+        print(f"{k:>4} | {len(vals):>5} | {na:>5} | "
+              f"{statistics.mean(vals):8.3f} | {statistics.median(vals):8.3f} | "
+              f"{statistics.pstdev(vals):8.3f} | {min(vals):8.3f} | {max(vals):8.3f}")
+    if any(na_by_k.values()):
+        print("\n  n/a = Subsets, bei denen dieser Punkt unbeobachtbar war "
+              "(Position unbekannt, nicht eingerechnet).")
+
+
+def print_best_worst(data: list[dict], metric: str) -> None:
+    ks = sorted({r["k"] for r in data})
+    print(f"\nBeste / schlechteste Subsets je k  ({metric}):")
+    for k in ks:
+        rows_k = [r for r in data if r["k"] == k and r.get(metric) is not None]
+        if not rows_k:
+            print(f"  k={k}:  keine beobachtbaren Werte")
+            continue
+        best = min(rows_k, key=lambda r: r[metric])
+        worst = max(rows_k, key=lambda r: r[metric])
+        print(f"  k={k}:  best  [{best['scene']}] {best['subset']}  "
+              f"({best[metric]:.3f})   "
+              f"worst [{worst['scene']}] {worst['subset']}  ({worst[metric]:.3f})")
+
+
+def plot(by_k: dict[int, list[float]], na_by_k: dict[int, int],
+         metric: str, out_path: str) -> None:
+    try:
+        import matplotlib.pyplot as plt
+    except ImportError:
+        print("\n[INFO] matplotlib nicht installiert — Plot übersprungen.")
+        print("       pip install matplotlib")
+        return
+
+    ks = sorted(by_k)
+    if not ks:
+        print("\n[INFO] Keine Werte zum Plotten (alle unbeobachtbar).")
+        return
+    data_plot = [by_k[k] for k in ks]
+
+    fig, ax = plt.subplots(figsize=(8, 5))
+    ax.boxplot(data_plot, labels=[str(k) for k in ks], patch_artist=True)
+    ax.set_xlabel("Anzahl Kameras")
+    ax.set_ylabel(metric)
+    ax.set_title("Anzahl Kameras vs. Pose-Schätzfehler")
+    ax.grid(True, axis="y", alpha=0.35)
+
+    ymax = max(max(v) for v in data_plot)
+    for i, k in enumerate(ks, start=1):
+        na = na_by_k.get(k, 0)
+        note = f"n={len(by_k[k])}" + (f"\nn/a={na}" if na else "")
+        ax.text(i, ymax * 1.02, note, ha="center", va="bottom", fontsize=8, color="gray")
+
+    fig.tight_layout()
+    fig.savefig(out_path, dpi=150)
+    print(f"\n[INFO] Plot gespeichert: {out_path}")
+
+
+def main() -> None:
+    ap = argparse.ArgumentParser(description="Auswertung der Kamera-Anzahl-Studie")
+    ap.add_argument("--input", default=str(RESULTS_DIR / "camera_study.json"))
+    ap.add_argument("--metric",
+                    choices=["finger_error_mm", "wrist_error_mm",
+                             "mean_abs_deg", "max_abs_deg", "mean_abs_mm", "max_abs_mm"],
+                    default="finger_error_mm",
+                    help="Metrik für Tabelle und Plot (Standard: finger_error_mm)")
+    ap.add_argument("--out-plot", default=None,
+                    help="Plot-Pfad (Standard: results/camera_count_<metric>.png)")
+    args = ap.parse_args()
+
+    data = load(args.input)
+    if not data:
+        return
+
+    by_k, na_by_k = group_by_k(data, args.metric)
+    if not by_k and not na_by_k:
+        print(f"[ERROR] Keine Werte für Metrik '{args.metric}' gefunden.")
+        return
+
+    out_plot = args.out_plot or str(RESULTS_DIR / f"camera_count_{args.metric}.png")
+
+    print_table(by_k, na_by_k, args.metric)
+    print_best_worst(data, args.metric)
+    plot(by_k, na_by_k, args.metric, out_plot)
+
+
+if __name__ == "__main__":
+    main()

benchmark/camera_count/run_camera_study.py

@@ -0,0 +1,243 @@
+#!/usr/bin/env python3
+"""
+benchmark/camera_count/run_camera_study.py
+==========================================
+Untersucht, wie sich die Pose-Genauigkeit (Gelenkwinkelfehler, Handgelenk- und
+Finger-Positionsfehler in mm) mit der Kameraanzahl verändert.
+
+Für jede Szene mit bekannter Ground-Truth (pose.json) und jede Kameraanzahl k
+wird eine zufällige Stichprobe von k-Kamera-Subsets durch die volle Pipeline
+geschickt und ausgewertet. Ergebnisse landen in benchmark/camera_count/results/.
+
+Erweiterbar: funktioniert mit beliebig vielen Szenen und Kameras — neue Szenen
+werden automatisch erkannt, sobald sie pose.json haben.
+
+Aufruf:
+  python benchmark/camera_count/run_camera_study.py
+  python benchmark/camera_count/run_camera_study.py --scenes Scene7 Scene9
+  python benchmark/camera_count/run_camera_study.py --k-min 3 --k-max 5 --samples 15
+  python benchmark/camera_count/run_camera_study.py --force   # Pipeline neu rechnen
+  python benchmark/camera_count/run_camera_study.py --clean   # alles löschen + neu
+  python benchmark/camera_count/run_camera_study.py --clean --scenes Scene10  # nur diese Szene
+"""
+from __future__ import annotations
+
+import argparse
+import glob
+import itertools
+import json
+import random
+import re
+import shutil
+import subprocess
+import sys
+from pathlib import Path
+from statistics import mean
+
+ROOT = Path(__file__).resolve().parent.parent.parent
+RESULTS_DIR = Path(__file__).resolve().parent / "results"
+PY = sys.executable
+
+
+def discover_scenes(sim_dir: Path) -> list[str]:
+    """Alle Szenen mit render_*.png und pose.json."""
+    scenes = []
+    for d in sorted(sim_dir.iterdir()):
+        if not d.name.startswith("Scene"):
+            continue
+        if not (d / "pose.json").exists():
+            continue
+        if not list(d.glob("render_*.png")):
+            continue
+        scenes.append(d.name)
+    return scenes
+
+
+def camera_ids(scene_dir: Path) -> list[str]:
+    """Sortierte Kamera-IDs (a, b, c ...) einer Szene."""
+    ids = []
+    for f in sorted(scene_dir.glob("render_*.png")):
+        m = re.match(r"render_([A-Za-z0-9]+)\.png", f.name)
+        if m:
+            ids.append(m.group(1))
+    return ids
+
+
+def run_pipeline(scene_dir: Path, cams: list[str], eval_dir: Path, robot: str) -> bool:
+    cmd = [
+        PY, str(ROOT / "pipeline" / "run_pipeline.py"),
+        str(scene_dir),
+        "--robot", robot,
+        "--evalDir", str(eval_dir),
+        "--cameras", ",".join(cams),
+    ]
+    r = subprocess.run(cmd, cwd=str(ROOT), capture_output=True, text=True)
+    if r.returncode != 0:
+        snippet = (r.stderr or r.stdout).strip()[-300:]
+        print(f"      [WARN] Pipeline fehlgeschlagen: {snippet}")
+        return False
+    return True
+
+
+def eval_pose(robot_state: Path, gt: Path, robot: str) -> dict | None:
+    out = robot_state.with_suffix(".eval.json")
+    cmd = [
+        PY, str(ROOT / "benchmark" / "eval_pose.py"),
+        str(robot_state), str(gt),
+        "--out", str(out),
+        "--robot", robot,
+        "--tolDeg", "999", "--tolMm", "999",
+    ]
+    subprocess.run(cmd, cwd=str(ROOT), capture_output=True, text=True)
+    return json.loads(out.read_text(encoding="utf-8")) if out.exists() else None
+
+
+def main() -> None:
+    ap = argparse.ArgumentParser(description="Kamera-Anzahl vs. Pose-Genauigkeit")
+    ap.add_argument("--scenes", nargs="*", default=None,
+                    help="Szenen-Namen oder Nummern (Standard: alle mit pose.json)")
+    ap.add_argument("--k-min", type=int, default=3, help="Minimale Kameraanzahl (Standard: 3)")
+    ap.add_argument("--k-max", type=int, default=None,
+                    help="Maximale Kameraanzahl (Standard: alle verfügbaren)")
+    ap.add_argument("--samples", type=int, default=10,
+                    help="Zufällige Subsets pro (Szene, k) — Standard: 10")
+    ap.add_argument("--seed", type=int, default=42)
+    ap.add_argument("--robot", default=str(ROOT / "data" / "robot" / "robot.json"))
+    ap.add_argument("--force", action="store_true",
+                    help="Vorhandene robot_state.json neu rechnen (Pipeline erneut laufen)")
+    ap.add_argument("--clean", action="store_true",
+                    help="Zwischenergebnisse vorher löschen und komplett neu rechnen. "
+                         "Mit --scenes nur diese Szenen, ohne --scenes alles.")
+    ap.add_argument("--out", default=str(RESULTS_DIR / "camera_study.json"))
+    ap.add_argument("--csv", default=str(RESULTS_DIR / "camera_study.csv"))
+    args = ap.parse_args()
+
+    random.seed(args.seed)
+    RESULTS_DIR.mkdir(parents=True, exist_ok=True)
+
+    sim_dir = ROOT / "data" / "simulation"
+    study_root = ROOT / "data" / "camera_study"
+    scenes = discover_scenes(sim_dir)
+    if args.scenes:
+        want = {s if s.startswith("Scene") else f"Scene{s}" for s in args.scenes}
+        scenes = [s for s in scenes if s in want]
+    if not scenes:
+        print("[ERROR] Keine Szenen mit pose.json und render_*.png gefunden.")
+        sys.exit(1)
+
+    # --clean: Zwischenergebnisse entfernen.
+    if args.clean:
+        if args.scenes:
+            for s in scenes:
+                d = study_root / s
+                if d.exists():
+                    shutil.rmtree(d)
+                    print(f"[CLEAN] entfernt {d}")
+        else:
+            if study_root.exists():
+                shutil.rmtree(study_root)
+                print(f"[CLEAN] entfernt {study_root}")
+            for f in (Path(args.out), Path(args.csv)):
+                if f.exists():
+                    f.unlink()
+                    print(f"[CLEAN] entfernt {f}")
+        args.force = True
+
+    print(f"[INFO] Szenen: {scenes}")
+    print(f"[INFO] Seed={args.seed}  Samples/k={args.samples}\n")
+
+    all_results: list[dict] = []
+
+    for scene in scenes:
+        scene_dir = sim_dir / scene
+        cams = camera_ids(scene_dir)
+        k_max = min(args.k_max or len(cams), len(cams))
+        k_range = range(args.k_min, k_max + 1)
+        gt = scene_dir / "pose.json"
+
+        print(f"[SZENE] {scene}  Kameras={cams}  k={list(k_range)}")
+
+        for k in k_range:
+            all_combos = list(itertools.combinations(cams, k))
+            n_sample = min(args.samples, len(all_combos))
+            sample = random.sample(all_combos, n_sample)
+
+            errs: list[float] = []
+            print(f"  k={k}: {n_sample}/{len(all_combos)} Subsets")
+
+            for combo in sample:
+                label = "".join(combo)
+                eval_dir = ROOT / "data" / "camera_study" / scene / f"k{k}_{label}"
+                rs = eval_dir / "robot_state.json"
+
+                if rs.exists() and not args.force:
+                    print(f"    {label}: übersprungen (robot_state.json existiert)")
+                else:
+                    eval_dir.mkdir(parents=True, exist_ok=True)
+                    ok = run_pipeline(scene_dir, list(combo), eval_dir, args.robot)
+                    if not ok or not rs.exists():
+                        print(f"    {label}: FEHLER — übersprungen")
+                        continue
+
+                ev = eval_pose(rs, gt, args.robot)
+                if not ev:
+                    print(f"    {label}: Auswertung fehlgeschlagen")
+                    continue
+
+                s = ev["summary"]
+                row = {
+                    "scene": scene,
+                    "k": k,
+                    "subset": label,
+                    "mean_abs_deg": s.get("mean_abs_deg"),
+                    "max_abs_deg": s.get("max_abs_deg"),
+                    "mean_abs_mm": s.get("mean_abs_mm"),
+                    "max_abs_mm": s.get("max_abs_mm"),
+                    "wrist_error_mm": s.get("wrist_error_mm"),
+                    "finger_error_mm": s.get("finger_error_mm"),
+                    "n_unobservable": s.get("n_unobservable"),
+                }
+                all_results.append(row)
+                errs.append(row["mean_abs_deg"] or 0.0)
+                we, fe = row["wrist_error_mm"], row["finger_error_mm"]
+                we_str = f"{we:.2f}mm" if we is not None else "n/a"
+                fe_str = f"{fe:.2f}mm" if fe is not None else "n/a"
+                print(f"    {label}: mean={row['mean_abs_deg']:.3f}°  "
+                      f"wrist={we_str}  finger={fe_str}  unobs={row['n_unobservable']}")
+
+            if errs:
+                print(f"  k={k} Zusammenfassung: mean={mean(errs):.3f}°  "
+                      f"min={min(errs):.3f}°  max={max(errs):.3f}°")
+
+    # Ergebnisse mergen — andere Szenen erhalten
+    out_path = Path(args.out)
+    processed = set(scenes)
+    merged: list[dict] = []
+    if out_path.exists():
+        try:
+            existing = json.loads(out_path.read_text(encoding="utf-8"))
+            merged = [r for r in existing if r.get("scene") not in processed]
+        except (json.JSONDecodeError, OSError):
+            merged = []
+    merged.extend(all_results)
+    merged.sort(key=lambda r: (r["scene"], r["k"], r["subset"]))
+    out_path.write_text(json.dumps(merged, indent=2), encoding="utf-8")
+
+    with open(args.csv, "w", encoding="utf-8") as f:
+        f.write("scene,k,subset,mean_abs_deg,max_abs_deg,mean_abs_mm,max_abs_mm,"
+                "wrist_error_mm,finger_error_mm,n_unobservable\n")
+        for r in merged:
+            f.write(f"{r['scene']},{r['k']},{r['subset']},"
+                    f"{r['mean_abs_deg'] or ''},"
+                    f"{r['max_abs_deg'] or ''},"
+                    f"{r['mean_abs_mm'] or ''},"
+                    f"{r['max_abs_mm'] or ''},"
+                    f"{r['wrist_error_mm'] if r['wrist_error_mm'] is not None else ''},"
+                    f"{r['finger_error_mm'] if r['finger_error_mm'] is not None else ''},"
+                    f"{r['n_unobservable'] if r['n_unobservable'] is not None else ''}\n")
+
+    print(f"\n[DONE] {len(all_results)} neu, {len(merged)} gesamt -> {args.out}")
+
+
+if __name__ == "__main__":
+    main()

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).
 """
@@ -17,11 +26,25 @@ from __future__ import annotations
 import argparse
 import json
 import sys
-from typing import Any, Dict
+from pathlib import Path
+from typing import Any, Dict, Optional
+
+import numpy as np
+
+sys.path.insert(0, str(Path(__file__).parent.parent / "pipeline"))
+from robot_fk import RobotFK  # noqa: E402
 
 LINEAR = {"x", "e"}
 JOINTS = ["x", "y", "z", "a", "b", "c", "e"]
 
+# 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]]:
     d = json.load(open(path, "r", encoding="utf-8"))
@@ -51,7 +74,49 @@ def joint_error(v: str, est: float, gt: float) -> float:
     return abs(((est - gt + 180.0) % 360.0) - 180.0)
 
 
-def evaluate(estimate_path: str, gt_path: str) -> Dict[str, Any]:
+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.
+    """
+    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
+    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))
+
+
+def evaluate(estimate_path: str, gt_path: str,
+             robot_path: Optional[str] = None) -> Dict[str, Any]:
     est = load_estimate(estimate_path)
     gt = load_gt(gt_path)
 
@@ -74,7 +139,19 @@ def evaluate(estimate_path: str, gt_path: str) -> Dict[str, Any]:
         "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,
+        "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}
 
 
@@ -83,14 +160,15 @@ def main() -> int:
     ap.add_argument("estimate", help="robot_state.json")
     ap.add_argument("gt", help="simulation/SceneX/pose.json")
     ap.add_argument("--out", default=None)
+    ap.add_argument("--robot", default=None,
+                    help="robot.json — aktiviert FK-basierten Fingerfehler in mm")
     ap.add_argument("--tolDeg", type=float, default=2.0)
     ap.add_argument("--tolMm", type=float, default=3.0)
     args = ap.parse_args()
 
-    res = evaluate(args.estimate, args.gt)
+    res = evaluate(args.estimate, args.gt, robot_path=args.robot)
     print(f"{'joint':>6} | {'est':>9} | {'gt':>9} | {'error':>9} | obs | nMk")
     print("-" * 58)
-    worst = 0.0
     for r in res["rows"]:
         flag = " " if r["observable"] else "U"
         print(f"{r['joint']:>6} | {r['estimate']:9.2f} | {r['gt']:9.2f} | "
@@ -102,6 +180,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")
+    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)