Recompute navmesh programmatically (#333)

* Adds navmesh recomputation without scene file reload

* Added recomputeNavMesh function to Simulator in C++ and exposed as recompute_navmesh in python.

* Added recomputeNavMesh function to viewer demo in C++

* NavMeshSettings object exposed to python.

* Added python CI test for navmesh recomputation: test_recompute_navmesh

habitat_sim/nav/__init__.py

@@ -3,6 +3,7 @@
     GreedyGeodesicFollowerImpl,
     HitRecord,
     MultiGoalShortestPath,
+    NavMeshSettings,
     PathFinder,
     ShortestPath,
     VectorGreedyCodes,
@@ -15,6 +16,7 @@
     "GreedyGeodesicFollowerImpl",
     "GreedyFollowerCodes",
     "MultiGoalShortestPath",
+    "NavMeshSettings",
     "PathFinder",
     "ShortestPath",
     "HitRecord",

habitat_sim/simulator.py

@@ -288,6 +288,9 @@ def apply_torque(self, torque, object_id, scene_id=0):
     def get_world_time(self, scene_id=0):
         return self._sim.get_world_time()
 
+    def recompute_navmesh(self, pathfinder, navmesh_settings):
+        return self._sim.recompute_navmesh(pathfinder, navmesh_settings)
+
 
 class Sensor:
     r"""Wrapper around habitat_sim.Sensor

src/esp/bindings/GfxBindings.cpp

@@ -193,7 +193,9 @@ void initGfxBindings(py::module& m) {
       .def("apply_force", &Simulator::applyForce, "force"_a,
            "relative_position"_a, "object_id"_a, "sceneID"_a = 0)
       .def("apply_torque", &Simulator::applyTorque, "torque"_a, "object_id"_a,
-           "sceneID"_a = 0);
+           "sceneID"_a = 0)
+      .def("recompute_navmesh", &Simulator::recomputeNavMesh, "pathfinder"_a,
+           "navmesh_settings"_a);
 }
 
 }  // namespace gfx

src/esp/bindings/ShortestPathBindings.cpp

@@ -45,6 +45,29 @@ void initShortestPathBindings(py::module& m) {
       .def_readwrite("geodesic_distance",
                      &MultiGoalShortestPath::geodesicDistance);
 
+  py::class_<NavMeshSettings, NavMeshSettings::ptr>(m, "NavMeshSettings")
+      .def(py::init(&NavMeshSettings::create<>))
+      .def_readwrite("cell_size", &NavMeshSettings::cellSize)
+      .def_readwrite("cell_height", &NavMeshSettings::cellHeight)
+      .def_readwrite("agent_height", &NavMeshSettings::agentHeight)
+      .def_readwrite("agent_radius", &NavMeshSettings::agentRadius)
+      .def_readwrite("agent_max_climb", &NavMeshSettings::agentMaxClimb)
+      .def_readwrite("agent_max_slope", &NavMeshSettings::agentMaxSlope)
+      .def_readwrite("region_min_size", &NavMeshSettings::regionMinSize)
+      .def_readwrite("region_merge_size", &NavMeshSettings::regionMergeSize)
+      .def_readwrite("edge_max_len", &NavMeshSettings::edgeMaxLen)
+      .def_readwrite("edge_max_error", &NavMeshSettings::edgeMaxError)
+      .def_readwrite("verts_per_poly", &NavMeshSettings::vertsPerPoly)
+      .def_readwrite("detail_sample_dist", &NavMeshSettings::detailSampleDist)
+      .def_readwrite("detail_sample_max_error",
+                     &NavMeshSettings::detailSampleMaxError)
+      .def_readwrite("filter_low_hanging_obstacles",
+                     &NavMeshSettings::filterLowHangingObstacles)
+      .def_readwrite("filter_ledge_spans", &NavMeshSettings::filterLedgeSpans)
+      .def_readwrite("filter_walkable_low_height_spans",
+                     &NavMeshSettings::filterWalkableLowHeightSpans)
+      .def("set_defaults", &NavMeshSettings::setDefaults);
+
   py::class_<PathFinder, PathFinder::ptr>(m, "PathFinder")
       .def(py::init(&PathFinder::create<>))
       .def("get_bounds", &PathFinder::bounds)

src/esp/gfx/Simulator.cpp

@@ -350,5 +350,19 @@ double Simulator::getWorldTime() {
   return NO_TIME;
 }
 
+bool Simulator::recomputeNavMesh(nav::PathFinder& pathfinder,
+                                 const nav::NavMeshSettings& navMeshSettings) {
+  assets::MeshData::uptr joinedMesh =
+      resourceManager_.createJoinedCollisionMesh(config_.scene.id);
+
+  if (!pathfinder.build(navMeshSettings, *joinedMesh)) {
+    LOG(ERROR) << "Failed to build navmesh";
+    return false;
+  }
+
+  LOG(INFO) << "reconstruct navmesh successful";
+  return true;
+}
+
 }  // namespace gfx
 }  // namespace esp

src/esp/gfx/Simulator.h

@@ -18,6 +18,7 @@
 namespace esp {
 namespace nav {
 class PathFinder;
+class NavMeshSettings;
 class ActionSpacePathFinder;
 }  // namespace nav
 namespace scene {
@@ -251,9 +252,6 @@ class Simulator {
    */
   Magnum::Quaternion getRotation(const int objectID, const int sceneID = 0);
 
-  // the physical world has a notion of time which passes during
-  // animation/simulation/action/etc... return the new world time after stepping
-
   /**
    * @brief the physical world has a notion of time which passes during
    * animation/simulation/action/etc... Step the physical world forward in time
@@ -267,7 +265,6 @@ class Simulator {
    */
   double stepWorld(const double dt = 1.0 / 60.0);
 
-  // get the simulated world time (0 if no physics enabled)
   /**
    * @brief Get the current time in the simulated world. This is always 0 if no
    * @ref esp::physics::PhysicsManager is initialized. See @ref stepWorld. See
@@ -277,6 +274,18 @@ class Simulator {
    */
   double getWorldTime();
 
+  /**
+   * @brief Compute the navmesh for the simulator's current active scene and
+   * assign it to the referenced @ref nav::PathFinder.
+   * @param pathfinder The pathfinder object to which the recomputed navmesh
+   * will be assigned.
+   * @param navMeshSettings The @ref nav::NavMeshSettings instance to
+   * parameterize the navmesh construction.
+   * @return Whether or not the navmesh recomputation succeeded.
+   */
+  bool recomputeNavMesh(nav::PathFinder& pathfinder,
+                        const nav::NavMeshSettings& navMeshSettings);
+
  protected:
   Simulator(){};
 

src/esp/nav/PathFinder.h

@@ -103,6 +103,8 @@ struct NavMeshSettings {
     filterLedgeSpans = true;
     filterWalkableLowHeightSpans = true;
   }
+
+  ESP_SMART_POINTERS(NavMeshSettings)
 };
 
 class PathFinder : public std::enable_shared_from_this<PathFinder> {

src/tests/ResourceManagerTest.cpp

@@ -40,7 +40,7 @@ TEST(ResourceManagerTest, createJoinedCollisionMesh) {
   const esp::assets::AssetInfo info = esp::assets::AssetInfo::fromPath(boxFile);
   resourceManager.loadScene(info, navSceneNode, nullptr);
 
-  std::unique_ptr<esp::assets::MeshData> joinedBox =
+  esp::assets::MeshData::uptr joinedBox =
       resourceManager.createJoinedCollisionMesh(boxFile);
 
   // transform_box.glb is composed of 6 identical triangulated plane meshes

src/utils/viewer/viewer.cpp

@@ -69,6 +69,9 @@ class Viewer : public Magnum::Platform::Application {
   void pokeLastObject();
   void pushLastObject();
 
+  void recomputeNavMesh(const std::string& sceneFilename,
+                        esp::nav::NavMeshSettings& navMeshSettings);
+
   void torqueLastObject();
   void removeLastObject();
   void invertGravity();
@@ -132,8 +135,11 @@ Viewer::Viewer(const Arguments& arguments)
       .setGlobalHelp("Displays a 3D scene file provided on command line")
       .addBooleanOption("enable-physics")
       .addBooleanOption("debug-bullet")
+      .setHelp("debug-bullet", "render Bullet physics debug wireframes")
       .addOption("physics-config", ESP_DEFAULT_PHYS_SCENE_CONFIG)
       .setHelp("physics-config", "physics scene config file")
+      .addBooleanOption("recompute-navmesh")
+      .setHelp("recompute-navmesh", "programmatically generate scene navmesh")
       .parse(arguments.argc, arguments.argv);
 
   const auto viewportSize = GL::defaultFramebuffer.viewport().size();
@@ -201,13 +207,16 @@ Viewer::Viewer(const Arguments& arguments)
       Magnum::SceneGraph::AspectRatioPolicy::Extend);
 
   // Load navmesh if available
-  const std::string navmeshFilename = io::changeExtension(file, ".navmesh");
-  if (io::exists(navmeshFilename)) {
-    LOG(INFO) << "Loading navmesh from " << navmeshFilename;
-    pathfinder_->loadNavMesh(navmeshFilename);
-
-    const vec3f position = pathfinder_->getRandomNavigablePoint();
-    agentBodyNode_->setTranslation(Vector3(position));
+  if (file.compare(esp::assets::EMPTY_SCENE) != 0) {
+    const std::string navmeshFilename = io::changeExtension(file, ".navmesh");
+    if (io::exists(navmeshFilename) && !args.isSet("recompute-navmesh")) {
+      LOG(INFO) << "Loading navmesh from " << navmeshFilename;
+      pathfinder_->loadNavMesh(navmeshFilename);
+    } else {
+      esp::nav::NavMeshSettings navMeshSettings;
+      navMeshSettings.setDefaults();
+      recomputeNavMesh(file, navMeshSettings);
+    }
   }
 
   // connect controls to navmesh if loaded
@@ -302,6 +311,22 @@ void Viewer::pushLastObject() {
   physicsManager_->applyForce(objectIDs_.back(), force, rel_pos);
 }
 
+void Viewer::recomputeNavMesh(const std::string& sceneFilename,
+                              nav::NavMeshSettings& navMeshSettings) {
+  nav::PathFinder::ptr pf = nav::PathFinder::create();
+
+  assets::MeshData::uptr joinedMesh =
+      resourceManager_.createJoinedCollisionMesh(sceneFilename);
+
+  if (!pf->build(navMeshSettings, *joinedMesh)) {
+    LOG(ERROR) << "Failed to build navmesh";
+    return;
+  }
+
+  LOG(INFO) << "reconstruct navmesh successful";
+  pathfinder_ = pf;
+}
+
 void Viewer::torqueLastObject() {
   if (physicsManager_ == nullptr || objectIDs_.size() == 0)
     return;

tests/test_recompute_navmesh.py

@@ -0,0 +1,89 @@
+import glob
+import os
+import os.path as osp
+
+import numpy as np
+import pytest
+import tqdm
+
+import examples.settings
+import habitat_sim
+
+EPS = 1e-5
+
+base_dir = osp.abspath(osp.join(osp.dirname(__file__), ".."))
+
+test_scenes = [
+    osp.join(base_dir, "data/scene_datasets/mp3d/17DRP5sb8fy/17DRP5sb8fy.glb"),
+    osp.join(base_dir, "data/scene_datasets/habitat-test-scenes/skokloster-castle.glb"),
+    osp.join(base_dir, "data/scene_datasets/habitat-test-scenes/van-gogh-room.glb"),
+]
+
+
+def get_shortest_path(sim, samples):
+    path_results = []
+
+    for sample in samples:
+        path = habitat_sim.ShortestPath()
+        path.requested_start = sample[0]
+        path.requested_end = sample[1]
+        found_path = sim.pathfinder.find_path(path)
+        path_results.append((found_path, path.geodesic_distance, path.points))
+
+    return path_results
+
+
+@pytest.mark.parametrize("test_scene", test_scenes)
+def test_recompute_navmesh(test_scene, sim):
+    if not osp.exists(test_scene):
+        pytest.skip(f"{test_scene} not found")
+
+    cfg_settings = examples.settings.default_sim_settings.copy()
+    cfg_settings["scene"] = test_scene
+    hab_cfg = examples.settings.make_cfg(cfg_settings)
+    sim.reconfigure(hab_cfg)
+
+    # generate random point pairs
+    num_samples = 100
+    samples = []
+    for _ in range(num_samples):
+        samples.append(
+            (
+                sim.pathfinder.get_random_navigable_point(),
+                sim.pathfinder.get_random_navigable_point(),
+            )
+        )
+
+    # compute shortest paths between these points on the loaded navmesh
+    loaded_navmesh_path_results = get_shortest_path(sim, samples)
+
+    navmesh_settings = habitat_sim.NavMeshSettings()
+    navmesh_settings.set_defaults()
+    assert sim.recompute_navmesh(sim.pathfinder, navmesh_settings)
+    assert sim.pathfinder.is_loaded
+
+    recomputed_navmesh_results = get_shortest_path(sim, samples)
+
+    navmesh_settings.agent_radius *= 2.0
+    assert sim.recompute_navmesh(sim.pathfinder, navmesh_settings)
+    assert sim.pathfinder.is_loaded  # this may not always be viable...
+
+    recomputed_2rad_navmesh_results = get_shortest_path(sim, samples)
+
+    some_diff = False
+    for i in range(num_samples):
+        assert loaded_navmesh_path_results[i][0] == recomputed_navmesh_results[i][0]
+        if loaded_navmesh_path_results[i][0]:
+            assert (
+                loaded_navmesh_path_results[i][1] - recomputed_navmesh_results[i][1]
+                < EPS
+            )
+
+        if (
+            loaded_navmesh_path_results[i][0] != recomputed_2rad_navmesh_results[i][0]
+            or loaded_navmesh_path_results[i][1] - recomputed_2rad_navmesh_results[i][1]
+            > EPS
+        ):
+            some_diff = True
+
+    assert some_diff