From ec6c1df14ea8d83f0e81eeb52823bfc7b33c4b6e Mon Sep 17 00:00:00 2001
From: Oleksandr Maksymets <maksymets@gmail.com>
Date: Tue, 28 Jan 2020 16:50:19 -0800
Subject: [PATCH] Added geodesic distance for multiple goals
---
.circleci/config.yml | 1 +
habitat/core/simulator.py | 9 ++++---
.../datasets/pointnav/pointnav_generator.py | 2 +-
.../habitat_simulator/habitat_simulator.py | 23 +++++++++++++++---
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, 62 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..fa3edb6db 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,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(
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