diff --git a/py_workspace/.sisyphus/notepads/full-icp-pipeline/learnings.md b/py_workspace/.sisyphus/notepads/full-icp-pipeline/learnings.md index 88bb776..aec4ac7 100644 --- a/py_workspace/.sisyphus/notepads/full-icp-pipeline/learnings.md +++ b/py_workspace/.sisyphus/notepads/full-icp-pipeline/learnings.md @@ -15,3 +15,13 @@ - Confirmed that empty inputs return 0.0 volume. - Confirmed that disjoint boxes return 0.0 volume. - Confirmed that partial and full overlaps return correct hand-calculable volumes. + +## Task 2: Point Extraction & Preprocessing +- Implemented `extract_scene_points` with floor, hybrid, and full modes. +- Implemented `preprocess_point_cloud` with statistical outlier removal (SOR). +- Added `region` field to `ICPConfig` dataclass. +- Added comprehensive tests for new extraction modes and preprocessing. +- Verified backward compatibility for floor mode. +- Verified hybrid mode behavior (vertical structure inclusion and fallback). +- Verified full mode behavior. +- Verified SOR preprocessing effectiveness. diff --git a/py_workspace/tests/test_icp_registration.py b/py_workspace/tests/test_icp_registration.py index 556693e..d4b9b1d 100644 --- a/py_workspace/tests/test_icp_registration.py +++ b/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]])