From 6d4a61f4d5ae708482c55d4bf487af1c5c328de4 Mon Sep 17 00:00:00 2001 From: chk <79915315+ChKendel@users.noreply.github.com> Date: Wed, 3 Jun 2026 07:30:28 +0200 Subject: [PATCH] merge vom thinkcentre > reconstruct --- benchmark/camera_count/README.md | 148 +++++++++++++ benchmark/camera_count/analyze.py | 152 +++++++++++++ benchmark/camera_count/results/.gitkeep | 0 benchmark/camera_count/run_camera_study.py | 243 +++++++++++++++++++++ benchmark/eval_pose.py | 91 +++++++- run/run_analyze.bat | 12 + run/run_camera_study.bat | 18 ++ run/run_eval_pose.bat | 29 +++ run/run_eval_pose.sh | 28 +++ run/run_evaluateMarker.sh | 36 +++ 10 files changed, 753 insertions(+), 4 deletions(-) create mode 100644 benchmark/camera_count/README.md create mode 100644 benchmark/camera_count/analyze.py create mode 100644 benchmark/camera_count/results/.gitkeep create mode 100644 benchmark/camera_count/run_camera_study.py create mode 100644 run/run_analyze.bat create mode 100644 run/run_camera_study.bat create mode 100644 run/run_eval_pose.bat create mode 100644 run/run_eval_pose.sh create mode 100644 run/run_evaluateMarker.sh diff --git a/benchmark/camera_count/README.md b/benchmark/camera_count/README.md new file mode 100644 index 0000000..f3a4637 --- /dev/null +++ b/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_.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. diff --git a/benchmark/camera_count/analyze.py b/benchmark/camera_count/analyze.py new file mode 100644 index 0000000..f18bedf --- /dev/null +++ b/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_.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() diff --git a/benchmark/camera_count/results/.gitkeep b/benchmark/camera_count/results/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/benchmark/camera_count/run_camera_study.py b/benchmark/camera_count/run_camera_study.py new file mode 100644 index 0000000..6bbbe68 --- /dev/null +++ b/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() diff --git a/benchmark/eval_pose.py b/benchmark/eval_pose.py index 8cec1fd..4f691f5 100644 --- a/benchmark/eval_pose.py +++ b/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) diff --git a/run/run_analyze.bat b/run/run_analyze.bat new file mode 100644 index 0000000..df11ecc --- /dev/null +++ b/run/run_analyze.bat @@ -0,0 +1,12 @@ +@echo off +REM run_analyze.bat +REM Aggregiert die Kamera-Studie: Genauigkeit je Kameraanzahl (3, 4 ... k) +REM ueber alle berechneten Szenen und Subsets. +REM +REM Aufruf: +REM run_analyze.bat (Finger-Fehler in mm, Standard) +REM run_analyze.bat --metric wrist_error_mm +REM run_analyze.bat --metric mean_abs_deg + +cd /d "%~dp0.." +python benchmark\camera_count\analyze.py %* diff --git a/run/run_camera_study.bat b/run/run_camera_study.bat new file mode 100644 index 0000000..82c5e57 --- /dev/null +++ b/run/run_camera_study.bat @@ -0,0 +1,18 @@ +@echo off +REM run_camera_study.bat +REM Untersucht wie viele Kameras fuer die Pose-Schaetzung benoetigt werden. +REM +REM Einfacher Aufruf (alle Szenen mit pose.json, 10 Samples pro k): +REM run_camera_study.bat +REM +REM Mit Optionen: +REM run_camera_study.bat --scenes Scene7 Scene9 --k-min 3 --k-max 5 --samples 15 +REM run_camera_study.bat --force +REM run_camera_study.bat --clean +REM run_camera_study.bat --clean --scenes Scene10 +REM +REM Auswertung danach: +REM python benchmark\camera_count\analyze.py + +cd /d "%~dp0.." +python benchmark\camera_count\run_camera_study.py %* diff --git a/run/run_eval_pose.bat b/run/run_eval_pose.bat new file mode 100644 index 0000000..ec64f24 --- /dev/null +++ b/run/run_eval_pose.bat @@ -0,0 +1,29 @@ +@echo off +REM run_eval_pose.bat +REM Wertet eine geschaetzte Pose gegen die Ground Truth aus. +REM Gibt Gelenkwinkelfehler (deg/mm) sowie Handgelenk- und Finger-Abstand (mm) aus. +REM +REM Aufruf: +REM run_eval_pose.bat data\evaluations\Scene8 +REM run_eval_pose.bat data\camera_study\Scene10\k3_abc + +cd /d "%~dp0.." + +if "%1"=="" ( + echo. + echo [INFO] Aufruf fehlt! + echo Beispiel: run_eval_pose.bat data\evaluations\Scene8 + echo. + exit /b 1 +) + +set "EVAL_DIR=%~1" +if "%EVAL_DIR:~-1%"=="\" set "EVAL_DIR=%EVAL_DIR:~0,-1%" + +for %%I in ("%EVAL_DIR%") do set "SCENE_NAME=%%~nxI" + +set "ROBOT_STATE=%EVAL_DIR%\robot_state.json" +set "GT=data\simulation\%SCENE_NAME%\pose.json" +set "ROBOT=data\robot\robot.json" + +python benchmark\eval_pose.py "%ROBOT_STATE%" "%GT%" --robot "%ROBOT%" diff --git a/run/run_eval_pose.sh b/run/run_eval_pose.sh new file mode 100644 index 0000000..c017ec4 --- /dev/null +++ b/run/run_eval_pose.sh @@ -0,0 +1,28 @@ +#!/usr/bin/env bash +# run_eval_pose.sh +# Wertet eine geschätzte Pose gegen die Ground Truth aus. +# Gibt Gelenkwinkelfehler (deg/mm) sowie Handgelenk- und Finger-Abstand (mm) aus. +# +# Aufruf: +# ./run/run_eval_pose.sh data/evaluations/Scene8 +# ./run/run_eval_pose.sh data/camera_study/Scene10/k3_abc + +set -euo pipefail + +ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" + +if [[ $# -eq 0 ]]; then + echo + echo "[INFO] Aufruf fehlt!" + echo "Beispiel: ./run/run_eval_pose.sh data/evaluations/Scene8" + echo + exit 1 +fi + +EVAL_DIR="${1%/}" +SCENE_NAME="$(basename "$EVAL_DIR")" + +python3 "$ROOT/benchmark/eval_pose.py" \ + "$ROOT/$EVAL_DIR/robot_state.json" \ + "$ROOT/data/simulation/$SCENE_NAME/pose.json" \ + --robot "$ROOT/data/robot/robot.json" diff --git a/run/run_evaluateMarker.sh b/run/run_evaluateMarker.sh new file mode 100644 index 0000000..516cfa2 --- /dev/null +++ b/run/run_evaluateMarker.sh @@ -0,0 +1,36 @@ +#!/usr/bin/env bash +# run_evaluateMarker.sh +# Vergleicht triangulierte Marker-Positionen (initial und optimiert) mit der +# Blender-Grundwahrheit (render_a.json der Simulationsszene). +# +# Aufruf: +# ./run_evaluateMarker.sh ../data/evaluations/Scene8 + +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +ROOT="$(dirname "$SCRIPT_DIR")" + +if [[ $# -eq 0 ]]; then + echo + echo "[INFO] Aufruf fehlt!" + echo "Beispiel: ./run_evaluateMarker.sh ../data/evaluations/Scene8" + echo + exit 1 +fi + +EVAL_DIR="${1%/}" # trailing slash entfernen +SCENE_NAME="$(basename "$EVAL_DIR")" +ORIGINAL="$ROOT/data/simulation/$SCENE_NAME/render_a.json" + +echo "====================================" +echo "INITIAL:" +python3 "$ROOT/pipeline/9_evaluateMarker.py" \ + "$EVAL_DIR/aruco_positions_initial.json" \ + "$ORIGINAL" + +echo "====================================" +echo "OPTIMIZED:" +python3 "$ROOT/pipeline/9_evaluateMarker.py" \ + "$EVAL_DIR/aruco_positions_optimized.json" \ + "$ORIGINAL"