feat: implement 3D AABB overlap check for ICP registration

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)