feat: implement 3D AABB overlap check for ICP registration

py_workspace/.sisyphus/notepads/full-icp-pipeline/learnings.md

@@ -1,20 +1,17 @@
-## 2026-02-10T09:45:00Z Session bootstrap
+- Corrected success gate logic to `> 0`.
-- Initial notepad created for full-icp-pipeline execution.
+- Added INFO logging for all attempted ICP pairs.
-- Baseline code references verified in `aruco/icp_registration.py` and `refine_ground_plane.py`.
+- 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
+## Task 3: 3D AABB Overlap Check
-
+- Implemented `compute_overlap_3d` in `aruco/icp_registration.py`.
-### Learnings
+- Added `overlap_mode` to `ICPConfig` (defaulting to "xz").
-- Open3D's `remove_statistical_outlier` returns a tuple `(pcd, ind)`, where `ind` is the list of indices. We only need the point cloud.
+- Verified 3D overlap logic with new tests in `tests/test_icp_registration.py`.
-- `estimate_normals` with `KDTreeSearchParamHybrid` is robust for mixed geometry (floor + walls).
+- Confirmed that empty inputs return 0.0 volume.
-- Hybrid extraction strategy:
+- Confirmed that disjoint boxes return 0.0 volume.
-  1. Extract floor band (spatial filter).
+- Confirmed that partial and full overlaps return correct hand-calculable volumes.
-  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.

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?

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)