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

include/hydra/places/region_growing_traversability_clustering.h

@@ -53,6 +53,15 @@ class RegionGrowingTraversabilityClustering : public TraversabilityClustering {
     float max_radius = 3.0f;
     //! Number of rays to consider for boundary computation.
     int num_orientation_bins = 16;
+    //! Use 8-connectivity (incl. diagonals) when true; 4-connectivity (orthogonal
+    //! only) when false. 4-connectivity stops region growth leaking through 1-voxel
+    //! diagonal gaps in walls.
+    bool use_diagonal_connectivity = true;
+    //! Minimum passage width (in voxels) that connectivity may traverse. 1 disables
+    //! the check. With value W, connectivity is grown only through voxels whose
+    //! (W-1)-radius orthogonal neighborhood is fully traversable, severing phantom
+    //! gaps narrower than W voxels between rooms. Voxel size 0.1 m.
+    int min_connection_width_voxels = 1;
   } const config;
 
   using Voxels = VoxelIndices;
@@ -168,14 +177,39 @@ class RegionGrowingTraversabilityClustering : public TraversabilityClustering {
 
   /**
    * @brief Breadth-first search to grow a region from a seed index.
+   * @param num_neighbors 4 (orthogonal only) or 8 (incl. diagonals) entries of
+   * neighbors_ to use.
    */
   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;
       });
 
+  /**
+   * @brief Erode a candidate set: keep a voxel only if every voxel within `radius` of
+   * it (by the structuring element) is also a candidate. The structuring element
+   * follows the connectivity so the width gate stays consistent with the BFS:
+   * `use_diagonal=false` -> 4-connected (plus / Manhattan ball); `use_diagonal=true`
+   * -> 8-connected (square / Chebyshev ball). radius <= 0 returns the input unchanged.
+   */
+  static VoxelSet erodeCandidates(const VoxelSet& candidates,
+                                  int radius,
+                                  bool use_diagonal);
+
+  /**
+   * @brief BFS from `seed` over `candidates`, propagating ONLY through `core` voxels
+   * (wide-enough voxels). Non-core voxels adjacent to the growing region are included
+   * as leaves but do not expand further, so connectivity cannot cross a gap thinner
+   * than the erosion that produced `core`.
+   */
+  static VoxelSet growConnectedWithMinWidth(const VoxelSet& candidates,
+                                            const VoxelSet& core,
+                                            const VoxelIndex& seed,
+                                            size_t num_neighbors);
+
   void updatePlaceNodeAttributes(spark_dsg::TravNodeAttributes& attrs,
                                  Region& region,
                                  const TraversabilityLayer& layer) const;

src/places/region_growing_traversability_clustering.cpp

@@ -42,6 +42,7 @@
 #include <spark_dsg/edge_attributes.h>
 #include <spark_dsg/node_symbol.h>
 
+#include <cstdlib>
 #include <queue>
 
 #include "hydra/utils/timing_utilities.h"
@@ -71,8 +72,11 @@ 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");
+  field(config.min_connection_width_voxels, "min_connection_width_voxels");
   check(config.max_radius, GT, 0.0f, "max_radius");
   check(config.num_orientation_bins, GE, 3, "num_orientation_bins");
+  check(config.min_connection_width_voxels, GE, 1, "min_connection_width_voxels");
 }
 
 RegionGrowingTraversabilityClustering::RegionGrowingTraversabilityClustering(
@@ -130,7 +134,21 @@ 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 ? 8u : 4u;
+  const int erosion_radius = config.min_connection_width_voxels - 1;
+  if (erosion_radius <= 0) {
+    return growRegion(candidates, start_index, num_neighbors);
+  }
+
+  const VoxelSet core =
+      erodeCandidates(candidates, erosion_radius, config.use_diagonal_connectivity);
+  if (core.find(start_index) == core.end()) {
+    // Robot cell is not "wide" (e.g. hugging a wall); fall back to plain
+    // connectivity so we never drop all places for a frame.
+    return growRegion(candidates, start_index, num_neighbors);
+  }
+  return growConnectedWithMinWidth(candidates, core, start_index, num_neighbors);
 }
 
 VoxelMap RegionGrowingTraversabilityClustering::initializeRegions(
@@ -160,6 +178,8 @@ VoxelMap RegionGrowingTraversabilityClustering::initializeRegions(
 
 void RegionGrowingTraversabilityClustering::growRegions(VoxelSet& all_voxels,
                                                         VoxelMap& assigned_voxels) {
+  const size_t num_neighbors = config.use_diagonal_connectivity ? 8u : 4u;
+
   // 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 +205,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 +220,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 +441,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 +456,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;
@@ -449,6 +471,69 @@ VoxelSet RegionGrowingTraversabilityClustering::growRegion(
   return result;
 }
 
+VoxelSet RegionGrowingTraversabilityClustering::erodeCandidates(
+    const VoxelSet& candidates, int radius, bool use_diagonal) {
+  if (radius <= 0) {
+    return candidates;
+  }
+  VoxelSet eroded;
+  for (const auto& v : candidates) {
+    bool keep = true;
+    for (int dx = -radius; dx <= radius && keep; ++dx) {
+      for (int dy = -radius; dy <= radius; ++dy) {
+        if (dx == 0 && dy == 0) {
+          continue;
+        }
+        // Structuring element follows the connectivity: Chebyshev (square) ball for
+        // 8-connected, Manhattan (plus) ball for 4-connected.
+        const int reach = use_diagonal ? std::max(std::abs(dx), std::abs(dy))
+                                       : std::abs(dx) + std::abs(dy);
+        if (reach > radius) {
+          continue;
+        }
+        if (candidates.find(v + VoxelIndex(dx, dy, 0)) == candidates.end()) {
+          keep = false;
+          break;
+        }
+      }
+    }
+    if (keep) {
+      eroded.insert(v);
+    }
+  }
+  return eroded;
+}
+
+VoxelSet RegionGrowingTraversabilityClustering::growConnectedWithMinWidth(
+    const VoxelSet& candidates,
+    const VoxelSet& core,
+    const VoxelIndex& seed,
+    size_t num_neighbors) {
+  VoxelSet result;
+  if (candidates.find(seed) == candidates.end()) {
+    return result;
+  }
+  VoxelSet visited;
+  std::queue<VoxelIndex> queue;
+  queue.push(seed);
+  visited.insert(seed);
+  while (!queue.empty()) {
+    const auto current = queue.front();
+    queue.pop();
+    result.insert(current);
+    if (core.find(current) == core.end()) {
+      continue;  // narrow voxel: include but do not propagate through it
+    }
+    for (size_t k = 0; k < num_neighbors && k < neighbors_.size(); ++k) {
+      const VoxelIndex n = current + neighbors_[k];
+      if (candidates.find(n) != candidates.end() && visited.insert(n).second) {
+        queue.push(n);
+      }
+    }
+  }
+  return result;
+}
+
 void RegionGrowingTraversabilityClustering::Region::merge(const Region& other) {
   voxels.insert(other.voxels.begin(), other.voxels.end());
   for (const auto& [n_id, n_count] : other.neighbors) {

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_connectivity.cpp
   reconstruction/test_integration_masking.cpp
   reconstruction/test_marching_cubes.cpp
   reconstruction/test_projection_interpolators.cpp

tests/places/test_region_growing_connectivity.cpp

@@ -0,0 +1,113 @@
+#include <gtest/gtest.h>
+
+#include <array>
+#include <vector>
+
+#include "hydra/places/region_growing_traversability_clustering.h"
+
+namespace hydra::places {
+
+// Test shim: expose the protected static helpers.
+struct RegionGrowingTest : public RegionGrowingTraversabilityClustering {
+  using RegionGrowingTraversabilityClustering::erodeCandidates;
+  using RegionGrowingTraversabilityClustering::growConnectedWithMinWidth;
+  using RegionGrowingTraversabilityClustering::growRegion;
+  using RegionGrowingTraversabilityClustering::RegionGrowingTraversabilityClustering;
+};
+
+using VoxelSet = RegionGrowingTraversabilityClustering::VoxelSet;
+
+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(RegionGrowingConnectivity, DiagonalGapBlockedBy4Connectivity) {
+  // Two 1-voxel rooms touching only diagonally: (0,0) and (1,1).
+  const VoxelSet candidates = makeSet({{0, 0}, {1, 1}});
+
+  // 8-connected: diagonal counts -> both reachable.
+  const auto c8 = RegionGrowingTest::growRegion(candidates, VoxelIndex(0, 0, 0), 8u);
+  EXPECT_EQ(c8.size(), 2u);
+
+  // 4-connected: diagonal does NOT count -> only the seed room.
+  const auto c4 = RegionGrowingTest::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 {
+VoxelSet makeRoom(int x0, int x1, int y0, int y1) {
+  VoxelSet s;
+  for (int x = x0; x <= x1; ++x) {
+    for (int y = y0; y <= y1; ++y) {
+      s.insert(VoxelIndex(x, y, 0));
+    }
+  }
+  return s;
+}
+}  // namespace
+
+TEST(RegionGrowingConnectivity, MinWidthSeversThinBridgeKeepsRoom) {
+  // Room A [0..2]x[0..2], Room B [4..6]x[0..2], joined by a 1-voxel bridge (3,1).
+  VoxelSet candidates = makeRoom(0, 2, 0, 2);
+  for (const auto& v : makeRoom(4, 6, 0, 2)) candidates.insert(v);
+  candidates.insert(VoxelIndex(3, 1, 0));
+
+  const int radius = 1;  // min_connection_width_voxels = 2 -> radius = 1
+  const VoxelSet core =
+      RegionGrowingTest::erodeCandidates(candidates, radius, /*use_diagonal=*/false);
+  EXPECT_FALSE(core.count(VoxelIndex(3, 1, 0)));  // 1-wide bridge eroded away
+  EXPECT_TRUE(core.count(VoxelIndex(1, 1, 0)));   // room-A interior survives
+
+  const auto result = RegionGrowingTest::growConnectedWithMinWidth(
+      candidates, core, VoxelIndex(1, 1, 0), 4u);
+  // Room B is unreachable across the thin bridge.
+  EXPECT_FALSE(result.count(VoxelIndex(4, 1, 0)));
+  EXPECT_FALSE(result.count(VoxelIndex(5, 1, 0)));
+  // Room A preserved (center + edges reachable through core).
+  EXPECT_TRUE(result.count(VoxelIndex(1, 1, 0)));
+  EXPECT_TRUE(result.count(VoxelIndex(2, 1, 0)));
+  EXPECT_TRUE(result.count(VoxelIndex(0, 1, 0)));
+}
+
+TEST(RegionGrowingConnectivity, ErosionStructuringElementFollowsConnectivity) {
+  // Full 3x3 block minus one diagonal corner (1,1). The center (0,0) has all 4
+  // orthogonal neighbors present but is missing a diagonal neighbor.
+  VoxelSet candidates = makeRoom(-1, 1, -1, 1);
+  candidates.erase(VoxelIndex(1, 1, 0));
+
+  // 4-connected (plus) erosion: only orthogonal neighbors checked -> center survives.
+  const auto plus =
+      RegionGrowingTest::erodeCandidates(candidates, 1, /*use_diagonal=*/false);
+  EXPECT_TRUE(plus.count(VoxelIndex(0, 0, 0)));
+
+  // 8-connected (square) erosion: the missing diagonal (1,1) disqualifies the center.
+  const auto square =
+      RegionGrowingTest::erodeCandidates(candidates, 1, /*use_diagonal=*/true);
+  EXPECT_FALSE(square.count(VoxelIndex(0, 0, 0)));
+}
+
+TEST(RegionGrowingConnectivity, WideDoorwayConnects) {
+  // Same rooms but a full 3-wide doorway at x=3 (y=0,1,2).
+  VoxelSet candidates = makeRoom(0, 2, 0, 2);
+  for (const auto& v : makeRoom(4, 6, 0, 2)) candidates.insert(v);
+  for (int y = 0; y <= 2; ++y) candidates.insert(VoxelIndex(3, y, 0));
+
+  const int radius = 1;
+  const VoxelSet core =
+      RegionGrowingTest::erodeCandidates(candidates, radius, /*use_diagonal=*/false);
+  EXPECT_TRUE(core.count(VoxelIndex(3, 1, 0)));  // wide doorway survives erosion
+
+  const auto result = RegionGrowingTest::growConnectedWithMinWidth(
+      candidates, core, VoxelIndex(1, 1, 0), 4u);
+  EXPECT_TRUE(result.count(VoxelIndex(5, 1, 0)));  // room B reached via doorway
+}
+
+}  // namespace hydra::places