diff --git a/.circleci/config.yml b/.circleci/config.yml
index 00a72d8ce46a3b2d828ab6653c1a480452955608..4cbbfa9a07dfa1f816e14624d05276f494a5e211 100644
--- a/.circleci/config.yml
+++ b/.circleci/config.yml
@@ -176,4 +176,4 @@ workflows:
install_and_test:
jobs:
- python_lint
- - install_and_test_ubuntu
\ No newline at end of file
+ - install_and_test_ubuntu
diff --git a/examples/shortest_path_follower_example.py b/examples/shortest_path_follower_example.py
index db781de5ddfb482dc15911752b968b2c5c5fb36b..b70c69db98eb2d66d88b9993c3a78d2721ea4deb 100644
--- a/examples/shortest_path_follower_example.py
+++ b/examples/shortest_path_follower_example.py
@@ -35,7 +35,9 @@ class SimpleRLEnv(habitat.RLEnv):
def draw_top_down_map(info, heading, output_size):
- top_down_map = maps.colorize_topdown_map(info["top_down_map"]["map"])
+ top_down_map = maps.colorize_topdown_map(
+ info["top_down_map"]["map"], info["top_down_map"]["fog_of_war_mask"]
+ )
original_map_size = top_down_map.shape[:2]
map_scale = np.array(
(1, original_map_size[1] * 1.0 / original_map_size[0])
@@ -59,8 +61,10 @@ def draw_top_down_map(info, heading, output_size):
def shortest_path_example(mode):
config = habitat.get_config(config_paths="configs/tasks/pointnav.yaml")
+ config.defrost()
config.TASK.MEASUREMENTS.append("TOP_DOWN_MAP")
config.TASK.SENSORS.append("HEADING_SENSOR")
+ config.freeze()
env = SimpleRLEnv(config=config)
goal_radius = env.episodes[0].goals[0].radius
if goal_radius is None:
diff --git a/habitat/config/default.py b/habitat/config/default.py
index 68634e3e44196792d580ea3a1b03b3bf0a9b2c91..8fd60e74baaa3f7ce6b2bbabfa2ec5c120d812f6 100644
--- a/habitat/config/default.py
+++ b/habitat/config/default.py
@@ -72,6 +72,10 @@ _C.TASK.TOP_DOWN_MAP.MAP_RESOLUTION = 1250
_C.TASK.TOP_DOWN_MAP.DRAW_SOURCE_AND_TARGET = 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
# -----------------------------------------------------------------------------
# # COLLISIONS MEASUREMENT
# -----------------------------------------------------------------------------
diff --git a/habitat/tasks/nav/nav_task.py b/habitat/tasks/nav/nav_task.py
index bc9f9aeef45449a5157761bb8444df6477588aeb..27b1451522f2c009e3161250725c33f9a505278e 100644
--- a/habitat/tasks/nav/nav_task.py
+++ b/habitat/tasks/nav/nav_task.py
@@ -24,7 +24,7 @@ from habitat.core.simulator import (
)
from habitat.core.utils import not_none_validator
from habitat.tasks.utils import cartesian_to_polar, quaternion_rotate_vector
-from habitat.utils.visualizations import maps
+from habitat.utils.visualizations import fog_of_war, maps
MAP_THICKNESS_SCALAR: int = 1250
@@ -450,6 +450,10 @@ class TopDownMap(Measure):
self._ind_x_max = range_x[-1]
self._ind_y_min = range_y[0]
self._ind_y_max = range_y[-1]
+
+ if self._config.FOG_OF_WAR.DRAW:
+ self._fog_of_war_mask = np.zeros_like(top_down_map)
+
return top_down_map
def draw_source_and_target(self, episode):
@@ -516,10 +520,23 @@ class TopDownMap(Measure):
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)
+ def _clip_map(self, _map):
+ return _map[
+ self._ind_x_min
+ - self._grid_delta : self._ind_x_max
+ + self._grid_delta,
+ self._ind_y_min
+ - self._grid_delta : self._ind_y_max
+ + self._grid_delta,
+ ]
+
def update_metric(self, episode, action):
self._step_count += 1
house_map, map_agent_x, map_agent_y = self.update_map(
@@ -528,17 +545,15 @@ class TopDownMap(Measure):
# Rather than return the whole map which may have large empty regions,
# only return the occupied part (plus some padding).
- house_map = house_map[
- self._ind_x_min
- - self._grid_delta : self._ind_x_max
- + self._grid_delta,
- self._ind_y_min
- - self._grid_delta : self._ind_y_max
- + self._grid_delta,
- ]
+ clipped_house_map = self._clip_map(house_map)
+
+ clipped_fog_of_war_map = None
+ if self._config.FOG_OF_WAR.DRAW:
+ clipped_fog_of_war_map = self._clip_map(self._fog_of_war_mask)
self._metric = {
- "map": house_map,
+ "map": clipped_house_map,
+ "fog_of_war_mask": clipped_fog_of_war_map,
"agent_map_coord": (
map_agent_x - (self._ind_x_min - self._grid_delta),
map_agent_y - (self._ind_y_min - self._grid_delta),
@@ -584,9 +599,24 @@ class TopDownMap(Measure):
thickness=thickness,
)
+ self.update_fog_of_war_mask(np.array([a_x, a_y]))
+
self._previous_xy_location = (a_y, a_x)
return self._top_down_map, a_x, a_y
+ def update_fog_of_war_mask(self, agent_position):
+ if self._config.FOG_OF_WAR.DRAW:
+ self._fog_of_war_mask = fog_of_war.reveal_fog_of_war(
+ self._top_down_map,
+ self._fog_of_war_mask,
+ agent_position,
+ self.get_polar_angle(),
+ fov=self._config.FOG_OF_WAR.FOV,
+ max_line_len=self._config.FOG_OF_WAR.VISIBILITY_DIST
+ * max(self._map_resolution)
+ / (self._coordinate_max - self._coordinate_min),
+ )
+
@registry.register_task(name="Nav-v0")
class NavigationTask(EmbodiedTask):
diff --git a/habitat/utils/visualizations/fog_of_war.py b/habitat/utils/visualizations/fog_of_war.py
new file mode 100644
index 0000000000000000000000000000000000000000..388acb0e387210e20e5acec583c80147adf08c52
--- /dev/null
+++ b/habitat/utils/visualizations/fog_of_war.py
@@ -0,0 +1,164 @@
+#!/usr/bin/env python3
+
+# Copyright (c) Facebook, Inc. and its affiliates.
+# This source code is licensed under the MIT license found in the
+# LICENSE file in the root directory of this source tree.
+
+import numba
+import numpy as np
+
+from habitat.utils.visualizations import maps
+
+
+@numba.jit(nopython=True)
+def bresenham_supercover_line(pt1, pt2):
+ r"""Line drawing algo based
+ on http://eugen.dedu.free.fr/projects/bresenham/
+ """
+
+ ystep, xstep = 1, 1
+
+ x, y = pt1
+ dx, dy = pt2 - pt1
+
+ if dy < 0:
+ ystep *= -1
+ dy *= -1
+
+ if dx < 0:
+ xstep *= -1
+ dx *= -1
+
+ line_pts = [[x, y]]
+
+ ddx, ddy = 2 * dx, 2 * dy
+ if ddx > ddy:
+ errorprev = dx
+ error = dx
+ for i in range(int(dx)):
+ x += xstep
+ error += ddy
+
+ if error > ddx:
+ y += ystep
+ error -= ddx
+ if error + errorprev < ddx:
+ line_pts.append([x, y - ystep])
+ elif error + errorprev > ddx:
+ line_pts.append([x - xstep, y])
+ else:
+ line_pts.append([x - xstep, y])
+ line_pts.append([x, y - ystep])
+
+ line_pts.append([x, y])
+
+ errorprev = error
+ else:
+ errorprev = dx
+ error = dx
+ for i in range(int(dy)):
+ y += ystep
+ error += ddx
+
+ if error > ddy:
+ x += xstep
+ error -= ddy
+ if error + errorprev < ddy:
+ line_pts.append([x - xstep, y])
+ elif error + errorprev > ddy:
+ line_pts.append([x, y - ystep])
+ else:
+ line_pts.append([x - xstep, y])
+ line_pts.append([x, y - ystep])
+
+ line_pts.append([x, y])
+
+ errorprev = error
+
+ return line_pts
+
+
+@numba.jit(nopython=True)
+def draw_fog_of_war_line(top_down_map, fog_of_war_mask, pt1, pt2):
+ r"""Draws a line on the fog_of_war_mask mask between pt1 and pt2
+ """
+
+ for pt in bresenham_supercover_line(pt1, pt2):
+ x, y = pt
+
+ if x < 0 or x >= fog_of_war_mask.shape[0]:
+ break
+
+ if y < 0 or y >= fog_of_war_mask.shape[1]:
+ break
+
+ if top_down_map[x, y] == maps.MAP_INVALID_POINT:
+ break
+
+ fog_of_war_mask[x, y] = 1
+
+
+@numba.jit(nopython=True)
+def _draw_loop(
+ top_down_map,
+ fog_of_war_mask,
+ current_point,
+ current_angle,
+ max_line_len,
+ angles,
+):
+ for angle in angles:
+ draw_fog_of_war_line(
+ top_down_map,
+ fog_of_war_mask,
+ current_point,
+ current_point
+ + max_line_len
+ * np.array(
+ [np.cos(current_angle + angle), np.sin(current_angle + angle)]
+ ),
+ )
+
+
+def reveal_fog_of_war(
+ top_down_map: np.ndarray,
+ current_fog_of_war_mask: np.ndarray,
+ current_point: np.ndarray,
+ current_angle: float,
+ fov: float = 90,
+ max_line_len: float = 100,
+) -> np.ndarray:
+ r"""Reveals the fog-of-war at the current location
+
+ This works by simply drawing lines from the agents current location
+ and stopping once a wall is hit
+
+ Args:
+ top_down_map: The current top down map. Used for respecting walls when revealing
+ current_fog_of_war_mask: The current fog-of-war mask to reveal the fog-of-war on
+ current_point: The current location of the agent on the fog_of_war_mask
+ current_angle: The current look direction of the agent on the fog_of_war_mask
+ fov: The feild of view of the agent
+ max_line_len: The maximum length of the lines used to reveal the fog-of-war
+
+ Returns:
+ The updated fog_of_war_mask
+ """
+ fov = np.deg2rad(fov)
+
+ # Set the angle step to a value such that delta_angle * max_line_len = 1
+ angles = np.arange(
+ -fov / 2, fov / 2, step=1.0 / max_line_len, dtype=np.float32
+ )
+
+ fog_of_war_mask = current_fog_of_war_mask.copy()
+ _draw_loop(
+ top_down_map,
+ fog_of_war_mask,
+ current_point,
+ current_angle,
+ max_line_len,
+ angles,
+ )
+
+ return fog_of_war_mask
diff --git a/habitat/utils/visualizations/maps.py b/habitat/utils/visualizations/maps.py
index 9f34c22b20f14db3463c17d8fb5e54a6d5f4a11c..714fd4e6355340ef24ade6476ebced6a0eeba851 100644
--- a/habitat/utils/visualizations/maps.py
+++ b/habitat/utils/visualizations/maps.py
@@ -327,14 +327,32 @@ def get_topdown_map(
return top_down_map
-def colorize_topdown_map(top_down_map: np.ndarray) -> np.ndarray:
+FOG_OF_WAR_COLOR_DESAT = np.array([[0.7], [1.0]])
+
+
+def colorize_topdown_map(
+ top_down_map: np.ndarray, fog_of_war_mask: Optional[np.ndarray] = None
+) -> np.ndarray:
r"""Convert the top down map to RGB based on the indicator values.
Args:
top_down_map: A non-colored version of the map.
+ fog_of_war_mask: A mask used to determine which parts of the
+ top_down_map are visible
+ Non-visible parts will be desaturated
Returns:
A colored version of the top-down map.
"""
- return TOP_DOWN_MAP_COLORS[top_down_map]
+ _map = TOP_DOWN_MAP_COLORS[top_down_map]
+
+ if fog_of_war_mask is not None:
+ # Only desaturate things that are valid points as only valid points get revealed
+ desat_mask = top_down_map != MAP_INVALID_POINT
+
+ _map[desat_mask] = (
+ _map * FOG_OF_WAR_COLOR_DESAT[fog_of_war_mask]
+ ).astype(np.uint8)[desat_mask]
+
+ return _map
def draw_path(
diff --git a/requirements.txt b/requirements.txt
index d4aedf5cbe3934572e3919889be8648b663f266e..a95a0d5abb9247bbd20cede63e0175029f83d2aa 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -9,3 +9,4 @@ imageio>=2.2.0
imageio-ffmpeg>=0.2.0
scipy>=1.0.0
tqdm>=4.0.0
+numba>=0.44.0