Port/traversability #101

Schmluk · 2025-09-11T14:43:21Z

Adds infrastructure for traversability estimation and place extraction. This largely does not touch existing Hydra.
Depends on Spark-DSG#64

Parts affecting Hydra:

Factored out the node deformation interpolation so several updaters can use it. These params are never set so the configs remain unchanged.
Adds reset DSG on loopclosure flag to the DSG updater, default behavior is as before
Various small patches for Places2d and Frontiers to avoid segfaults
For now added sane traversability params to the uHumans config. No preference whether they should be in or not.

Tests:

Builds and tests pass
Tested on uHumans

nathanhhughes

LGTM!

nathanhhughes · 2025-09-14T15:10:48Z

src/rooms/room_finder_config.cpp

 #include <config_utilities/config.h>
 #include <config_utilities/types/conversions.h>
 #include <config_utilities/types/enum.h>
+#include <config_utilities/validation.h>


(minor) not needed here

src/utils/nearest_neighbor_utilities.cpp

nathanhhughes · 2025-09-14T15:16:45Z

tests/CMakeLists.txt

+  src/place_fixtures.cpp
+  src/resources.cpp


(minor) idk if you're using an automatic alphabetical sorter or doing it by hand, but main.cpp and src/* are slightly distinct to the actual test files, so I'd have a slight preference to keeping them at the top

Thank you for cleaning up the order on the files though!

This was alphabetical sort, the intention was to keep main at the top though 😅

nathanhhughes · 2025-09-14T15:18:33Z

include/hydra/frontend/graph_builder.h

    bool enable_place_mesh_mapping = false;
    config::VirtualConfig<SurfacePlacesInterface> surface_places;
    config::VirtualConfig<FreespacePlacesInterface> freespace_places;
-    bool use_frontiers = false;


Good catch, thanks!

nathanhhughes · 2025-09-15T12:52:12Z

src/backend/deformation_interpolator.cpp

+    auto node_ptr = dsg.findNode(node.id);
+    if (node_ptr) {
+      node_ptr->attributes().position = node.attributes().position;
+    }
+    // dsg.setNodeAttributes(node.id, node.attributes().clone());


Can we drop the commented code and add a note that this is copying the newly interpolated positions from the unmerged graph to the merged graph? I probably should have added the note myself when I wrote this (took me a minute to refresh my memory as to what the implementation was supposed to do)

nathanhhughes · 2025-09-15T13:03:59Z

include/hydra/places/traversability_clustering.h

+  inline static const auto registration_ =
+      config::RegistrationWithConfig<TraversabilityClustering,
+                                     BlockTraversabilityClustering,
+                                     Config>("BlockTraversabilityClustering");


Ditto regarding where the registration is defined

nathanhhughes · 2025-09-15T13:04:16Z

include/hydra/frontend/traversability_place_extractor.h

+  inline static const auto registration_ =
+      config::RegistrationWithConfig<SurfacePlacesInterface,
+                                     TraversabilityPlaceExtractor,
+                                     Config>("traversability");


Ditto for registration definition location

nathanhhughes · 2025-09-15T13:05:20Z

include/hydra/places/traversability_postprocessing.h

+  inline static const auto registration_ =
+      config::RegistrationWithConfig<TraversabilityProcessor, ErosionDilation, Config>(
+          "ErosionDilation");


Ditto for registration location

nathanhhughes · 2025-09-15T13:08:22Z

src/backend/update_traversability_functor.cpp

+      auto& to_attrs = dsg.getNode(to_id).attributes<TraversabilityNodeAttributes>();
+      // Avoid merging nodes with active window (temporal) overlap.
+      if (from_attrs.last_observed_ns >= to_attrs.first_observed_ns &&
+          from_attrs.first_observed_ns <= to_attrs.last_observed_ns) {
+        continue;
+      }
+
+      // Check boundaries.
+      auto to_boundary = Boundary(to_attrs);
+      if (!to_boundary.intersects(from_boundary)) {
+        continue;
+      }
+
+      // Simple case: If places are completely contained.
+      if (isContained(to_boundary, from_boundary)) {
+        result.push_back({to_id, from_node.id});
+        merged.insert(to_id);
+        continue;
+      }
+      if (isContained(from_boundary, to_boundary)) {
+        result.push_back({from_node.id, to_id});
+        merged.insert(from_node.id);
+        continue;
+      }
+
+      // Large overlap: No need to keep both, as transition is always possible between.
+      auto intersection = from_boundary.intersection(to_boundary);
+      if (intersection.height() < config.min_place_size ||
+          intersection.width() < config.min_place_size) {
+        // TODO(lschmid): Can consider cropping and dropping in the future.
+        // Always update the classification of the boundary sides.
+        overlapping_nodes_to_cleanup_.insert({from_node.id, to_id});
+        continue;
+      }
+
+      // Aligned in X or Y direction: Merge by fusing boundary.
+      if ((std::abs(from_boundary.min.x() - to_boundary.min.x()) < config.tolerance &&
+           std::abs(from_boundary.max.x() - to_boundary.max.x()) < config.tolerance) ||
+          (std::abs(from_boundary.min.y() - to_boundary.min.y()) < config.tolerance &&
+           std::abs(from_boundary.max.y() - to_boundary.max.y()) < config.tolerance)) {
+        merged.insert(to_id);
+        result.push_back({to_id, from_node.id});
+        continue;
+      }
+
+      // If not fuseable easily keep larger.
+      if (from_boundary.area() >= to_boundary.area()) {
+        result.push_back({to_id, from_node.id});
+        merged.insert(to_id);
+      } else {
+        result.push_back({from_node.id, to_id});
+        merged.insert(from_node.id);
+      }
+    }


(minor) I'd maybe split this up into a helper function or two, but definitely not needed

I was hoping to make this clearer but I can't think of a great way to break this up, since essentially the 'how to fuse traversability attrs' is a bit of cases/branching problem.

nathanhhughes · 2025-09-15T13:08:56Z

src/backend/update_traversability_functor.cpp

+    if ((std::abs(from_boundary.min.x() - to_boundary.min.x()) < config.tolerance &&
+         std::abs(from_boundary.max.x() - to_boundary.max.x()) < config.tolerance) ||
+        (std::abs(from_boundary.min.y() - to_boundary.min.y()) < config.tolerance &&
+         std::abs(from_boundary.max.y() - to_boundary.max.y()) < config.tolerance)) {


(minor) this feels like something that could be a method in the boundary

…passes tests

Co-authored-by: Nathan Hughes <nathan.h.hughes@gmail.com>

* WIP: part 1 * Port traversability: Part 2/2 * updates to new hydra architecture and few missed updates, builds and passes tests * update gitignore * minor fixes and config * run linting * fix missing distance adaptor call * Update src/utils/nearest_neighbor_utilities.cpp Co-authored-by: Nathan Hughes <nathan.h.hughes@gmail.com> * address review comments * run linter --------- Co-authored-by: Nathan Hughes <nathan.h.hughes@gmail.com>

Schmluk requested a review from nathanhhughes September 11, 2025 14:43

Schmluk mentioned this pull request Sep 11, 2025

Port/traversability MIT-SPARK/Hydra-ROS#35

Merged

nathanhhughes approved these changes Sep 15, 2025

View reviewed changes

nathanhhughes force-pushed the develop branch from 424cec2 to a087977 Compare September 15, 2025 19:10

Schmluk and others added 10 commits September 15, 2025 15:57

WIP: part 1

72d696a

Port traversability: Part 2/2

4ddf10b

updates to new hydra architecture and few missed updates, builds and …

e7d7108

…passes tests

update gitignore

aad28be

minor fixes and config

2d7c687

run linting

5d9f0f0

fix missing distance adaptor call

7852fd9

Update src/utils/nearest_neighbor_utilities.cpp

ae136c9

Co-authored-by: Nathan Hughes <nathan.h.hughes@gmail.com>

address review comments

806ccd8

run linter

e3e3474

Schmluk force-pushed the port/traversability branch from 1a26912 to e3e3474 Compare September 15, 2025 19:57

Schmluk merged commit d2dd781 into develop Sep 15, 2025
2 checks passed

Schmluk deleted the port/traversability branch September 15, 2025 20:38

nathanhhughes mentioned this pull request Oct 8, 2025

Feature/deformation interpolators #109

Merged

Port/traversability #101

Port/traversability #101

Uh oh!

Conversation

Schmluk commented Sep 11, 2025

Uh oh!

nathanhhughes left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants