Add common-space calibration and surface tracking

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()