From a7e9dfdf8c60eb058999d361d77ef4449a2504c6 Mon Sep 17 00:00:00 2001
From: Naoki Yokoyama <nyokoyama@theaiinstitute.com>
Date: Tue, 29 Aug 2023 22:59:39 -0400
Subject: [PATCH] using median instead of dbscan for speed

---
 zsos/mapping/object_point_cloud_map.py |  2 +-
 zsos/mapping/obstacle_map.py           | 13 -------------
 zsos/policy/base_objectnav_policy.py   |  6 ++++--
 zsos/utils/habitat_visualizer.py       |  3 +--
 4 files changed, 6 insertions(+), 18 deletions(-)

diff --git a/zsos/mapping/object_point_cloud_map.py b/zsos/mapping/object_point_cloud_map.py
index 962c2c7..e661cfb 100644
--- a/zsos/mapping/object_point_cloud_map.py
+++ b/zsos/mapping/object_point_cloud_map.py
@@ -9,7 +9,7 @@
 
 class ObjectPointCloudMap:
     clouds: Dict[str, np.ndarray] = {}
-    use_dbscan: bool = True
+    use_dbscan: bool = False
 
     def __init__(self, erosion_size: float) -> None:
         self._erosion_size = erosion_size
diff --git a/zsos/mapping/obstacle_map.py b/zsos/mapping/obstacle_map.py
index d812441..35d7df2 100644
--- a/zsos/mapping/obstacle_map.py
+++ b/zsos/mapping/obstacle_map.py
@@ -8,7 +8,6 @@
 from zsos.mapping.base_map import BaseMap
 from zsos.mapping.value_map import JSON_PATH, KWARGS_JSON
 from zsos.utils.geometry_utils import extract_yaw, get_point_cloud, transform_points
-from zsos.utils.img_utils import remove_small_blobs
 
 
 class ObstacleMap(BaseMap):
@@ -90,18 +89,6 @@ def update_map(
         pixel_points = self._xy_to_px(xy_points)
         self._map[pixel_points[:, 1], pixel_points[:, 0]] = 1
 
-        # Clear out the space around the camera at its current location
-        robot_xy_location_px = self._xy_to_px(robot_xy_location.reshape(1, 2))[0]
-        self._map = cv2.circle(
-            self._map.astype(np.uint8),
-            tuple(robot_xy_location_px[::-1]),
-            int(self.pixels_per_meter * 0.5),
-            0,
-            -1,
-        ).astype(bool)
-
-        self._map = remove_small_blobs(self._map.astype(np.uint8), 20).astype(bool)
-
         # Update the navigable area, which is an inverse of the obstacle map after a
         # dilation operation to accommodate the robot's radius.
         self._navigable_map = 1 - cv2.dilate(
diff --git a/zsos/policy/base_objectnav_policy.py b/zsos/policy/base_objectnav_policy.py
index c90de5f..601baa6 100644
--- a/zsos/policy/base_objectnav_policy.py
+++ b/zsos/policy/base_objectnav_policy.py
@@ -119,13 +119,15 @@ def act(
                 goal[:2], deterministic=deterministic, stop=True
             )
 
-        action_numpy = pointnav_action.detach().cpu().numpy()
+        action_numpy = pointnav_action.detach().cpu().numpy()[0]
+        if len(action_numpy) == 1:
+            action_numpy = action_numpy[0]
         print(f"Step: {self._num_steps} | Mode: {mode} | Action: {action_numpy}")
         self._policy_info = self._get_policy_info(detections[0])  # a little hacky
         self._num_steps += 1
 
         self._observations_cache = {}
-        self._did_reset = True
+        self._did_reset = False
 
         return pointnav_action, rnn_hidden_states
 
diff --git a/zsos/utils/habitat_visualizer.py b/zsos/utils/habitat_visualizer.py
index 260b1dc..41fbe96 100644
--- a/zsos/utils/habitat_visualizer.py
+++ b/zsos/utils/habitat_visualizer.py
@@ -240,7 +240,6 @@ def color_point_cloud_on_map(infos, policy_info):
     )
 
     new_map = infos[0]["top_down_map"]["map"].copy()
-    for x, y in grid_xy:
-        new_map[x, y] = MAP_TARGET_POINT_INDICATOR
+    new_map[grid_xy[:, 0], grid_xy[:, 1]] = MAP_TARGET_POINT_INDICATOR
 
     infos[0]["top_down_map"]["map"] = new_map