diff --git a/include/hydra/places/region_growing_traversability_clustering.h b/include/hydra/places/region_growing_traversability_clustering.h index bf37b441..e0718119 100644 --- a/include/hydra/places/region_growing_traversability_clustering.h +++ b/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 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; diff --git a/src/places/region_growing_traversability_clustering.cpp b/src/places/region_growing_traversability_clustering.cpp index b2f6c7b7..3949b995 100644 --- a/src/places/region_growing_traversability_clustering.cpp +++ b/src/places/region_growing_traversability_clustering.cpp @@ -42,6 +42,7 @@ #include #include +#include #include #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(); 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(max_region_size_ * max_region_size_); std::vector 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(); VoxelSet grown_voxels = - growRegion(all_voxels, seed_index, [&](const VoxelIndex& index) { + growRegion(all_voxels, seed_index, num_neighbors, [&](const VoxelIndex& index) { return (index.cast() - 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 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 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) { diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 67315da1..0bcec229 100644 --- a/tests/CMakeLists.txt +++ b/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 diff --git a/tests/places/test_region_growing_connectivity.cpp b/tests/places/test_region_growing_connectivity.cpp new file mode 100644 index 00000000..29be5f56 --- /dev/null +++ b/tests/places/test_region_growing_connectivity.cpp @@ -0,0 +1,113 @@ +#include + +#include +#include + +#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>& 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