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

2026-02-10 16:24:59 +00:00
parent 1d2e66a34c
commit 2c93c77db8
3 changed files with 181 additions and 2 deletions
@@ -25,3 +25,19 @@
 - Verified hybrid mode behavior (vertical structure inclusion and fallback).
 - Verified full mode behavior.
 - Verified SOR preprocessing effectiveness.
+
+## Task 4: TukeyLoss Robust Kernel Support
+- Added `robust_kernel` and `robust_kernel_k` to `ICPConfig`.
+- Implemented TukeyLoss application in `pairwise_icp` for both Point-to-Plane and Generalized ICP.
+- Verified that TukeyLoss correctly handles outliers in synthetic tests, maintaining convergence accuracy.
+- Default behavior remains backward-compatible with `robust_kernel="none"`.
+
+## Task 6: FPFH+RANSAC Global Pre-alignment
+- Implemented `compute_fpfh_features` and `global_registration` using Open3D RANSAC.
+- Added `global_init` flag to `ICPConfig` (default False).
+- Integrated global registration into `refine_with_icp` as a pre-alignment step before pairwise ICP.
+- Added safety checks: global registration result is only used if fitness > 0.1 and the resulting transform is within `max_rotation_deg` and `max_translation_m` bounds relative to the initial extrinsic guess.
+- Verified with synthetic tests:
+  - `test_compute_fpfh_features`: Validates feature dimension and count.
+  - `test_global_registration_known_transform`: Confirms RANSAC can recover a known large transform (30 deg rotation).
+  - `test_refine_with_icp_global_init_success`: End-to-end test showing global init can recover from a very bad initial guess (90 deg error) where local ICP would fail.
@@ -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
@@ -15,6 +15,8 @@ from aruco.icp_registration import (
    pairwise_icp,
    build_pose_graph,
    refine_with_icp,
+    compute_fpfh_features,
+    global_registration,
 )
 from aruco.ground_plane import FloorPlane

@@ -291,6 +293,62 @@ def test_pairwise_icp_insufficient_points():
    assert not result.converged


+def test_robust_kernel_tukey():
+    source = create_box_pcd()
+    T_true = np.eye(4)
+    T_true[:3, 3] = [0.05, 0, 0.02]
+
+    target = o3d.geometry.PointCloud()
+    target.points = o3d.utility.Vector3dVector(
+        (np.asarray(source.points) @ T_true[:3, :3].T) + T_true[:3, 3]
+    )
+
+    # Add some outliers to test robustness
+    outliers = np.random.uniform(2, 3, (10, 3))
+    target_points = np.vstack([np.asarray(target.points), outliers])
+    target.points = o3d.utility.Vector3dVector(target_points)
+
+    config = ICPConfig(
+        min_fitness=0.5,
+        voxel_size=0.01,
+        robust_kernel="tukey",
+        robust_kernel_k=0.1,
+    )
+    result = pairwise_icp(source, target, config, np.eye(4))
+
+    assert result.converged
+    # Should still converge close to T_true despite outliers
+    np.testing.assert_allclose(result.transformation, T_true, atol=2e-2)
+
+
+def test_robust_kernel_tukey_gicp():
+    source = create_box_pcd()
+    T_true = np.eye(4)
+    T_true[:3, 3] = [0.05, 0, 0.02]
+
+    target = o3d.geometry.PointCloud()
+    target.points = o3d.utility.Vector3dVector(
+        (np.asarray(source.points) @ T_true[:3, :3].T) + T_true[:3, 3]
+    )
+
+    # Add some outliers
+    outliers = np.random.uniform(2, 3, (10, 3))
+    target_points = np.vstack([np.asarray(target.points), outliers])
+    target.points = o3d.utility.Vector3dVector(target_points)
+
+    config = ICPConfig(
+        min_fitness=0.5,
+        voxel_size=0.01,
+        method="gicp",
+        robust_kernel="tukey",
+        robust_kernel_k=0.1,
+    )
+    result = pairwise_icp(source, target, config, np.eye(4))
+
+    assert result.converged
+    np.testing.assert_allclose(result.transformation, T_true, atol=2e-2)
+
+
 def test_build_pose_graph_basic():
    serials = ["cam1", "cam2"]
    extrinsics = {"cam1": np.eye(4), "cam2": np.eye(4)}