fix uint8 issue with vlm detections

Author: NishanthJKumar

predicators/pretrained_model_interface.py

@@ -306,11 +306,12 @@ def _sample_completions(
         assert imgs is not None
         config = types.GenerateContentConfig(
             temperature=temperature,
-            candidate_count=num_completions)
+            candidate_count=num_completions,
+            http_options=types.HttpOptions(timeout=15_000))
         response = self._client.models.generate_content(
             model=self._model_name,
             contents=[prompt] + imgs,
-            config=config)    
+            config=config)
         return [response.text]
 
     def get_id(self) -> str:

predicators/spot_utils/perception/object_detection.py

@@ -578,8 +578,9 @@ def _query_vlm(
             box = (x1, y1, x2, y2)
 
             # Create a rectangular mask (no segmentation, just the bbox).
-            mask = np.zeros((h, w), dtype=np.uint8)
-            mask[int(y1):int(y2), int(x1):int(x2)] = 1
+            # Must be bool so numpy uses boolean masking, not integer indexing.
+            mask = np.zeros((h, w), dtype=bool)
+            mask[int(y1):int(y2), int(x1):int(x2)] = True
 
             score = float(det.get("confidence",
                                    CFG.spot_vlm_detection_default_score))

predicators/spot_utils/perception/spot_cameras.py

@@ -4,7 +4,7 @@
 import cv2
 import numpy as np
 from bosdyn.api import image_pb2
-from bosdyn.client.frame_helpers import BODY_FRAME_NAME, get_a_tform_b
+from bosdyn.client.frame_helpers import ODOM_FRAME_NAME, get_a_tform_b
 from bosdyn.client.image import ImageClient, build_image_request
 from bosdyn.client.sdk import Robot
 from numpy.typing import NDArray
@@ -60,12 +60,15 @@ def capture_images(
 
     rgbds: Dict[str, RGBDImageWithContext] = {}
 
-    # Get the world->robot transform so we can store world->camera transforms
-    # in the RGBDWithContexts.
+    # Get world_tform_odom so we can compute world_tform_camera using
+    # the odom frame from each image's own transforms_snapshot. This avoids
+    # timing mismatches between localization and image capture — odom is
+    # continuously tracked, so odom_tform_camera from the image snapshot is
+    # exact at capture time, and world_tform_odom is stable between
+    # localizations.
     if relocalize:
         localizer.localize()
-    world_tform_body = localizer.get_last_robot_pose()
-    body_tform_world = world_tform_body.inverse()
+    world_tform_odom = localizer.get_world_tform_odom()
 
     # Package all the requests together.
     img_reqs: image_pb2.ImageRequest = []
@@ -96,12 +99,12 @@ def capture_images(
         depth_img_resp = name_to_response[RGB_TO_DEPTH_CAMERAS[camera_name]]
         rgb_img = _image_response_to_image(rgb_img_resp)
         depth_img = _image_response_to_image(depth_img_resp)
-        # Create transform.
-        camera_tform_body = get_a_tform_b(
+        # Create transform using odom frame from the image's own snapshot
+        # to avoid timing mismatch between localization and image capture.
+        odom_tform_camera = get_a_tform_b(
             rgb_img_resp.shot.transforms_snapshot,
-            rgb_img_resp.shot.frame_name_image_sensor, BODY_FRAME_NAME)
-        camera_tform_world = camera_tform_body * body_tform_world
-        world_tform_camera = camera_tform_world.inverse()
+            ODOM_FRAME_NAME, rgb_img_resp.shot.frame_name_image_sensor)
+        world_tform_camera = world_tform_odom * odom_tform_camera
         # Extract other context.
         rot = ROTATION_ANGLE[camera_name]
         depth_scale = depth_img_resp.source.depth_scale

predicators/spot_utils/spot_localization.py

@@ -53,6 +53,7 @@ def __init__(self, robot: Robot, upload_path: Path,
 
         # Initialize robot pose, which will be updated in localize().
         self._robot_pose = math_helpers.SE3Pose(0, 0, 0, math_helpers.Quat())
+        self._odom_tform_body_at_localization = None
         # Initialize the robot's position in the map.
         robot_state = get_robot_state(self._robot)
         current_odom_tform_body = get_odom_tform_body(
@@ -125,6 +126,19 @@ def get_last_robot_pose(self) -> math_helpers.SE3Pose:
         """
         return self._robot_pose
 
+    def get_world_tform_odom(self) -> math_helpers.SE3Pose:
+        """Get world_tform_odom derived from the last localization.
+
+        This is: world_tform_body * (odom_tform_body)^{-1}
+        Both captured at the same localization instant, so they are
+        consistent. Use this to bridge from odom-frame transforms
+        (available in image snapshots) to the world frame.
+        """
+        assert self._odom_tform_body_at_localization is not None, \
+            "Must call localize() before get_world_tform_odom()"
+        body_tform_odom = self._odom_tform_body_at_localization.inverse()
+        return self._robot_pose * body_tform_odom
+
     def localize(self,
                  num_retries: int = 10,
                  retry_wait_time: float = 1.0) -> None:
@@ -150,6 +164,11 @@ def localize(self,
                                  retry_wait_time=retry_wait_time)
         logging.info("Localization succeeded.")
         self._robot_pose = math_helpers.SE3Pose.from_proto(transform)
+        # Also store odom_tform_body at localization time so we can
+        # compute world_tform_odom for bridging to image timestamps.
+        robot_state = get_robot_state(self._robot)
+        self._odom_tform_body_at_localization = get_odom_tform_body(
+            robot_state.kinematic_state.transforms_snapshot)
         return None