Add common-space calibration and surface tracking

scripts/charuco_calibration.py

@@ -121,6 +121,10 @@ def capture(args):
                     "file": filename,
                     "timestamp_monotonic": now,
                     "charuco_corners": corner_count,
+                    "charuco_ids": [] if charuco_ids is None else [int(value) for value in charuco_ids.reshape(-1)],
+                    "charuco_image_points": []
+                    if charuco_corners is None
+                    else [[float(x), float(y)] for x, y in charuco_corners.reshape((-1, 2))],
                     "marker_count": 0 if marker_ids is None else int(len(marker_ids)),
                 }
             )

scripts/solve_common_camera_space.py

@@ -0,0 +1,96 @@
+import argparse
+import json
+from pathlib import Path
+import sys
+
+import numpy as np
+
+ROOT = Path(__file__).resolve().parents[1]
+if str(ROOT) not in sys.path:
+    sys.path.insert(0, str(ROOT))
+
+from localcast.sensor_fusion import (
+    BoardObservation,
+    BoardSpec,
+    CameraIntrinsics,
+    camera_models_to_config,
+    solve_common_space_from_fixed_board,
+)
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description="Solve camera extrinsics into one fixed-board world frame.")
+    parser.add_argument("--input", required=True, help="JSON with board, intrinsics, and observations.")
+    parser.add_argument("--output", default=str(ROOT / "config" / "sensor-fusion.json"))
+    parser.add_argument("--max-reprojection-error-px", type=float, default=2.5)
+    args = parser.parse_args()
+
+    data = json.loads(Path(args.input).read_text(encoding="utf-8"))
+    board_data = data["board"]
+    board = BoardSpec(
+        squares_x=int(board_data["squares_x"]),
+        squares_y=int(board_data["squares_y"]),
+        square_length_m=float(board_data["square_length"]),
+    )
+    intrinsics = [
+        CameraIntrinsics(
+            sensor_id=item["id"],
+            camera_matrix=np.asarray(item["camera_matrix"], dtype=np.float64),
+            dist_coeffs=np.asarray(item.get("dist_coeffs", []), dtype=np.float64),
+            width=int(item["width"]),
+            height=int(item["height"]),
+            role=item.get("role", "tracking"),
+            latency_ms=float(item.get("latency_ms", 0.0)),
+        )
+        for item in data.get("intrinsics", [])
+    ]
+    observations = [
+        BoardObservation(
+            sensor_id=item["sensor_id"],
+            timestamp_ns=int(item.get("timestamp_ns", 0)),
+            corner_ids=np.asarray(item["corner_ids"], dtype=np.int32),
+            image_points=np.asarray(item["image_points"], dtype=np.float64),
+            confidence=float(item.get("confidence", 1.0)),
+        )
+        for item in data.get("observations", [])
+    ]
+    cameras, solves = solve_common_space_from_fixed_board(
+        intrinsics,
+        board,
+        observations,
+        max_reprojection_error_px=args.max_reprojection_error_px,
+    )
+    output = {
+        "fusion": data.get(
+            "fusion",
+            {
+                "max_pair_dt_ns": 25000000,
+                "max_reprojection_error_px": 4.0,
+                "min_confidence": 0.1,
+                "cache_ttl_ns": 500000000,
+            },
+        ),
+        "render_bridge": data.get("render_bridge", {}),
+        "cameras": camera_models_to_config(cameras.values()),
+        "calibration_report": {
+            "world_frame": "fixed ChArUco board coordinates",
+            "solves": [
+                {
+                    "sensor_id": solve.sensor_id,
+                    "timestamp_ns": solve.timestamp_ns,
+                    "corner_count": solve.corner_count,
+                    "reprojection_error_px": solve.reprojection_error_px,
+                }
+                for solve in solves
+            ],
+        },
+    }
+    out = Path(args.output)
+    out.parent.mkdir(parents=True, exist_ok=True)
+    out.write_text(json.dumps(output, indent=2), encoding="utf-8")
+    print(out)
+    print(json.dumps(output["calibration_report"], indent=2))
+
+
+if __name__ == "__main__":
+    main()

scripts/triangulate_surface_features.py

@@ -0,0 +1,68 @@
+import argparse
+from pathlib import Path
+import sys
+import time
+
+import cv2
+
+ROOT = Path(__file__).resolve().parents[1]
+if str(ROOT) not in sys.path:
+    sys.path.insert(0, str(ROOT))
+
+from localcast.sensor_fusion import (
+    PointCloud,
+    load_fusion_config,
+    match_surface_features,
+    orb_surface_observations,
+    triangulate_surface_tracks,
+)
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description="Match surface features between two calibrated views and write a PLY.")
+    parser.add_argument("--config", default=str(ROOT / "config" / "sensor-fusion.json"))
+    parser.add_argument("--left-id", required=True)
+    parser.add_argument("--right-id", required=True)
+    parser.add_argument("--left-image", required=True)
+    parser.add_argument("--right-image", required=True)
+    parser.add_argument("--output", required=True)
+    parser.add_argument("--max-features", type=int, default=2000)
+    parser.add_argument("--max-descriptor-distance", type=float, default=44.0)
+    parser.add_argument("--max-reprojection-error-px", type=float, default=4.0)
+    args = parser.parse_args()
+
+    rig = load_fusion_config(Path(args.config))
+    left = cv2.imread(args.left_image)
+    right = cv2.imread(args.right_image)
+    if left is None:
+        raise SystemExit(f"Could not read {args.left_image}")
+    if right is None:
+        raise SystemExit(f"Could not read {args.right_image}")
+    timestamp_ns = time.monotonic_ns()
+    left_features = orb_surface_observations(args.left_id, left, timestamp_ns, max_features=args.max_features)
+    right_features = orb_surface_observations(args.right_id, right, timestamp_ns, max_features=args.max_features)
+    tracks = match_surface_features(
+        left_features,
+        right_features,
+        max_descriptor_distance=args.max_descriptor_distance,
+        max_dt_ns=25_000_000,
+    )
+    points = triangulate_surface_tracks(
+        tracks,
+        rig.cameras,
+        max_reprojection_error_px=args.max_reprojection_error_px,
+    )
+    PointCloud(points).write_ply(Path(args.output))
+    print(
+        {
+            "left_features": len(left_features),
+            "right_features": len(right_features),
+            "tracks": len(tracks),
+            "points": len(points),
+            "output": args.output,
+        }
+    )
+
+
+if __name__ == "__main__":
+    main()