From a9ebc1584b7c8fa54ec4da8800b71faebc9aaab9 Mon Sep 17 00:00:00 2001
From: Oleksandr <maksymets.o@gmail.com>
Date: Fri, 31 Jan 2020 10:51:17 -0800
Subject: [PATCH] Added geodesic distance for multiple goals (#290)

Added interface to calculate geodesic distance to multiple points. The functionality is super convenient for multi target navigation and speed up metrics calculation process.

We still support single point functionality through API and are fully backward compatible.
Switched existing code to multi goal function usage using a single element list.
---
 .circleci/config.yml                          |  1 +
 habitat/core/simulator.py                     |  9 ++++---
 .../datasets/pointnav/pointnav_generator.py   |  2 +-
 .../habitat_simulator/habitat_simulator.py    | 15 +++++++++---
 habitat/tasks/nav/shortest_path_follower.py   |  4 ++--
 habitat_baselines/common/environments.py      |  4 ++--
 test/test_habitat_env.py                      |  4 ++--
 test/test_habitat_sim.py                      | 24 +++++++++++++++++++
 test/test_mp3d_eqa.py                         |  6 +++--
 9 files changed, 54 insertions(+), 15 deletions(-)

diff --git a/.circleci/config.yml b/.circleci/config.yml
index b4c20110e..47ab27cfd 100644
--- a/.circleci/config.yml
+++ b/.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
diff --git a/habitat/core/simulator.py b/habitat/core/simulator.py
index bee5c2116..e074c295a 100644
--- a/habitat/core/simulator.py
+++ b/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
diff --git a/habitat/datasets/pointnav/pointnav_generator.py b/habitat/datasets/pointnav/pointnav_generator.py
index 1b276104b..1ad36698c 100644
--- a/habitat/datasets/pointnav/pointnav_generator.py
+++ b/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:
diff --git a/habitat/sims/habitat_simulator/habitat_simulator.py b/habitat/sims/habitat_simulator/habitat_simulator.py
index 3dff4ffdc..2487c97f6 100644
--- a/habitat/sims/habitat_simulator/habitat_simulator.py
+++ b/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,19 @@ 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)
+        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(
diff --git a/habitat/tasks/nav/shortest_path_follower.py b/habitat/tasks/nav/shortest_path_follower.py
index c678a5421..ada66edc3 100644
--- a/habitat/tasks/nav/shortest_path_follower.py
+++ b/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:
diff --git a/habitat_baselines/common/environments.py b/habitat_baselines/common/environments.py
index b7ffd48da..b7d43dd88 100644
--- a/habitat_baselines/common/environments.py
+++ b/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
 
diff --git a/test/test_habitat_env.py b/test/test_habitat_env.py
index 698734679..9890b9c91 100644
--- a/test/test_habitat_env.py
+++ b/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
diff --git a/test/test_habitat_sim.py b/test/test_habitat_sim.py
index 5fea24ca9..3875503a6 100644
--- a/test/test_habitat_sim.py
+++ b/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()
diff --git a/test/test_mp3d_eqa.py b/test/test_mp3d_eqa.py
index 41569dada..59d2cffbb 100644
--- a/test/test_mp3d_eqa.py
+++ b/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()
-- 
GitLab