diff --git a/include/hydra/places/region_growing_traversability_clustering.h b/include/hydra/places/region_growing_traversability_clustering.h index bf37b441..2f86c185 100644 --- a/include/hydra/places/region_growing_traversability_clustering.h +++ b/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 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 condition = [](const VoxelIndex&) { - return true; - }); - 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..5ea25526 100644 --- a/src/places/region_growing_traversability_clustering.cpp +++ b/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(); 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(max_region_size_ * max_region_size_); std::vector 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(); 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 +425,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 +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; diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 67315da1..78625139 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_traversability_clustering.cpp reconstruction/test_integration_masking.cpp reconstruction/test_marching_cubes.cpp reconstruction/test_projection_interpolators.cpp diff --git a/tests/places/test_region_growing_traversability_clustering.cpp b/tests/places/test_region_growing_traversability_clustering.cpp new file mode 100644 index 00000000..1cd58822 --- /dev/null +++ b/tests/places/test_region_growing_traversability_clustering.cpp @@ -0,0 +1,40 @@ +#include + +#include +#include + +#include "hydra/places/region_growing_traversability_clustering.h" + +namespace hydra::places { + +using VoxelSet = RegionGrowingTraversabilityClustering::VoxelSet; +using Clustering = RegionGrowingTraversabilityClustering; + +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(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