feat: implement geometry-first auto-align heuristic

py_workspace/tests/test_alignment.py

@@ -148,10 +148,18 @@ def test_detect_ground_face():
     assert face_name == "bottom"
     np.testing.assert_allclose(normal, np.array([0, -1, 0]), atol=1e-10)
 
-    # Only top visible
+    # Case 1: We know about bottom, but only top is visible. Should pick bottom (best alignment).
     res = detect_ground_face({2}, marker_geometry, camera_up, face_marker_map)
     assert res is not None
     face_name, normal = res
+    assert face_name == "bottom"
+    np.testing.assert_allclose(normal, np.array([0, -1, 0]), atol=1e-10)
+
+    # Case 2: We don't know about bottom (e.g. partial map). Should pick top (best available).
+    partial_geometry = {2: marker_geometry[2]}
+    res = detect_ground_face({2}, partial_geometry, camera_up, face_marker_map)
+    assert res is not None
+    face_name, normal = res
     assert face_name == "top"
     np.testing.assert_allclose(normal, np.array([0, 1, 0]), atol=1e-10)
 
@@ -162,3 +170,40 @@ def test_detect_ground_face():
 
     # Missing map
     assert detect_ground_face({1, 2}, marker_geometry, camera_up, None) is None
+
+
+def test_detect_ground_face_geometric_priority():
+    # Test that geometric alignment is preferred over semantic names
+    # Scenario: 'bottom' face is tilted 45 deg, 'side' face is perfectly aligned with camera up
+    # This simulates a box placed on its side
+
+    face_marker_map = {
+        "bottom": [1],
+        "side": [2],
+    }
+
+    # Camera up is [0, -1, 0] (Y-down convention common in CV, or Y-up depending on setup)
+    # Let's assume we want to align with [0, -1, 0]
+    camera_up = np.array([0, -1, 0], dtype=np.float64)
+
+    # Marker 1 (bottom): Tilted 45 deg. Normal = [0.707, -0.707, 0]
+    # Dot product with [0, -1, 0] = 0.707
+    marker_geometry = {
+        1: np.array([[0, 0, 0], [1, 1, 0], [1, 1, 1], [0, 0, 1]], dtype=np.float64),
+        # v1=[1,1,0], v2=[0,0,1] -> cross=[1, -1, 0] -> norm=[0.707, -0.707, 0]
+        # Marker 2 (side): Perfectly aligned. Normal = [0, -1, 0]
+        # Dot product with [0, -1, 0] = 1.0
+        2: np.array([[0, 0, 0], [1, 0, 0], [1, 0, 1], [0, 0, 1]], dtype=np.float64),
+        # v1=[1,0,0], v2=[0,0,1] -> cross=[0, -1, 0]
+    }
+
+    # OLD BEHAVIOR: would pick 'bottom' because of name
+    # NEW BEHAVIOR: should pick 'side' because of better alignment score
+
+    res = detect_ground_face({1, 2}, marker_geometry, camera_up, face_marker_map)
+    assert res is not None
+    face_name, normal = res
+
+    # This assertion will fail until we fix the code
+    assert face_name == "side"
+    np.testing.assert_allclose(normal, np.array([0, -1, 0]), atol=1e-10)