Claude: Findet .npz Fehler

pipeline/3_multiview_bundle_adjustment_v4.py

@@ -1091,9 +1091,11 @@ def optimize_with_constraints(
 # ===================================================================
 
 def print_constraint_summary(constraints: List[Constraint]) -> None:
-    num_dist = sum(isinstance(c, MarkerDistanceConstraint) for c in constraints)
+    num_dist  = sum(isinstance(c, MarkerDistanceConstraint) for c in constraints)
     num_joint = sum(isinstance(c, JointAxisConstraint) for c in constraints)
-    print(f"[INFO] Constraint summary: total={len(constraints)} distance={num_dist} joint/chain={num_joint}")
+    num_other = len(constraints) - num_dist - num_joint
+    extra = f" other={num_other}" if num_other else ""
+    print(f"[INFO] Constraint summary: total={len(constraints)} distance={num_dist} joint/chain={num_joint}{extra}")
 
 
 def print_constraint_list(constraints: List[Constraint]) -> None:
@@ -1108,7 +1110,7 @@ def print_constraint_list(constraints: List[Constraint]) -> None:
                 f"Weight={constraint.weight} | "
                 f"Source={constraint.source}"
             )
-        else:
+        elif isinstance(constraint, JointAxisConstraint):
             axis_str = np.array2string(constraint.joint_axis, precision=3, suppress_small=True)
             print(
                 f"  [{idx:03d}] JOINT_AXIS | "
@@ -1119,6 +1121,12 @@ def print_constraint_list(constraints: List[Constraint]) -> None:
                 f"Weight={constraint.weight} | "
                 f"Source={constraint.source}"
             )
+        else:
+            print(
+                f"  [{idx:03d}] {type(constraint).__name__} | "
+                f"weight={getattr(constraint, 'weight', '?')} | "
+                f"source={getattr(constraint, 'source', '?')}"
+            )
 
 
 def print_constraints_with_errors(

pipeline/3_multiview_bundle_adjustment_v5_board_anchor_patch.py

@@ -165,6 +165,7 @@ def _monkey_patch_v4():
 
     spec   = importlib.util.spec_from_file_location("ba_v4", v4_path)
     mod    = importlib.util.module_from_spec(spec)
+    sys.modules["ba_v4"] = mod          # must be registered before exec_module for @dataclass
     spec.loader.exec_module(mod)
 
     # Inject PositionAnchorConstraint into the module namespace
@@ -198,9 +199,19 @@ def _monkey_patch_v4():
     def _patched_main():
         import sys as _sys
         # We hijack the module-level function calls by patching compile_constraints
-        _orig_compile = mod.compile_constraints
+        _orig_compile  = mod.compile_constraints
+        _orig_initial  = mod.initial_triangulation
+
+        # robot_data / length_scale are captured here so the initial-triangulation
+        # wrapper below can pin Board markers to their true world positions.
+        # In v4.main(), compile_constraints() runs BEFORE initial_triangulation(),
+        # so this dict is populated by the time the triangulation wrapper fires.
+        captured: Dict[str, Any] = {}
 
         def _compile_with_anchors(robot_data, length_scale, cfg):
+            captured["robot_data"]   = robot_data
+            captured["length_scale"] = length_scale
+
             marker_to_link, link_markers, constraints, issues, marker_meta = \
                 _orig_compile(robot_data, length_scale, cfg)
 
@@ -209,9 +220,40 @@ def _monkey_patch_v4():
             constraints = constraints + anchors
             return marker_to_link, link_markers, constraints, issues, marker_meta
 
-        mod.compile_constraints = _compile_with_anchors
+        def _initial_with_board(marker_observations, cameras):
+            """
+            Run the normal multi-view triangulation, then overwrite every
+            OBSERVED Board marker with its exact robot.json world position.
+            Board markers define the world frame, so they must not be left at
+            their (z-noisy) triangulated values — not even in the initial JSON.
+            """
+            tri = _orig_initial(marker_observations, cameras)
+
+            robot_data   = captured.get("robot_data")
+            length_scale = captured.get("length_scale", 1.0)
+            if robot_data is None:
+                return tri
+
+            board = (robot_data.get("links", {}) or {}).get("Board", {}) or {}
+            pinned = 0
+            for m in board.get("markers", []):
+                mid = int(m.get("id", -1))
+                pos = m.get("position", None)
+                if mid < 0 or pos is None or len(pos) != 3:
+                    continue
+                # Only pin markers that were actually observed/triangulated,
+                # so we don't introduce markers that no camera ever saw.
+                if mid in tri:
+                    tri[mid] = np.array(pos, dtype=np.float64) * float(length_scale)
+                    pinned += 1
+            print(f"[PATCH] Board markers pinned to robot.json in initial triangulation: {pinned}")
+            return tri
+
+        mod.compile_constraints   = _compile_with_anchors
+        mod.initial_triangulation = _initial_with_board
         _orig_main()
-        mod.compile_constraints = _orig_compile  # restore
+        mod.compile_constraints   = _orig_compile   # restore
+        mod.initial_triangulation = _orig_initial   # restore
 
     mod.main = _patched_main
     return mod

pipeline/5_v8_camera_z_refine.py

@@ -0,0 +1,218 @@
+#!/usr/bin/env python3
+"""
+5_v8_camera_z_refine.py
+-----------------------
+Refines camera z-positions using the FK-predicted z of the Ellbow link.
+
+Background
+----------
+Camera poses from step 2 are estimated from board markers alone.  Board
+markers are roughly coplanar (z ≈ 0), so the camera's z-height above the
+board is poorly constrained and can be 20–40 % off.
+
+Once we have the elbow angle (step 3b, from 4_v8_4b_revolute_angle.py),
+FK predicts where the Ellbow markers *should* be in world space.  The
+difference between FK-predicted z and triangulated z gives a global
+z-offset for the cameras.
+
+Algorithm
+---------
+1. Load elbow-angle result (v8_ellbow_angle.json) → joint state dict
+2. Run FK with that state → predicted world positions for Ellbow markers (mm)
+3. Load triangulated Ellbow marker positions from aruco_positions_optimized.json
+4. delta_z_mm = median(fk_z - triangulated_z)  over matched markers
+5. For each camera pose file:
+   a. camera center world  C = -R_wc.T @ t_wc
+   b. C_new = C + [0, 0, delta_z_mm / 1000]
+   c. t_wc_new = -R_wc @ C_new
+   d. Save as *_camera_pose_v8.json
+6. Save v8_z_correction.json summary
+
+Usage
+-----
+python 5_v8_camera_z_refine.py \\
+    --angle  path/to/v8_ellbow_angle.json \\
+    --robot  path/to/robot.json \\
+    --aruco  path/to/aruco_positions_optimized.json \\
+    -pose    path/to/render_a_camera_pose.json \\
+    -pose    path/to/render_b_camera_pose.json \\
+    --outDir path/to/output_dir \\
+    [--elbowLink Ellbow]
+"""
+
+from __future__ import annotations
+
+import argparse
+import copy
+import json
+import os
+import sys
+import time
+from pathlib import Path
+from typing import Dict, List, Optional
+
+import numpy as np
+
+# robot_fk lives in the same directory as this script
+sys.path.insert(0, str(Path(__file__).parent))
+from robot_fk import RobotFK
+
+
+# ──────────────────────────────────────────────────────────────
+# I/O helpers
+# ──────────────────────────────────────────────────────────────
+
+def load_json(path: str) -> dict:
+    with open(path, "r", encoding="utf-8") as f:
+        return json.load(f)
+
+
+def save_json(path: str, data: dict) -> None:
+    os.makedirs(os.path.dirname(path) or ".", exist_ok=True)
+    with open(path, "w", encoding="utf-8") as f:
+        json.dump(data, f, indent=2)
+
+
+def load_triangulated_mm(aruco_json: dict) -> Dict[int, np.ndarray]:
+    """Read marker world positions (mm) from aruco_positions_*.json."""
+    result: Dict[int, np.ndarray] = {}
+    for m in aruco_json.get("markers", []):
+        mid = int(m.get("marker_id", m.get("id", -1)))
+        if mid < 0:
+            continue
+        if "position_mm" in m:
+            result[mid] = np.array(m["position_mm"], dtype=float)
+        elif "position_m" in m:
+            result[mid] = np.array(m["position_m"], dtype=float) * 1000.0
+    return result
+
+
+# ──────────────────────────────────────────────────────────────
+# Main
+# ──────────────────────────────────────────────────────────────
+
+def main() -> None:
+    parser = argparse.ArgumentParser(
+        description="5v8: refine camera z-positions using FK-predicted Ellbow marker z"
+    )
+    parser.add_argument("--angle",     required=True,
+                        help="v8_ellbow_angle.json from 4_v8_4b_revolute_angle.py")
+    parser.add_argument("--robot",     required=True, help="robot.json")
+    parser.add_argument("--aruco",     required=True,
+                        help="aruco_positions_optimized.json (first-pass triangulation)")
+    parser.add_argument("-pose", "--poses", action="append", required=True,
+                        help="*_camera_pose.json files (repeat for each camera)")
+    parser.add_argument("--outDir",    required=True, help="Output directory")
+    parser.add_argument("--elbowLink", default="Ellbow",
+                        help="Link name used for z estimation (default: Ellbow)")
+    args = parser.parse_args()
+
+    os.makedirs(args.outDir, exist_ok=True)
+
+    # ── 1. Load angle / state ────────────────────────────────
+    angle_data = load_json(args.angle)
+    if angle_data.get("status") != "ok":
+        print(f"[ERROR] Angle estimation was not ok: {angle_data.get('reason', '?')}")
+        sys.exit(1)
+
+    accumulated_state = angle_data.get("accumulated_state", {})
+    elbow_link        = angle_data.get("link", args.elbowLink)
+
+    print(f"[INFO] Joint state from 4b:  {accumulated_state}")
+    print(f"[INFO] Link used for z-ref:  {elbow_link}")
+
+    # ── 2. FK prediction ─────────────────────────────────────
+    fk = RobotFK.from_file(args.robot)
+    T  = fk.compute(accumulated_state)
+    all_fk = fk.all_markers_world(T)      # marker_id -> {world_mm, link, …}
+
+    elbow_fk = {mid: v for mid, v in all_fk.items() if v["link"] == elbow_link}
+
+    if not elbow_fk:
+        print(f"[ERROR] No FK markers found for link '{elbow_link}'")
+        sys.exit(1)
+
+    print(f"[INFO] FK markers for '{elbow_link}': {sorted(elbow_fk.keys())}")
+
+    # ── 3. Load triangulated positions ───────────────────────
+    aruco_data    = load_json(args.aruco)
+    triangulated  = load_triangulated_mm(aruco_data)
+
+    # ── 4. Compute z-deltas ──────────────────────────────────
+    z_deltas: List[float] = []
+    print("\n[INFO] Per-marker z comparison (FK vs triangulated):")
+    for mid in sorted(elbow_fk.keys()):
+        fk_z = float(elbow_fk[mid]["world_mm"][2])
+        if mid not in triangulated:
+            print(f"  Marker {mid:4d}: FK z={fk_z:8.1f} mm   [NOT triangulated – skip]")
+            continue
+        obs_z = float(triangulated[mid][2])
+        delta = fk_z - obs_z
+        z_deltas.append(delta)
+        print(f"  Marker {mid:4d}: FK z={fk_z:8.1f} mm   obs z={obs_z:8.1f} mm   Δz={delta:+7.1f} mm")
+
+    if not z_deltas:
+        print(f"\n[ERROR] No matched markers for '{elbow_link}' — cannot compute z correction.")
+        sys.exit(1)
+
+    delta_z_mm = float(np.median(z_deltas))
+    print(f"\n[INFO] z-correction (median): {delta_z_mm:+.1f} mm  "
+          f"(from {len(z_deltas)} markers)")
+    if abs(delta_z_mm) < 1.0:
+        print("[INFO] Correction < 1 mm — cameras already well-calibrated in z.")
+
+    # ── 5. Apply correction to each camera pose ──────────────
+    corrected_files: List[str] = []
+
+    for pose_file in args.poses:
+        pose_data = load_json(pose_file)
+
+        pose_section = pose_data.get("camera_pose", {}) or {}
+        w2c          = pose_section.get("world_to_camera", {}) or {}
+
+        R_wc = np.array(w2c.get("rotation_matrix", []), dtype=float).reshape(3, 3)
+        t_wc = np.array(w2c.get("translation_m",   []), dtype=float).reshape(3)
+
+        # Camera centre in world (metres)
+        C_world     = -R_wc.T @ t_wc
+        C_world_new =  C_world + np.array([0.0, 0.0, delta_z_mm / 1000.0])
+
+        t_wc_new = -R_wc @ C_world_new
+
+        pose_new = copy.deepcopy(pose_data)
+        pose_new["camera_pose"]["world_to_camera"]["translation_m"] = \
+            [float(v) for v in t_wc_new.tolist()]
+        pose_new["camera_pose"]["camera_in_world"]["position_m"] = \
+            [float(v) for v in C_world_new.tolist()]
+        pose_new["camera_pose"]["camera_in_world"]["position_mm"] = \
+            [float(v * 1000.0) for v in C_world_new.tolist()]
+        pose_new["v8_z_correction_mm"] = delta_z_mm
+        pose_new["created_utc"] = time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime())
+
+        basename = os.path.basename(pose_file)
+        out_name = basename.replace("_camera_pose.json", "_camera_pose_v8.json")
+        if out_name == basename:
+            out_name = os.path.splitext(basename)[0] + "_v8.json"
+
+        out_path = os.path.join(args.outDir, out_name)
+        save_json(out_path, pose_new)
+        print(f"[INFO] Saved: {out_path}")
+        corrected_files.append(out_path)
+
+    # ── 6. Save correction summary ───────────────────────────
+    summary = {
+        "schema_version":     "1.0",
+        "created_utc":        time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
+        "elbow_link":         elbow_link,
+        "z_correction_mm":    delta_z_mm,
+        "n_markers_used":     len(z_deltas),
+        "z_deltas_mm":        z_deltas,
+        "corrected_pose_files": corrected_files,
+    }
+    summary_path = os.path.join(args.outDir, "v8_z_correction.json")
+    save_json(summary_path, summary)
+    print(f"\n[INFO] Correction summary → {summary_path}")
+
+
+if __name__ == "__main__":
+    main()