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

py_workspace/tests/test_icp_registration.py

@@ -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)}