Added geodesic distance for multiple goals

.circleci/config.yml

@@ -186,6 +186,7 @@ jobs:
             - ~/miniconda
       - run:
           name: Run api tests
+          no_output_timeout: 20m
           command: |
               export PATH=$HOME/miniconda/bin:/usr/local/cuda/bin:$PATH
               . activate habitat; cd habitat-api

habitat/core/simulator.py

@@ -6,7 +6,7 @@
 
 from collections import OrderedDict
 from enum import Enum
-from typing import Any, Dict, Iterable, List, Optional
+from typing import Any, Dict, Iterable, List, Optional, Union
 
 import attr
 from gym import Space
@@ -265,12 +265,15 @@ class Simulator:
         raise NotImplementedError
 
     def geodesic_distance(
-        self, position_a: List[float], position_b: List[float]
+        self,
+        position_a: List[float],
+        position_b: Union[List[float], List[List[float]]],
     ) -> float:
         r"""Calculates geodesic distance between two points.
 
         :param position_a: coordinates of first point.
-        :param position_b: coordinates of second point.
+        :param position_b: coordinates of second point or list of goal points
+        coordinates.
         :return:
             the geodesic distance in the cartesian space between points
             :p:`position_a` and :p:`position_b`, if no path is found between

habitat/datasets/pointnav/pointnav_generator.py

@@ -40,7 +40,7 @@ def is_compatible_episode(
     if np.abs(s[1] - t[1]) > 0.5:  # check height difference to assure s and
         #  t are from same floor
         return False, 0
-    d_separation = sim.geodesic_distance(s, t)
+    d_separation = sim.geodesic_distance(s, [t])
     if d_separation == np.inf:
         return False, 0
     if not near_dist <= d_separation <= far_dist:

habitat/sims/habitat_simulator/habitat_simulator.py

@@ -4,7 +4,7 @@
 # This source code is licensed under the MIT license found in the
 # LICENSE file in the root directory of this source tree.
 
-from typing import Any, List, Optional
+from typing import Any, List, Optional, Union
 
 import numpy as np
 from gym import spaces
@@ -281,10 +281,27 @@ class HabitatSim(Simulator):
         self._update_agents_state()
 
     def geodesic_distance(self, position_a, position_b):
-        path = habitat_sim.ShortestPath()
+        path = habitat_sim.MultiGoalShortestPath()
         path.requested_start = np.array(position_a, dtype=np.float32)
-        path.requested_end = np.array(position_b, dtype=np.float32)
+        if isinstance(position_b[0], List) or isinstance(
+            position_b[0], np.ndarray
+        ):
+            path.requested_ends = np.array(position_b, dtype=np.float32)
+            # TODO(erikwijmans) Remove next line as soon as multi goal shortest
+            # path passes the tests.
+            return np.min(
+                [
+                    self.geodesic_distance(position_a, position)
+                    for position in position_b
+                ]
+            )
+        else:
+            path.requested_ends = np.array(
+                [np.array(position_b, dtype=np.float32)]
+            )
+
         self._sim.pathfinder.find_path(path)
+
         return path.geodesic_distance
 
     def action_space_shortest_path(

habitat/tasks/nav/shortest_path_follower.py

@@ -72,7 +72,7 @@ class ShortestPathFollower:
         """
         if (
             self._sim.geodesic_distance(
-                self._sim.get_agent_state().position, goal_pos
+                self._sim.get_agent_state().position, [goal_pos]
             )
             <= self._goal_radius
         ):
@@ -113,7 +113,7 @@ class ShortestPathFollower:
 
     def _geo_dist(self, goal_pos: np.array) -> float:
         return self._sim.geodesic_distance(
-            self._sim.get_agent_state().position, goal_pos
+            self._sim.get_agent_state().position, [goal_pos]
         )
 
     def _est_max_grad_dir(self, goal_pos: np.array) -> np.array:

habitat_baselines/common/environments.py

@@ -75,9 +75,9 @@ class NavRLEnv(habitat.RLEnv):
 
     def _distance_target(self):
         current_position = self._env.sim.get_agent_state().position.tolist()
-        target_position = self._env.current_episode.goals[0].position
         distance = self._env.sim.geodesic_distance(
-            current_position, target_position
+            current_position,
+            [goal.position for goal in self._env.current_episode.goals],
         )
         return distance
 

test/test_habitat_env.py

@@ -390,7 +390,7 @@ def test_action_space_shortest_path():
         angles = [x for x in range(-180, 180, config.SIMULATOR.TURN_ANGLE)]
         angle = np.radians(np.random.choice(angles))
         rotation = [0, np.sin(angle / 2), 0, np.cos(angle / 2)]
-        if env.sim.geodesic_distance(source_position, position) != np.inf:
+        if env.sim.geodesic_distance(source_position, [position]) != np.inf:
             reachable_targets.append(AgentState(position, rotation))
 
     while len(unreachable_targets) < 3:
@@ -400,7 +400,7 @@ def test_action_space_shortest_path():
         angles = [x for x in range(-180, 180, config.SIMULATOR.TURN_ANGLE)]
         angle = np.radians(np.random.choice(angles))
         rotation = [0, np.sin(angle / 2), 0, np.cos(angle / 2)]
-        if env.sim.geodesic_distance(source_position, position) == np.inf:
+        if env.sim.geodesic_distance(source_position, [position]) == np.inf:
             unreachable_targets.append(AgentState(position, rotation))
 
     targets = reachable_targets

test/test_habitat_sim.py

@@ -82,3 +82,27 @@ def test_sim_no_sensors():
     sim = make_sim(config.SIMULATOR.TYPE, config=config.SIMULATOR)
     sim.reset()
     sim.close()
+
+
+def test_sim_geodesic_distance():
+    config = get_config()
+    if not os.path.exists(config.SIMULATOR.SCENE):
+        pytest.skip("Please download Habitat test data to data folder.")
+    sim = make_sim(config.SIMULATOR.TYPE, config=config.SIMULATOR)
+    sim.seed(0)
+    sim.reset()
+    start_point = sim.sample_navigable_point()
+    navigable_points = [sim.sample_navigable_point() for _ in range(10)]
+    assert np.isclose(
+        sim.geodesic_distance(start_point, navigable_points[0]), 1.3849650
+    ), "Geodesic distance or sample navigable points mechanism has been changed."
+    assert np.isclose(
+        sim.geodesic_distance(start_point, navigable_points), 0.6194838
+    ), "Geodesic distance or sample navigable points mechanism has been changed."
+    assert sim.geodesic_distance(start_point, navigable_points) == np.min(
+        [
+            sim.geodesic_distance(start_point, position)
+            for position in navigable_points
+        ]
+    ), "Geodesic distance for multi target setup isn't equal to separate single target calls."
+    sim.close()

test/test_mp3d_eqa.py

@@ -16,11 +16,12 @@ from habitat.core.embodied_task import Episode
 from habitat.core.logging import logger
 from habitat.datasets import make_dataset
 from habitat.tasks.eqa.eqa import AnswerAction
-from habitat.tasks.nav.nav import MoveForwardAction
+from habitat.tasks.nav.nav import MoveForwardAction, StopAction
 from habitat.utils.test_utils import sample_non_stop_action
 
 CFG_TEST = "configs/test/habitat_mp3d_eqa_test.yaml"
 CLOSE_STEP_THRESHOLD = 0.028
+OLD_STOP_ACTION_ID = 3
 
 
 # List of episodes each from unique house
@@ -207,7 +208,8 @@ def test_mp3d_eqa_sim_correspondence():
                 atol=CLOSE_STEP_THRESHOLD * (step_id + 1),
             ), "Agent's path diverges from the shortest path."
 
-            obs = env.step(action=point.action)
+            if point.action != OLD_STOP_ACTION_ID:
+                obs = env.step(action=point.action)
 
             if not env.episode_over:
                 rgb_mean += obs["rgb"][:, :, :3].mean()