fix(icp): correct pose graph edge direction

py_workspace/aruco/icp_registration.py

@@ -33,6 +33,14 @@ class ICPConfig:
     max_correspondence_distance_factor: float = 2.5
     max_rotation_deg: float = 10.0  # Safety bound on ICP delta
     max_translation_m: float = 0.3  # Safety bound on ICP delta
+    max_pair_rotation_deg: float = 5.0  # Plausibility gate for pairwise updates
+    max_pair_translation_m: float = 0.5  # Plausibility gate for pairwise updates
+    max_final_rotation_deg: float = (
+        15.0  # Stricter post-opt gate for final camera update
+    )
+    max_final_translation_m: float = (
+        1.0  # Stricter post-opt gate for final camera update
+    )
     region: str = "floor"  # "floor", "hybrid", or "full"
     robust_kernel: str = "none"  # "none" or "tukey"
     robust_kernel_k: float = 0.1
@@ -474,11 +482,13 @@ def build_pose_graph(
         idx1 = serial_to_idx[s1]
         idx2 = serial_to_idx[s2]
 
-        # Edge from idx2 to idx1 (transformation maps 1 to 2)
-        # Open3D PoseGraphEdge(source, target, T) means P_source = T * P_target
-        # Here P_2 = T_21 * P_1, so source=2, target=1
+        # Edge from idx1 to idx2
+        # Open3D PoseGraphEdge(s, t, T) enforces T = Pose(t)^-1 * Pose(s)
+        # i.e., T is the pose of s in t's frame (T_t_s).
+        # result.transformation is T_c2_c1 (aligns pcd1 to pcd2), which is Pose of c1 in c2.
+        # So s=c1 (idx1), t=c2 (idx2).
         edge = o3d.pipelines.registration.PoseGraphEdge(
-            idx2, idx1, result.transformation, result.information_matrix, uncertain=True
+            idx1, idx2, result.transformation, result.information_matrix, uncertain=True
         )
         pose_graph.edges.append(edge)
 
@@ -523,10 +533,16 @@ def refine_with_icp(
 
     metrics.reference_camera = serials[0]
 
-    # 1. Extract near-floor bands
+    # 1. Extract scene points
     camera_pcds: Dict[str, o3d.geometry.PointCloud] = {}
     camera_points: Dict[str, PointsNC] = {}
 
+    # Auto-set overlap mode based on region
+    if config.region == "floor":
+        config.overlap_mode = "xz"
+    elif config.region in ["hybrid", "full"]:
+        config.overlap_mode = "3d"
+
     for serial in serials:
         if serial not in floor_planes or serial not in extrinsics:
             continue
@@ -540,17 +556,29 @@ def refine_with_icp(
 
         # floor_y = -plane.d (distance to origin along normal)
         floor_y = -plane.d
-        band_points = extract_near_floor_band(
-            points_world, floor_y, config.band_height, plane.normal
+
+        scene_points = extract_scene_points(
+            points_world,
+            floor_y,
+            plane.normal,
+            mode=config.region,
+            band_height=config.band_height,
         )
 
-        if len(band_points) < 100:
+        if len(scene_points) < 100:
             continue
 
         pcd = o3d.geometry.PointCloud()
-        pcd.points = o3d.utility.Vector3dVector(band_points)
+        pcd.points = o3d.utility.Vector3dVector(scene_points)
+
+        # Apply SOR preprocessing
+        pcd = preprocess_point_cloud(pcd, config.voxel_size)
+
+        if len(pcd.points) < 100:
+            continue
+
         camera_pcds[serial] = pcd
-        camera_points[serial] = band_points
+        camera_points[serial] = np.asarray(pcd.points)
 
     # 2. Pairwise ICP
     valid_serials = sorted(list(camera_pcds.keys()))
@@ -560,12 +588,20 @@ def refine_with_icp(
         for j in range(i + 1, len(valid_serials)):
             s2 = valid_serials[j]
 
-            area = compute_overlap_xz(
-                camera_points[s1], camera_points[s2], config.overlap_margin
-            )
+            # Dispatch overlap check
+            if config.overlap_mode == "3d":
+                area = compute_overlap_3d(
+                    camera_points[s1], camera_points[s2], config.overlap_margin
+                )
+            else:
+                area = compute_overlap_xz(
+                    camera_points[s1], camera_points[s2], config.overlap_margin
+                )
+
             if area < config.min_overlap_area:
+                unit = "m^3" if config.overlap_mode == "3d" else "m^2"
                 logger.debug(
-                    f"Skipping pair ({s1}, {s2}) due to insufficient overlap: {area:.2f} m^2 < {config.min_overlap_area:.2f} m^2"
+                    f"Skipping pair ({s1}, {s2}) due to insufficient overlap: {area:.2f} {unit} < {config.min_overlap_area:.2f} {unit}"
                 )
                 continue
 
@@ -577,22 +613,24 @@ def refine_with_icp(
             T_w2 = extrinsics[s2]
             init_T = invert_transform(T_w2) @ T_w1
 
-            # pairwise_icp aligns source_pcd to target_pcd.
-            # We pass camera-frame points to pairwise_icp to use init_T meaningfully.
-            pcd1_cam = o3d.geometry.PointCloud()
-            pcd1_cam.points = o3d.utility.Vector3dVector(
+            # Prepare point clouds for ICP
+            # Always transform back to camera frame to refine relative transform directly
+            # This avoids world-frame identity traps and ensures consistent behavior across regions
+            pcd1 = o3d.geometry.PointCloud()
+            pcd1.points = o3d.utility.Vector3dVector(
                 (np.asarray(camera_pcds[s1].points) - T_w1[:3, 3]) @ T_w1[:3, :3]
             )
-            pcd2_cam = o3d.geometry.PointCloud()
-            pcd2_cam.points = o3d.utility.Vector3dVector(
+            pcd2 = o3d.geometry.PointCloud()
+            pcd2.points = o3d.utility.Vector3dVector(
                 (np.asarray(camera_pcds[s2].points) - T_w2[:3, 3]) @ T_w2[:3, :3]
             )
+            current_init = init_T
 
             if config.global_init:
                 # Downsample for global registration
                 voxel_size = config.voxel_size
-                source_down = pcd1_cam.voxel_down_sample(voxel_size)
-                target_down = pcd2_cam.voxel_down_sample(voxel_size)
+                source_down = pcd1.voxel_down_sample(voxel_size)
+                target_down = pcd2.voxel_down_sample(voxel_size)
 
                 source_fpfh = compute_fpfh_features(source_down, voxel_size)
                 target_fpfh = compute_fpfh_features(target_down, voxel_size)
@@ -604,7 +642,10 @@ def refine_with_icp(
                 if global_result.fitness > 0.1:
                     # Validate against safety bounds relative to extrinsic init
                     T_global = global_result.transformation
-                    T_diff = T_global @ invert_transform(init_T)
+
+                    # Compare T_global against current_init
+                    T_diff = T_global @ invert_transform(current_init)
+
                     rot_diff = Rotation.from_matrix(T_diff[:3, :3]).as_euler(
                         "xyz", degrees=True
                     )
@@ -618,7 +659,7 @@ def refine_with_icp(
                         logger.info(
                             f"Global registration accepted for ({s1}, {s2}): fitness={global_result.fitness:.3f}"
                         )
-                        init_T = T_global
+                        current_init = T_global
                     else:
                         logger.warning(
                             f"Global registration rejected for ({s1}, {s2}): exceeds bounds (rot={rot_mag:.1f}, trans={trans_mag:.3f})"
@@ -628,11 +669,24 @@ def refine_with_icp(
                         f"Global registration failed for ({s1}, {s2}): low fitness {global_result.fitness:.3f}"
                     )
 
-            result = pairwise_icp(pcd1_cam, pcd2_cam, config, init_T)
+            result = pairwise_icp(pcd1, pcd2, config, current_init)
 
             # Apply gravity constraint to the result relative to original transform
+            # T_original is the initial T_21 (from extrinsics)
+            T_original_21 = invert_transform(T_w2) @ T_w1
+
             result.transformation = apply_gravity_constraint(
-                result.transformation, init_T, config.gravity_penalty_weight
+                result.transformation, T_original_21, config.gravity_penalty_weight
+            )
+
+            # Debug logging for transform deltas
+            T_delta = result.transformation @ invert_transform(init_T)
+            rot_delta = Rotation.from_matrix(T_delta[:3, :3]).as_euler(
+                "xyz", degrees=True
+            )
+            trans_delta = np.linalg.norm(T_delta[:3, 3])
+            logger.debug(
+                f"Pair ({s1}, {s2}) delta: rot={np.linalg.norm(rot_delta):.2f}deg, trans={trans_delta:.3f}m"
             )
 
             metrics.per_pair_results[(s1, s2)] = result
@@ -640,8 +694,20 @@ def refine_with_icp(
                 f"Pair ({s1}, {s2}) ICP result: fitness={result.fitness:.3f}, rmse={result.inlier_rmse:.4f}, converged={result.converged}"
             )
             if result.converged:
-                metrics.num_pairs_converged += 1
-                pair_results[(s1, s2)] = result
+                # Pair-level plausibility gate
+                rot_mag = np.linalg.norm(rot_delta)
+                if (
+                    rot_mag > config.max_pair_rotation_deg
+                    or trans_delta > config.max_pair_translation_m
+                ):
+                    logger.warning(
+                        f"Pair ({s1}, {s2}) converged but rejected by pair-level gate: "
+                        f"rot={rot_mag:.2f}deg (max={config.max_pair_rotation_deg}), "
+                        f"trans={trans_delta:.3f}m (max={config.max_pair_translation_m})"
+                    )
+                else:
+                    metrics.num_pairs_converged += 1
+                    pair_results[(s1, s2)] = result
 
     if not pair_results:
         metrics.message = "No converged ICP pairs"
@@ -676,6 +742,8 @@ def refine_with_icp(
         list(connected - {metrics.reference_camera})
     )
 
+    logger.info(f"Optimized connected component: {optimized_serials}")
+
     metrics.num_disconnected = len(valid_serials) - len(optimized_serials)
     metrics.num_cameras_optimized = 0
 
@@ -689,9 +757,32 @@ def refine_with_icp(
         rot_mag = np.linalg.norm(rot_delta)
         trans_mag = np.linalg.norm(T_delta[:3, 3])
 
-        if rot_mag > config.max_rotation_deg or trans_mag > config.max_translation_m:
+        logger.debug(
+            f"Camera {serial} optimization delta: rot={rot_mag:.2f}deg, trans={trans_mag:.3f}m"
+        )
+
+        # Validate delta against both existing and final safety bounds
+        if rot_mag > config.max_rotation_deg:
             logger.warning(
-                f"Camera {serial} ICP correction exceeds bounds: rot={rot_mag:.2f} deg, trans={trans_mag:.3f} m. Rejecting."
+                f"Camera {serial} ICP correction exceeds max_rotation_deg: {rot_mag:.2f}deg > {config.max_rotation_deg:.2f}deg. Rejecting."
+            )
+            continue
+
+        if rot_mag > config.max_final_rotation_deg:
+            logger.warning(
+                f"Camera {serial} ICP correction exceeds max_final_rotation_deg: {rot_mag:.2f}deg > {config.max_final_rotation_deg:.2f}deg. Rejecting."
+            )
+            continue
+
+        if trans_mag > config.max_translation_m:
+            logger.warning(
+                f"Camera {serial} ICP correction exceeds max_translation_m: {trans_mag:.3f}m > {config.max_translation_m:.3f}m. Rejecting."
+            )
+            continue
+
+        if trans_mag > config.max_final_translation_m:
+            logger.warning(
+                f"Camera {serial} ICP correction exceeds max_final_translation_m: {trans_mag:.3f}m > {config.max_final_translation_m:.3f}m. Rejecting."
             )
             continue