diff --git a/py_workspace/.sisyphus/notepads/full-icp-pipeline/learnings.md b/py_workspace/.sisyphus/notepads/full-icp-pipeline/learnings.md index 8e85ef0..88bb776 100644 --- a/py_workspace/.sisyphus/notepads/full-icp-pipeline/learnings.md +++ b/py_workspace/.sisyphus/notepads/full-icp-pipeline/learnings.md @@ -1,20 +1,17 @@ -## 2026-02-10T09:45:00Z Session bootstrap -- Initial notepad created for full-icp-pipeline execution. -- Baseline code references verified in `aruco/icp_registration.py` and `refine_ground_plane.py`. +- Corrected success gate logic to `> 0`. +- Added INFO logging for all attempted ICP pairs. +- Ensured all pairs are stored in `metrics.per_pair_results`. +- Fixed overlap skip logging to use DEBUG level. +- Fixed syntax and indentation errors in `aruco/icp_registration.py` that were causing unreachable code and malformed control flow. +- Relaxed success gate to `metrics.num_cameras_optimized > 0`, allowing single-camera optimizations to be considered successful. +- Implemented comprehensive per-pair diagnostic logging: INFO for ICP results (fitness, RMSE, convergence) and DEBUG for overlap skips. +- Ensured all attempted ICP results are stored in `metrics.per_pair_results` for better downstream diagnostics. +- Updated `tests/test_icp_registration.py` to reflect the new success gate logic. -## Task 2: Point Extraction Functions - -### Learnings -- Open3D's `remove_statistical_outlier` returns a tuple `(pcd, ind)`, where `ind` is the list of indices. We only need the point cloud. -- `estimate_normals` with `KDTreeSearchParamHybrid` is robust for mixed geometry (floor + walls). -- Hybrid extraction strategy: - 1. Extract floor band (spatial filter). - 2. Extract vertical points (normal-based filter: `abs(normal ยท floor_normal) < 0.3`). - 3. Combine using boolean masks on the original point set to avoid duplicates. -- `extract_scene_points` provides a unified interface for different registration strategies (floor-only vs full-scene). - -### Decisions -- Kept `extract_near_floor_band` as a standalone function for backward compatibility and as a helper for `extract_scene_points`. -- Used `mode='floor'` as default to match existing behavior. -- Implemented `preprocess_point_cloud` to encapsulate downsampling and SOR, making the pipeline cleaner. -- Added `region` field to `ICPConfig` to control the extraction mode in future tasks. +## Task 3: 3D AABB Overlap Check +- Implemented `compute_overlap_3d` in `aruco/icp_registration.py`. +- Added `overlap_mode` to `ICPConfig` (defaulting to "xz"). +- Verified 3D overlap logic with new tests in `tests/test_icp_registration.py`. +- 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. diff --git a/py_workspace/aruco/icp_registration.py b/py_workspace/aruco/icp_registration.py index d913915..9aa4276 100644 --- a/py_workspace/aruco/icp_registration.py +++ b/py_workspace/aruco/icp_registration.py @@ -131,6 +131,9 @@ def extract_scene_points( vertical_pts = points_world[vertical_mask] if len(vertical_pts) == 0: + logger.warning( + "No vertical structure found in hybrid mode, falling back to floor points" + ) return floor_pts # Combine unique points (though sets are disjoint by definition of mask vs band? diff --git a/py_workspace/tests/test_icp_registration.py b/py_workspace/tests/test_icp_registration.py index e281595..556693e 100644 --- a/py_workspace/tests/test_icp_registration.py +++ b/py_workspace/tests/test_icp_registration.py @@ -8,6 +8,7 @@ from aruco.icp_registration import ( ICPMetrics, extract_near_floor_band, compute_overlap_xz, + compute_overlap_3d, apply_gravity_constraint, pairwise_icp, build_pose_graph, @@ -74,6 +75,35 @@ def test_compute_overlap_xz_with_margin(): assert area_with_margin > 0.0 +def test_compute_overlap_3d_full(): + points_a = np.array([[0, 0, 0], [1, 1, 1]]) + points_b = np.array([[0, 0, 0], [1, 1, 1]]) + volume = compute_overlap_3d(points_a, points_b) + assert abs(volume - 1.0) < 1e-6 + + +def test_compute_overlap_3d_no(): + points_a = np.array([[0, 0, 0], [1, 1, 1]]) + points_b = np.array([[2, 2, 2], [3, 3, 3]]) + volume = compute_overlap_3d(points_a, points_b) + assert volume == 0.0 + + +def test_compute_overlap_3d_partial(): + # Overlap in [0.5, 1.0] for all axes -> 0.5^3 = 0.125 + points_a = np.array([[0, 0, 0], [1, 1, 1]]) + points_b = np.array([[0.5, 0.5, 0.5], [1.5, 1.5, 1.5]]) + volume = compute_overlap_3d(points_a, points_b) + assert abs(volume - 0.125) < 1e-6 + + +def test_compute_overlap_3d_empty(): + points_a = np.zeros((0, 3)) + points_b = np.array([[0, 0, 0], [1, 1, 1]]) + assert compute_overlap_3d(points_a, points_b) == 0.0 + assert compute_overlap_3d(points_b, points_a) == 0.0 + + def test_apply_gravity_constraint_identity(): T = np.eye(4) result = apply_gravity_constraint(T, T)