diff --git a/configs/tasks/obj_nav_mp3d.yaml b/configs/tasks/obj_nav_mp3d.yaml
index 8fdcb4aa73940c33728ef6ca9f384f7257f70135..34ea206eaa7832c1bb4cca8f1c9f6a0875b0184a 100644
--- a/configs/tasks/obj_nav_mp3d.yaml
+++ b/configs/tasks/obj_nav_mp3d.yaml
@@ -1,12 +1,12 @@
ENVIRONMENT:
MAX_EPISODE_STEPS: 750
SIMULATOR:
- TURN_ANGLE: 30
+ TURN_ANGLE: 45
TILT_ANGLE: 30
AGENT_0:
SENSORS: ['RGB_SENSOR', 'DEPTH_SENSOR']
HEIGHT: 0.88
- RADIUS: 0.18
+ RADIUS: 0.2
HABITAT_SIM_V0:
GPU_DEVICE_ID: 0
SEMANTIC_SENSOR:
diff --git a/habitat/config/default.py b/habitat/config/default.py
index c2de0fdf664ca184f3f1b4506e39771de0b640ec..cd4a448b579c5feef9aa5afbe9430915cc83fe11 100644
--- a/habitat/config/default.py
+++ b/habitat/config/default.py
@@ -138,13 +138,17 @@ _C.TASK.TOP_DOWN_MAP.MAX_EPISODE_STEPS = _C.ENVIRONMENT.MAX_EPISODE_STEPS
_C.TASK.TOP_DOWN_MAP.MAP_PADDING = 3
_C.TASK.TOP_DOWN_MAP.NUM_TOPDOWN_MAP_SAMPLE_POINTS = 20000
_C.TASK.TOP_DOWN_MAP.MAP_RESOLUTION = 1250
-_C.TASK.TOP_DOWN_MAP.DRAW_SOURCE_AND_TARGET = True
+_C.TASK.TOP_DOWN_MAP.DRAW_SOURCE = True
_C.TASK.TOP_DOWN_MAP.DRAW_BORDER = True
_C.TASK.TOP_DOWN_MAP.DRAW_SHORTEST_PATH = True
_C.TASK.TOP_DOWN_MAP.FOG_OF_WAR = CN()
_C.TASK.TOP_DOWN_MAP.FOG_OF_WAR.DRAW = True
_C.TASK.TOP_DOWN_MAP.FOG_OF_WAR.VISIBILITY_DIST = 5.0
_C.TASK.TOP_DOWN_MAP.FOG_OF_WAR.FOV = 90
+_C.TASK.TOP_DOWN_MAP.DRAW_VIEW_POINTS = True
+_C.TASK.TOP_DOWN_MAP.DRAW_GOAL_POSITIONS = True
+# Axes aligned bounding boxes
+_C.TASK.TOP_DOWN_MAP.DRAW_GOAL_AABBS = True
# -----------------------------------------------------------------------------
# COLLISIONS MEASUREMENT
# -----------------------------------------------------------------------------
diff --git a/habitat/tasks/nav/nav.py b/habitat/tasks/nav/nav.py
index 8eecdaa22b8adfd52f6f16e2c37f55609ecc2741..d7b9e5981e78f7fa6d7bf7edb86ccfef04a3dff5 100644
--- a/habitat/tasks/nav/nav.py
+++ b/habitat/tasks/nav/nav.py
@@ -20,6 +20,7 @@ from habitat.core.embodied_task import (
from habitat.core.logging import logger
from habitat.core.registry import registry
from habitat.core.simulator import (
+ AgentState,
Sensor,
SensorTypes,
ShortestPathPoint,
@@ -532,6 +533,9 @@ class TopDownMap(Measure):
self.line_thickness = int(
np.round(self._map_resolution[0] * 2 / MAP_THICKNESS_SCALAR)
)
+ self.point_padding = 2 * int(
+ np.ceil(self._map_resolution[0] / MAP_THICKNESS_SCALAR)
+ )
super().__init__()
def _get_uuid(self, *args: Any, **kwargs: Any):
@@ -561,89 +565,112 @@ class TopDownMap(Measure):
return top_down_map
- def draw_source_and_target(self, episode):
- # mark source point
- s_x, s_y = maps.to_grid(
- episode.start_position[0],
- episode.start_position[2],
+ def _draw_point(self, position, point_type):
+ t_x, t_y = maps.to_grid(
+ position[0],
+ position[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
- point_padding = 2 * int(
- np.ceil(self._map_resolution[0] / MAP_THICKNESS_SCALAR)
- )
self._top_down_map[
- s_x - point_padding : s_x + point_padding + 1,
- s_y - point_padding : s_y + point_padding + 1,
- ] = maps.MAP_SOURCE_POINT_INDICATOR
-
- for goal in episode.goals:
- try:
- if goal.view_points is not None:
- for view_point in goal.view_points:
- # mark view point
- t_x, t_y = maps.to_grid(
- view_point.agent_state.position[0],
- view_point.agent_state.position[2],
+ t_x - self.point_padding : t_x + self.point_padding + 1,
+ t_y - self.point_padding : t_y + self.point_padding + 1,
+ ] = point_type
+
+ def _draw_goals_view_points(self, episode):
+ if self._config.DRAW_VIEW_POINTS:
+ for goal in episode.goals:
+ try:
+ if goal.view_points is not None:
+ for view_point in goal.view_points:
+ self._draw_point(
+ view_point.agent_state.position,
+ maps.MAP_VIEW_POINT_INDICATOR,
+ )
+ except AttributeError:
+ pass
+
+ def _draw_goals_positions(self, episode):
+ if self._config.DRAW_GOAL_POSITIONS:
+
+ for goal in episode.goals:
+ try:
+ self._draw_point(
+ goal.position, maps.MAP_TARGET_POINT_INDICATOR
+ )
+ except AttributeError:
+ pass
+
+ def _draw_goals_aabb(self, episode):
+ if self._config.DRAW_GOAL_AABBS:
+ for goal in episode.goals:
+ try:
+ sem_scene = self._sim.semantic_annotations()
+ object_id = goal.object_id
+ assert int(
+ sem_scene.objects[object_id].id.split("_")[-1]
+ ) == int(
+ goal.object_id
+ ), f"Object_id doesn't correspond to id in semantic scene objects dictionary for episode: {episode}"
+
+ center = sem_scene.objects[object_id].aabb.center
+ x_len, _, z_len = (
+ sem_scene.objects[object_id].aabb.sizes / 2.0
+ )
+ # Nodes to draw rectangle
+ corners = [
+ center + np.array([x, 0, z])
+ for x, z in [
+ (-x_len, -z_len),
+ (-x_len, z_len),
+ (x_len, z_len),
+ (x_len, -z_len),
+ (-x_len, -z_len),
+ ]
+ ]
+
+ map_corners = [
+ maps.to_grid(
+ p[0],
+ p[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
)
-
- self._top_down_map[
- t_x - point_padding : t_x + point_padding + 1,
- t_y - point_padding : t_y + point_padding + 1,
- ] = maps.MAP_VIEW_POINT_INDICATOR
- except AttributeError:
- pass
-
- for goal in episode.goals:
- # mark target point
- t_x, t_y = maps.to_grid(
- goal.position[0],
- goal.position[2],
- self._coordinate_min,
- self._coordinate_max,
- self._map_resolution,
- )
-
- self._top_down_map[
- t_x - point_padding : t_x + point_padding + 1,
- t_y - point_padding : t_y + point_padding + 1,
- ] = maps.MAP_TARGET_POINT_INDICATOR
-
- sem_scene = self._sim.semantic_annotations()
- object_id = goal.object_id
- assert int(sem_scene.objects[object_id].id.split("_")[-1]) == int(
- goal.object_id
+ for p in corners
+ ]
+
+ maps.draw_path(
+ self._top_down_map,
+ map_corners,
+ maps.MAP_TARGET_BOUNDING_BOX,
+ self.line_thickness,
+ )
+ except AttributeError:
+ pass
+
+ def _draw_shortest_path(
+ self, episode: Episode, agent_position: AgentState
+ ):
+ if self._config.DRAW_SHORTEST_PATH:
+ self._shortest_path_points = self._sim.get_straight_shortest_path_points(
+ agent_position, episode.goals[0].position
)
-
- center = sem_scene.objects[object_id].aabb.center
- import itertools
-
- x_len, _, z_len = sem_scene.objects[object_id].aabb.sizes / 2.0
- corners = [
- center + np.array([x, 0, z])
- for x, z in itertools.product([-x_len, x_len], [z_len, -z_len])
- ]
- corners[2], corners[3] = corners[3], corners[2]
- corners.append(corners[0])
- map_corners = [
+ self._shortest_path_points = [
maps.to_grid(
p[0],
p[2],
self._coordinate_min,
self._coordinate_max,
self._map_resolution,
- )[::-1]
- for p in corners
+ )
+ for p in self._shortest_path_points
]
-
maps.draw_path(
self._top_down_map,
- map_corners,
- maps.MAP_TARGET_BOUNDING_BOX,
+ self._shortest_path_points,
+ maps.MAP_SHORTEST_PATH_COLOR,
self.line_thickness,
)
@@ -660,33 +687,20 @@ class TopDownMap(Measure):
self._map_resolution,
)
self._previous_xy_location = (a_y, a_x)
- if self._config.DRAW_SHORTEST_PATH:
- # draw shortest path
- self._shortest_path_points = self._sim.get_straight_shortest_path_points(
- agent_position, episode.goals[0].position
- )
- self._shortest_path_points = [
- maps.to_grid(
- p[0],
- p[2],
- self._coordinate_min,
- self._coordinate_max,
- self._map_resolution,
- )[::-1]
- for p in self._shortest_path_points
- ]
- maps.draw_path(
- self._top_down_map,
- self._shortest_path_points,
- maps.MAP_SHORTEST_PATH_COLOR,
- self.line_thickness,
- )
self.update_fog_of_war_mask(np.array([a_x, a_y]))
- # draw source and target points last to avoid overlap
- if self._config.DRAW_SOURCE_AND_TARGET:
- self.draw_source_and_target(episode)
+ # draw source and target parts last to avoid overlap
+ self._draw_goals_view_points(episode)
+ self._draw_goals_aabb(episode)
+ self._draw_goals_positions(episode)
+
+ self._draw_shortest_path(episode, agent_position)
+
+ if self._config.DRAW_SOURCE:
+ self._draw_point(
+ episode.start_position, maps.MAP_SOURCE_POINT_INDICATOR
+ )
def _clip_map(self, _map):
return _map[
diff --git a/habitat/tasks/nav/object_nav_task.py b/habitat/tasks/nav/object_nav_task.py
index d7adfdb1beccc978f0dd2ddb7abb1296e7bf085b..f44bdb93319bd0bf294f585a37695c0ce9e67423 100644
--- a/habitat/tasks/nav/object_nav_task.py
+++ b/habitat/tasks/nav/object_nav_task.py
@@ -138,7 +138,9 @@ class ObjectGoalSensor(Sensor):
elif self.config.GOAL_SPEC == "OBJECT_ID":
return np.array([episode.goals[0].object_name_id], dtype=np.int64)
else:
- return None
+ raise RuntimeError(
+ "Wrong GOAL_SPEC specified for ObjectGoalSensor."
+ )
@registry.register_task(name="ObjectNav-v0")
diff --git a/habitat/utils/visualizations/maps.py b/habitat/utils/visualizations/maps.py
index 0b98f026148e59dca8e38f06a8c6e1903f60edc3..b990333196c045448c4b02255548905fea80a2e2 100644
--- a/habitat/utils/visualizations/maps.py
+++ b/habitat/utils/visualizations/maps.py
@@ -37,6 +37,8 @@ MAP_BORDER_INDICATOR = 2
MAP_SOURCE_POINT_INDICATOR = 4
MAP_TARGET_POINT_INDICATOR = 6
MAP_SHORTEST_PATH_COLOR = 7
+MAP_VIEW_POINT_INDICATOR = 8
+MAP_TARGET_BOUNDING_BOX = 9
TOP_DOWN_MAP_COLORS = np.full((256, 3), 150, dtype=np.uint8)
TOP_DOWN_MAP_COLORS[10:] = cv2.applyColorMap(
np.arange(246, dtype=np.uint8), cv2.COLORMAP_JET
@@ -47,6 +49,8 @@ TOP_DOWN_MAP_COLORS[MAP_BORDER_INDICATOR] = [50, 50, 50]
TOP_DOWN_MAP_COLORS[MAP_SOURCE_POINT_INDICATOR] = [0, 0, 200]
TOP_DOWN_MAP_COLORS[MAP_TARGET_POINT_INDICATOR] = [200, 0, 0]
TOP_DOWN_MAP_COLORS[MAP_SHORTEST_PATH_COLOR] = [0, 200, 0]
+TOP_DOWN_MAP_COLORS[MAP_VIEW_POINT_INDICATOR] = [245, 150, 150]
+TOP_DOWN_MAP_COLORS[MAP_TARGET_BOUNDING_BOX] = [0, 175, 0]
def draw_agent(
@@ -375,4 +379,11 @@ def draw_path(
thickness: thickness of the path.
"""
for prev_pt, next_pt in zip(path_points[:-1], path_points[1:]):
- cv2.line(top_down_map, prev_pt, next_pt, color, thickness=thickness)
+ # Swapping x y
+ cv2.line(
+ top_down_map,
+ prev_pt[::-1],
+ next_pt[::-1],
+ color,
+ thickness=thickness,
+ )