feat: Implement FPFH+RANSAC global pre-alignment (Task 6)

py_workspace/aruco/icp_registration.py

@@ -34,6 +34,9 @@ class ICPConfig:
     max_rotation_deg: float = 5.0  # Safety bound on ICP delta
     max_translation_m: float = 0.1  # Safety bound on ICP delta
     region: str = "floor"  # "floor", "hybrid", or "full"
+    robust_kernel: str = "none"  # "none" or "tukey"
+    robust_kernel_k: float = 0.1
+    global_init: bool = False  # Enable FPFH+RANSAC global pre-alignment
 
 
 @dataclass
@@ -175,6 +178,57 @@ def extract_near_floor_band(
     return points_world[mask]
 
 
+def compute_fpfh_features(
+    pcd_down: o3d.geometry.PointCloud,
+    voxel_size: float,
+) -> o3d.pipelines.registration.Feature:
+    """
+    Compute FPFH features for a downsampled point cloud.
+    """
+    radius_normal = voxel_size * 2
+    pcd_down.estimate_normals(
+        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30)
+    )
+
+    radius_feature = voxel_size * 5
+    pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
+        pcd_down,
+        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100),
+    )
+    return pcd_fpfh
+
+
+def global_registration(
+    source_down: o3d.geometry.PointCloud,
+    target_down: o3d.geometry.PointCloud,
+    source_fpfh: o3d.pipelines.registration.Feature,
+    target_fpfh: o3d.pipelines.registration.Feature,
+    voxel_size: float,
+) -> o3d.pipelines.registration.RegistrationResult:
+    """
+    Perform RANSAC-based global registration.
+    """
+    distance_threshold = voxel_size * 1.5
+    result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
+        source_down,
+        target_down,
+        source_fpfh,
+        target_fpfh,
+        True,  # mutual_filter
+        distance_threshold,
+        o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
+        3,  # ransac_n
+        [
+            o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
+            o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(
+                distance_threshold
+            ),
+        ],
+        o3d.pipelines.registration.RANSACConvergenceCriteria(4000000, 500),
+    )
+    return result
+
+
 def compute_overlap_xz(
     points_a: PointsNC,
     points_b: PointsNC,
@@ -271,6 +325,11 @@ def pairwise_icp(
     # but we expect at least one scale.
     reg_result = None
 
+    # Robust kernel setup
+    loss = None
+    if config.robust_kernel == "tukey":
+        loss = o3d.pipelines.registration.TukeyLoss(k=config.robust_kernel_k)
+
     for i, scale in enumerate(voxel_scales):
         voxel_size = config.voxel_size * scale
         max_iter = config.max_iterations[i]
@@ -292,7 +351,9 @@ def pairwise_icp(
 
         if config.method == "point_to_plane":
             estimation = (
-                o3d.pipelines.registration.TransformationEstimationPointToPlane()
+                o3d.pipelines.registration.TransformationEstimationPointToPlane(loss)
+                if loss
+                else o3d.pipelines.registration.TransformationEstimationPointToPlane()
             )
             reg_result = o3d.pipelines.registration.registration_icp(
                 source_down,
@@ -304,7 +365,11 @@ def pairwise_icp(
             )
         elif config.method == "gicp":
             estimation = (
-                o3d.pipelines.registration.TransformationEstimationForGeneralizedICP()
+                o3d.pipelines.registration.TransformationEstimationForGeneralizedICP(
+                    loss
+                )
+                if loss
+                else o3d.pipelines.registration.TransformationEstimationForGeneralizedICP()
             )
             reg_result = o3d.pipelines.registration.registration_generalized_icp(
                 source_down,
@@ -523,6 +588,46 @@ def refine_with_icp(
                 (np.asarray(camera_pcds[s2].points) - T_w2[:3, 3]) @ T_w2[:3, :3]
             )
 
+            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_fpfh = compute_fpfh_features(source_down, voxel_size)
+                target_fpfh = compute_fpfh_features(target_down, voxel_size)
+
+                global_result = global_registration(
+                    source_down, target_down, source_fpfh, target_fpfh, voxel_size
+                )
+
+                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)
+                    rot_diff = Rotation.from_matrix(T_diff[:3, :3]).as_euler(
+                        "xyz", degrees=True
+                    )
+                    rot_mag = np.linalg.norm(rot_diff)
+                    trans_mag = np.linalg.norm(T_diff[:3, 3])
+
+                    if (
+                        rot_mag <= config.max_rotation_deg
+                        and trans_mag <= config.max_translation_m
+                    ):
+                        logger.info(
+                            f"Global registration accepted for ({s1}, {s2}): fitness={global_result.fitness:.3f}"
+                        )
+                        init_T = T_global
+                    else:
+                        logger.warning(
+                            f"Global registration rejected for ({s1}, {s2}): exceeds bounds (rot={rot_mag:.1f}, trans={trans_mag:.3f})"
+                        )
+                else:
+                    logger.warning(
+                        f"Global registration failed for ({s1}, {s2}): low fitness {global_result.fitness:.3f}"
+                    )
+
             result = pairwise_icp(pcd1_cam, pcd2_cam, config, init_T)
 
             # Apply gravity constraint to the result relative to original transform