From 9f4abc21c76249b3a8a7ba10436d2a6509b1b620 Mon Sep 17 00:00:00 2001 From: crosstyan Date: Thu, 5 Feb 2026 04:09:09 +0000 Subject: [PATCH] feat(cli): integrate depth verification into calibration workflow --- py_workspace/calibrate_extrinsics.py | 45 +++++++++++++++++++++++++++- 1 file changed, 44 insertions(+), 1 deletion(-) diff --git a/py_workspace/calibrate_extrinsics.py b/py_workspace/calibrate_extrinsics.py index 7bc5488..872e63c 100644 --- a/py_workspace/calibrate_extrinsics.py +++ b/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