feat: implement ICP registration for ground plane refinement and add tests

py_workspace/aruco/icp_registration.py

@@ -245,11 +245,10 @@ def build_pose_graph(
     extrinsics: Dict[str, Mat44],
     pair_results: Dict[Tuple[str, str], ICPResult],
     reference_serial: str,
-) -> Tuple[o3d.pipelines.registration.PoseGraph, List[str]]:
+) -> o3d.pipelines.registration.PoseGraph:
     """
     Build a PoseGraph from pairwise results.
     Only includes cameras reachable from the reference camera.
-    Returns (pose_graph, optimized_serials).
     """
     # 1. Detect connected component from reference
     connected = {reference_serial}
@@ -273,6 +272,13 @@ def build_pose_graph(
     )
     serial_to_idx = {s: i for i, s in enumerate(optimized_serials)}
 
+    # Log disconnected cameras
+    disconnected = set(serials) - connected
+    if disconnected:
+        logger.warning(
+            f"Cameras disconnected from reference {reference_serial}: {disconnected}"
+        )
+
     pose_graph = o3d.pipelines.registration.PoseGraph()
     for serial in optimized_serials:
         T_wc = extrinsics[serial]
@@ -295,7 +301,7 @@ def build_pose_graph(
         )
         pose_graph.edges.append(edge)
 
-    return pose_graph, optimized_serials
+    return pose_graph
 
 
 def optimize_pose_graph(
@@ -415,7 +421,7 @@ def refine_with_icp(
         return extrinsics, metrics
 
     # 3. Pose Graph
-    pose_graph, optimized_serials = build_pose_graph(
+    pose_graph = build_pose_graph(
         valid_serials, extrinsics, pair_results, metrics.reference_camera
     )
 
@@ -425,13 +431,26 @@ def refine_with_icp(
     # 5. Extract and Validate
     new_extrinsics = extrinsics.copy()
 
-    metrics.num_disconnected = len(valid_serials) - len(optimized_serials)
-    if metrics.num_disconnected > 0:
-        disconnected_serials = set(valid_serials) - set(optimized_serials)
-        logger.warning(
-            f"Cameras disconnected from reference {metrics.reference_camera}: {disconnected_serials}"
-        )
+    # Re-derive optimized_serials to match build_pose_graph logic for node-to-serial mapping
+    connected = {metrics.reference_camera}
+    queue = [metrics.reference_camera]
+    while queue:
+        curr = queue.pop(0)
+        for (s1, s2), result in pair_results.items():
+            if not result.converged:
+                continue
+            if s1 == curr and s2 not in connected:
+                connected.add(s2)
+                queue.append(s2)
+            elif s2 == curr and s1 not in connected:
+                connected.add(s1)
+                queue.append(s1)
 
+    optimized_serials = [metrics.reference_camera] + sorted(
+        list(connected - {metrics.reference_camera})
+    )
+
+    metrics.num_disconnected = len(valid_serials) - len(optimized_serials)
     metrics.num_cameras_optimized = 0
 
     for i, serial in enumerate(optimized_serials):
@@ -457,8 +476,3 @@ def refine_with_icp(
     metrics.message = f"Optimized {metrics.num_cameras_optimized} cameras"
 
     return new_extrinsics, metrics
-
-    metrics.success = metrics.num_cameras_optimized > 1
-    metrics.message = f"Optimized {metrics.num_cameras_optimized} cameras"
-
-    return new_extrinsics, metrics