Prevent traversability places from leaking through walls (configurable 4-connectivity + minimum passage width)

include/hydra/places/region_growing_traversability_clustering.h

@@ -53,6 +53,8 @@ class RegionGrowingTraversabilityClustering : public TraversabilityClustering {
     float max_radius = 3.0f;
     //! Number of rays to consider for boundary computation.
     int num_orientation_bins = 16;
+    //! Toggle between 8-connectivity and 4-connectivity for region growing.
+    bool use_diagonal_connectivity = true;
   } const config;
 
   using Voxels = VoxelIndices;
@@ -115,6 +117,18 @@ class RegionGrowingTraversabilityClustering : public TraversabilityClustering {
                    spark_dsg::SceneGraph& graph,
                    const std::string& layer_name) override;
 
+  /**
+   * @brief Breadth-first search to grow a region from a seed index.
+   * @param num_neighbors How many neighbors to consider during region growing.
+   */
+  static VoxelSet growRegion(
+      const VoxelSet& candidates,
+      const VoxelIndex& seed_index,
+      size_t num_neighbors = 8,
+      std::function<bool(const VoxelIndex&)> condition = [](const VoxelIndex&) {
+        return true;
+      });
+
  protected:
   size_t current_id_ = 0;
   uint64_t current_time_ns_ = 0;
@@ -166,16 +180,6 @@ class RegionGrowingTraversabilityClustering : public TraversabilityClustering {
    */
   Region& allocateNewRegion();
 
-  /**
-   * @brief Breadth-first search to grow a region from a seed index.
-   */
-  static VoxelSet growRegion(
-      const VoxelSet& candidates,
-      const VoxelIndex& seed_index,
-      std::function<bool(const VoxelIndex&)> condition = [](const VoxelIndex&) {
-        return true;
-      });
-
   void updatePlaceNodeAttributes(spark_dsg::TravNodeAttributes& attrs,
                                  Region& region,
                                  const TraversabilityLayer& layer) const;

src/places/region_growing_traversability_clustering.cpp

@@ -71,6 +71,7 @@ void declare_config(RegionGrowingTraversabilityClustering::Config& config) {
   name("RegionGrowingTraversabilityClustering::Config");
   field(config.max_radius, "max_radius", "m");
   field(config.num_orientation_bins, "num_orientation_bins");
+  field(config.use_diagonal_connectivity, "use_diagonal_connectivity");
   check(config.max_radius, GT, 0.0f, "max_radius");
   check(config.num_orientation_bins, GE, 3, "num_orientation_bins");
 }
@@ -130,7 +131,8 @@ VoxelSet RegionGrowingTraversabilityClustering::initializeVoxels(
   Eigen::Vector3f start_2d = start_position.cast<float>();
   start_2d.z() = 0.0f;
   const auto start_index = layer.globalIndexFromPoint(start_2d);
-  return growRegion(candidates, start_index);
+  const size_t num_neighbors = config.use_diagonal_connectivity ? 8 : 4;
+  return growRegion(candidates, start_index, num_neighbors);
 }
 
 VoxelMap RegionGrowingTraversabilityClustering::initializeRegions(
@@ -160,6 +162,8 @@ VoxelMap RegionGrowingTraversabilityClustering::initializeRegions(
 
 void RegionGrowingTraversabilityClustering::growRegions(VoxelSet& all_voxels,
                                                         VoxelMap& assigned_voxels) {
+  const size_t num_neighbors = config.use_diagonal_connectivity ? 8 : 4;
+
   // Try to grow existing regions into the closest voxels to each region.
   const auto max_dist_sq = static_cast<float>(max_region_size_ * max_region_size_);
   std::vector<Eigen::Vector3f> centroids;
@@ -185,7 +189,7 @@ void RegionGrowingTraversabilityClustering::growRegions(VoxelSet& all_voxels,
   for (auto& [region_id, candidates] : region_candidates) {
     Region& region = regions_.at(region_id);
     region.is_active = true;
-    VoxelSet grown_voxels = growRegion(candidates, *candidates.begin());
+    VoxelSet grown_voxels = growRegion(candidates, *candidates.begin(), num_neighbors);
     region.voxels.insert(grown_voxels.begin(), grown_voxels.end());
     for (const auto& voxel_index : grown_voxels) {
       assigned_voxels[voxel_index] = region_id;
@@ -200,7 +204,7 @@ void RegionGrowingTraversabilityClustering::growRegions(VoxelSet& all_voxels,
     const auto seed_index = *all_voxels.begin();
     new_region.centroid = seed_index.cast<float>();
     VoxelSet grown_voxels =
-        growRegion(all_voxels, seed_index, [&](const VoxelIndex& index) {
+        growRegion(all_voxels, seed_index, num_neighbors, [&](const VoxelIndex& index) {
           return (index.cast<float>() - new_region.centroid).squaredNorm() <=
                  max_dist_sq;
         });
@@ -421,6 +425,7 @@ RegionGrowingTraversabilityClustering::allocateNewRegion() {
 VoxelSet RegionGrowingTraversabilityClustering::growRegion(
     const VoxelSet& candidates,
     const VoxelIndex& seed_index,
+    size_t num_neighbors,
     std::function<bool(const VoxelIndex&)> condition) {
   VoxelSet result;
   if (candidates.find(seed_index) == candidates.end()) {
@@ -435,7 +440,8 @@ VoxelSet RegionGrowingTraversabilityClustering::growRegion(
   while (!queue.empty()) {
     const auto current_index = queue.front();
     queue.pop();
-    for (const auto& offset : neighbors_) {
+    for (size_t k = 0; k < num_neighbors && k < neighbors_.size(); ++k) {
+      const auto& offset = neighbors_[k];
       const VoxelIndex n_index = current_index + offset;
       if (candidates.find(n_index) == candidates.end() || !condition(n_index)) {
         continue;

tests/CMakeLists.txt

@@ -37,6 +37,7 @@ add_executable(
   places/test_gvd_integrator.cpp
   places/test_gvd_utilities.cpp
   places/test_traversability.cpp
+  places/test_region_growing_traversability_clustering.cpp
   reconstruction/test_integration_masking.cpp
   reconstruction/test_marching_cubes.cpp
   reconstruction/test_projection_interpolators.cpp

tests/places/test_region_growing_traversability_clustering.cpp

@@ -0,0 +1,40 @@
+#include <gtest/gtest.h>
+
+#include <array>
+#include <vector>
+
+#include "hydra/places/region_growing_traversability_clustering.h"
+
+namespace hydra::places {
+
+using VoxelSet = RegionGrowingTraversabilityClustering::VoxelSet;
+using Clustering = RegionGrowingTraversabilityClustering;
+
+namespace {
+
+VoxelSet makeSet(const std::vector<std::array<int, 2>>& pts) {
+  VoxelSet s;
+  for (const auto& p : pts) {
+    s.insert(VoxelIndex(p[0], p[1], 0));
+  }
+
+  return s;
+}
+
+}  // namespace
+
+TEST(RegionGrowingTraversabilityClustering, DiagonalGapBlockedBy4Connectivity) {
+  const auto candidates = makeSet({{0, 0}, {1, 1}});
+
+  // 8-connected means single cluster
+  const auto c8 = Clustering::growRegion(candidates, VoxelIndex(0, 0, 0), 8u);
+  EXPECT_EQ(c8.size(), 2u);
+
+  // 4-connected means two cluster
+  const auto c4 = Clustering::growRegion(candidates, VoxelIndex(0, 0, 0), 4u);
+  EXPECT_EQ(c4.size(), 1u);
+  EXPECT_TRUE(c4.count(VoxelIndex(0, 0, 0)));
+  EXPECT_FALSE(c4.count(VoxelIndex(1, 1, 0)));
+}
+
+}  // namespace hydra::places