feat(icp): add hybrid/full point extraction with SOR preprocessing

py_workspace/tests/test_icp_registration.py

@@ -7,6 +7,8 @@ from aruco.icp_registration import (
     ICPResult,
     ICPMetrics,
     extract_near_floor_band,
+    extract_scene_points,
+    preprocess_point_cloud,
     compute_overlap_xz,
     compute_overlap_3d,
     apply_gravity_constraint,
@@ -45,6 +47,92 @@ def test_extract_near_floor_band_all_in():
     assert len(result) == 100
 
 
+def test_extract_scene_points_floor_mode_legacy():
+    points = np.array(
+        [[0, -0.1, 0], [0, 0.1, 0], [0, 0.2, 0], [0, 0.4, 0]], dtype=np.float64
+    )
+    floor_y = 0.0
+    band_height = 0.3
+    floor_normal = np.array([0, 1, 0], dtype=np.float64)
+
+    # Should match extract_near_floor_band exactly
+    expected = extract_near_floor_band(points, floor_y, band_height, floor_normal)
+    result = extract_scene_points(
+        points, floor_y, floor_normal, mode="floor", band_height=band_height
+    )
+
+    np.testing.assert_array_equal(result, expected)
+
+
+def test_extract_scene_points_full_mode():
+    points = np.random.rand(100, 3)
+    floor_y = 0.0
+    floor_normal = np.array([0, 1, 0], dtype=np.float64)
+
+    result = extract_scene_points(points, floor_y, floor_normal, mode="full")
+    np.testing.assert_array_equal(result, points)
+
+
+def test_extract_scene_points_hybrid_vertical():
+    # Create floor points + vertical wall points
+    floor_pts = np.random.uniform(-1, 1, (100, 3))
+    floor_pts[:, 1] = 0.1  # Within band
+
+    wall_pts = np.random.uniform(-1, 1, (100, 3))
+    wall_pts[:, 0] = 2.0  # Wall at x=2
+    # Wall points are tall, outside floor band
+    wall_pts[:, 1] = np.random.uniform(0.5, 2.0, 100)
+
+    points = np.vstack([floor_pts, wall_pts])
+    floor_y = 0.0
+    floor_normal = np.array([0, 1, 0], dtype=np.float64)
+
+    # Hybrid should capture both
+    result = extract_scene_points(
+        points, floor_y, floor_normal, mode="hybrid", band_height=0.3
+    )
+
+    # Should have more points than just floor band
+    floor_only = extract_near_floor_band(points, floor_y, 0.3, floor_normal)
+    assert len(result) > len(floor_only)
+
+    # Should include high points (walls)
+    assert np.any(result[:, 1] > 0.3)
+
+
+def test_extract_scene_points_hybrid_fallback():
+    # Only floor points, no vertical structure
+    points = np.random.uniform(-1, 1, (100, 3))
+    points[:, 1] = 0.1
+
+    floor_y = 0.0
+    floor_normal = np.array([0, 1, 0], dtype=np.float64)
+
+    result = extract_scene_points(
+        points, floor_y, floor_normal, mode="hybrid", band_height=0.3
+    )
+
+    # Should fall back to floor points
+    floor_only = extract_near_floor_band(points, floor_y, 0.3, floor_normal)
+    np.testing.assert_array_equal(result, floor_only)
+
+
+def test_preprocess_point_cloud_sor():
+    # Create a dense cluster + sparse outliers
+    cluster = np.random.normal(0, 0.1, (100, 3))
+    outliers = np.random.uniform(2, 3, (5, 3))
+    points = np.vstack([cluster, outliers])
+
+    pcd = o3d.geometry.PointCloud()
+    pcd.points = o3d.utility.Vector3dVector(points)
+
+    cleaned = preprocess_point_cloud(pcd, voxel_size=0.02)
+
+    # Should remove outliers
+    assert len(cleaned.points) < len(points)
+    assert len(cleaned.points) >= 90  # Keep most inliers
+
+
 def test_compute_overlap_xz_full():
     points_a = np.array([[0, 0, 0], [1, 0, 1]])
     points_b = np.array([[0, 0, 0], [1, 0, 1]])