removing second-class detections when the robot comes close to them

zsos/mapping/object_point_cloud_map.py

@@ -4,7 +4,12 @@
 import numpy as np
 import open3d as o3d
 
-from zsos.utils.geometry_utils import get_point_cloud, transform_points
+from zsos.utils.geometry_utils import (
+    extract_yaw,
+    get_point_cloud,
+    transform_points,
+    within_fov_cone,
+)
 
 
 class ObjectPointCloudMap:
@@ -20,7 +25,7 @@ def reset(self):
         self.last_target_coord = None
 
     def has_object(self, target_class: str) -> bool:
-        return target_class in self.clouds
+        return target_class in self.clouds and len(self.clouds[target_class]) > 0
 
     def update_map(
         self,
@@ -40,12 +45,20 @@ def update_map(
         if len(local_cloud) == 0:
             return
 
+        # For second-class, bad detections that are too offset or out of range, we
+        # assign a random number to the last column of its point cloud that can later
+        # be used to identify which points came from the same detection.
         if too_offset(object_mask):
-            within_range = np.zeros_like(local_cloud[:, 0])
+            within_range = np.ones_like(local_cloud[:, 0]) * np.random.rand()
         else:
             # Mark all points of local_cloud whose distance from the camera is too far
             # as being out of range
             within_range = local_cloud[:, 0] <= max_depth * 0.95  # 5% margin
+            # All values of 1 in within_range will be considered within range, and all
+            # values of 0 will be considered out of range; these 0s need to be
+            # assigned with a random number so that they can be identified later.
+            within_range = within_range.astype(np.float32)
+            within_range[within_range == 0] = np.random.rand()
         global_cloud = transform_points(tf_camera_to_episodic, local_cloud)
         global_cloud = np.concatenate((global_cloud, within_range[:, None]), axis=1)
 
@@ -93,8 +106,41 @@ def get_best_object(
 
         return self.last_target_coord
 
-    def update_explored(self, *args, **kwargs):
-        pass
+    def update_explored(
+        self, tf_camera_to_episodic: np.ndarray, max_depth: float, cone_fov: float
+    ) -> None:
+        """
+        This method will remove all point clouds in self.clouds that were originally
+        detected to be out-of-range, but are now within range. This is just a heuristic
+        that suppresses ephemeral false positives that we now confirm are not actually
+        target objects.
+
+        Args:
+            tf_camera_to_episodic: The transform from the camera to the episode frame.
+            max_depth: The maximum distance from the camera that we consider to be
+                within range.
+            cone_fov: The field of view of the camera.
+        """
+        camera_coordinates = tf_camera_to_episodic[:3, 3]
+        camera_yaw = extract_yaw(tf_camera_to_episodic)
+
+        for obj in self.clouds:
+            within_range = within_fov_cone(
+                camera_coordinates,
+                camera_yaw,
+                cone_fov,
+                max_depth * 0.5,
+                self.clouds[obj],
+            )
+            range_ids = set(within_range[..., -1].tolist())
+            for range_id in range_ids:
+                if range_id == 1:
+                    # Detection was originally within range
+                    continue
+                # Remove all points from self.clouds[obj] that have the same range_id
+                self.clouds[obj] = self.clouds[obj][
+                    self.clouds[obj][..., -1] != range_id
+                ]
 
     def get_target_cloud(self, target_class: str) -> np.ndarray:
         target_cloud = self.clouds[target_class].copy()

zsos/policy/base_objectnav_policy.py

@@ -12,7 +12,7 @@
 from zsos.mapping.obstacle_map import ObstacleMap
 from zsos.obs_transformers.utils import image_resize
 from zsos.policy.utils.pointnav_policy import WrappedPointNavResNetPolicy
-from zsos.utils.geometry_utils import rho_theta
+from zsos.utils.geometry_utils import get_fov, rho_theta
 from zsos.vlm.blip2 import BLIP2Client
 from zsos.vlm.coco_classes import COCO_CLASSES
 from zsos.vlm.grounding_dino import GroundingDINOClient, ObjectDetections
@@ -86,7 +86,6 @@ def __init__(
         self._did_reset = False
         self._last_goal = np.zeros(2)
         self._done_initializing = False
-        self._target_detected = False
         self._called_stop = False
         self._compute_frontiers = compute_frontiers
         if compute_frontiers:
@@ -105,7 +104,6 @@ def _reset(self):
         self._last_goal = np.zeros(2)
         self._num_steps = 0
         self._done_initializing = False
-        self._target_detected = False
         self._called_stop = False
         if self._compute_frontiers:
             self._obstacle_map.reset()
@@ -173,7 +171,6 @@ def _explore(self, observations: "TensorDict") -> Tensor:
 
     def _get_target_object_location(self, position) -> Union[None, np.ndarray]:
         if self._object_map.has_object(self._target_object):
-            self._target_detected = True
             return self._object_map.get_best_object(self._target_object, position)
         else:
             return None
@@ -187,7 +184,7 @@ def _get_policy_info(self, detections: ObjectDetections) -> Dict[str, Any]:
             "target_object": self._target_object,
             "gps": str(self._observations_cache["robot_xy"] * np.array([1, -1])),
             "yaw": np.rad2deg(self._observations_cache["robot_heading"]),
-            "target_detected": self._target_detected,
+            "target_detected": self._object_map.has_object(self._target_object),
             "target_point_cloud": target_point_cloud,
             "nav_goal": self._last_goal,
             "stop_called": self._called_stop,
@@ -365,7 +362,8 @@ def _update_object_map(
                 fy,
             )
 
-        self._object_map.update_explored(tf_camera_to_episodic)
+        cone_fov = get_fov(fx, depth.shape[1])
+        self._object_map.update_explored(tf_camera_to_episodic, max_depth, cone_fov)
 
         return detections
 

zsos/policy/reality_policies.py

@@ -93,7 +93,6 @@ def _cache_observations(
                 fx,
                 fy,
                 topdown_fov,
-                observations["robot_xy"],
             )
         self._obstacle_map.update_agent_traj(
             observations["robot_xy"], observations["robot_heading"]

zsos/utils/geometry_utils.py

@@ -98,24 +98,27 @@ def within_fov_cone(
     cone_angle: float,
     cone_fov: float,
     cone_range: float,
-    point: np.ndarray,
-) -> bool:
-    """Checks if a point is within a cone of a given origin, angle, fov, and range.
+    points: np.ndarray,
+) -> np.ndarray:
+    """Checks if points are within a cone of a given origin, angle, fov, and range.
 
     Args:
         cone_origin (np.ndarray): The origin of the cone.
         cone_angle (float): The angle of the cone in radians.
         cone_fov (float): The field of view of the cone in radians.
         cone_range (float): The range of the cone.
-        point (np.ndarray): The point to check.
+        points (np.ndarray): The points to check.
 
+    Returns:
+        np.ndarray: The subarray of points that are within the cone.
     """
-    direction = point - cone_origin
-    dist = np.linalg.norm(direction)
-    angle = np.arctan2(direction[1], direction[0])
-    angle_diff = wrap_heading(angle - cone_angle)
+    directions = points[:, :3] - cone_origin
+    dists = np.linalg.norm(directions, axis=1)
+    angles = np.arctan2(directions[:, 1], directions[:, 0])
+    angle_diffs = np.mod(angles - cone_angle + np.pi, 2 * np.pi) - np.pi
 
-    return dist <= cone_range and abs(angle_diff) <= cone_fov / 2
+    mask = np.logical_and(dists <= cone_range, np.abs(angle_diffs) <= cone_fov / 2)
+    return points[mask]
 
 
 def convert_to_global_frame(