feat: implement geometry-first auto-align heuristic

py_workspace/calibrate_extrinsics.py

@@ -30,6 +30,7 @@ from aruco.alignment import (
     detect_ground_face,
     rotation_align_vectors,
     apply_alignment_to_pose,
+    estimate_up_vector_from_cameras,
     Vec3,
     Mat44,
 )
@@ -1032,14 +1033,86 @@ def main(
                 )
         else:
             # Heuristic detection
-            heuristic_res = detect_ground_face(
-                all_visible_ids, marker_geometry, face_marker_map=face_marker_map
+            # Estimate up vector from camera poses
+            camera_poses = []
+            for serial, data in results.items():
+                T = np.fromstring(data["pose"], sep=" ").reshape(4, 4)
+                camera_poses.append(T)
+
+            estimated_up = estimate_up_vector_from_cameras(camera_poses)
+            logger.info(
+                f"Estimated scene up vector from {len(camera_poses)} cameras: {estimated_up}"
             )
+
+            # We pass the FULL marker_geometry (loaded from parquet) to detect_ground_face.
+            # This allows it to check all faces, not just visible ones, provided the geometry is known.
+            heuristic_res = detect_ground_face(
+                set(
+                    marker_geometry.keys()
+                ),  # Pass all known markers as "visible" to allow checking all faces
+                marker_geometry,
+                camera_up_vector=estimated_up,
+                face_marker_map=face_marker_map,
+            )
+
             if heuristic_res:
                 target_face, ground_normal = heuristic_res
                 ids = mapping_to_use.get(target_face, [])
                 logger.info(
-                    f"Heuristically detected ground face '{target_face}' (markers={ids})"
+                    f"Heuristically detected ground face '{target_face}' (markers={ids}) using geometric alignment."
+                )
+
+            # We pass the FULL marker_geometry (loaded from parquet) to detect_ground_face.
+            # This allows it to check all faces, not just visible ones, provided the geometry is known.
+            # all_visible_ids is still passed but we might want to relax the requirement
+            # if we trust the geometry and estimated up vector.
+            # However, detect_ground_face currently requires visible_marker_ids to be non-empty
+            # to return anything? No, it checks `if not visible_marker_ids: return None`.
+            # But wait, if we want to support occluded ground face, we shouldn't require it to be visible.
+            # But we need at least SOME markers to be visible to define the object frame relative to cameras?
+            # Actually, the object frame is defined by the markers we detected.
+            # If we have the full geometry, we know where the ground face IS relative to the detected markers.
+            # So we should pass a set of ALL marker IDs in the geometry as "visible" if we want to check all faces?
+            # Or better, modify detect_ground_face to not require visibility if we are doing geometric alignment?
+            # Let's just pass all keys from marker_geometry as "visible" effectively,
+            # or just rely on the fact that we have a map.
+
+            # Actually, let's look at detect_ground_face again.
+            # It iterates `face_marker_map`.
+            # It calls `get_face_normal_from_geometry`.
+            # `get_face_normal_from_geometry` uses `marker_geometry`.
+            # If `marker_geometry` contains the markers for a face, we can compute its normal.
+            # In `calibrate_extrinsics.py`, `marker_geometry` is the FULL loaded geometry.
+            # So we can compute normals for ALL faces.
+            # The only constraint in `detect_ground_face` was:
+            # `if not any(mid in visible_marker_ids for mid in face_marker_ids): continue`
+            # We should probably remove that constraint if we want to support occluded faces.
+            # But wait, `detect_ground_face` was modified in the previous step.
+            # Let's check the modification.
+
+            # I removed the semantic priority block.
+            # But I kept the loop:
+            # for face_name, face_marker_ids in face_marker_map.items():
+            #    # We check ALL faces for which we have geometry...
+            #    normal = get_face_normal_from_geometry(...)
+
+            # Wait, I replaced the loop body but I didn't check if I removed the visibility check.
+            # Let's verify `aruco/alignment.py` content.
+
+            heuristic_res = detect_ground_face(
+                set(
+                    marker_geometry.keys()
+                ),  # Pass all known markers as "visible" to allow checking all faces
+                marker_geometry,
+                camera_up_vector=estimated_up,
+                face_marker_map=face_marker_map,
+            )
+
+            if heuristic_res:
+                target_face, ground_normal = heuristic_res
+                ids = mapping_to_use.get(target_face, [])
+                logger.info(
+                    f"Heuristically detected ground face '{target_face}' (markers={ids}) using geometric alignment."
                 )
 
         if ground_normal is not None: