feat(cli): integrate depth verification into calibration workflow

py_workspace/calibrate_extrinsics.py

@@ -17,6 +17,8 @@ from aruco.detector import (
 from aruco.pose_math import rvec_tvec_to_matrix, invert_transform, matrix_to_rvec_tvec
 from aruco.pose_averaging import PoseAccumulator
 from aruco.preview import draw_detected_markers, draw_pose_axes, show_preview
+from aruco.depth_verify import verify_extrinsics_with_depth
+from aruco.depth_refine import refine_extrinsics_with_depth
 
 
 @click.command()
@@ -79,6 +81,14 @@ def main(
     """
     Calibrate camera extrinsics relative to a global coordinate system defined by ArUco markers.
     """
+    depth_mode_map = {
+        "NEURAL": sl.DEPTH_MODE.NEURAL,
+        "ULTRA": sl.DEPTH_MODE.ULTRA,
+        "PERFORMANCE": sl.DEPTH_MODE.PERFORMANCE,
+        "NONE": sl.DEPTH_MODE.NONE,
+    }
+    sl_depth_mode = depth_mode_map.get(depth_mode, sl.DEPTH_MODE.NONE)
+
     # 1. Load Marker Geometry
     try:
         marker_geometry = load_marker_geometry(markers)
@@ -100,7 +110,7 @@ def main(
             raise click.UsageError("Missing option '--svo' / '-s'.")
 
     # 2. Initialize SVO Reader
-    reader = SVOReader(svo)
+    reader = SVOReader(svo, depth_mode=sl_depth_mode)
     if not reader.cameras:
         click.echo("No SVO files could be opened.", err=True)
         return
@@ -176,10 +186,43 @@ def main(
                             T_cam_world = rvec_tvec_to_matrix(rvec, tvec)
                             # We want T_world_from_cam
                             T_world_cam = invert_transform(T_cam_world)
+
+                            if refine_depth and frame.depth_map is not None:
+                                marker_corners_world = {
+                                    int(mid): marker_geometry[int(mid)]
+                                    for mid in ids.flatten()
+                                    if int(mid) in marker_geometry
+                                }
+                                if marker_corners_world:
+                                    T_world_cam_refined, refine_stats = (
+                                        refine_extrinsics_with_depth(
+                                            T_world_cam,
+                                            marker_corners_world,
+                                            frame.depth_map,
+                                            K,
+                                        )
+                                    )
+                                    T_world_cam = T_world_cam_refined
+
                             accumulators[serial].add_pose(
                                 T_world_cam, reproj_err, frame_count
                             )
 
+                            if verify_depth and frame.depth_map is not None:
+                                marker_corners_world = {
+                                    int(mid): marker_geometry[int(mid)]
+                                    for mid in ids.flatten()
+                                    if int(mid) in marker_geometry
+                                }
+                                if marker_corners_world:
+                                    verify_extrinsics_with_depth(
+                                        T_world_cam,
+                                        marker_corners_world,
+                                        frame.depth_map,
+                                        K,
+                                        confidence_thresh=depth_confidence_threshold,
+                                    )
+
                         if preview:
                             img = draw_detected_markers(
                                 frame.image.copy(), corners, ids