From 98838becda1861878c800126cf4e7e3aa40f511e Mon Sep 17 00:00:00 2001 From: Multyxu Date: Thu, 23 Apr 2026 15:42:21 -0400 Subject: [PATCH 1/7] optionally add height information to traversability voxel for visualization --- include/hydra/places/traversability_layer.h | 4 ++++ src/places/traversability_estimator.cpp | 1 + 2 files changed, 5 insertions(+) diff --git a/include/hydra/places/traversability_layer.h b/include/hydra/places/traversability_layer.h index 98d0f389..97e82a3d 100644 --- a/include/hydra/places/traversability_layer.h +++ b/include/hydra/places/traversability_layer.h @@ -52,6 +52,10 @@ struct TraversabilityVoxel { //! @brief Confidence in the traversability value in [0, 1]. float confidence = 0.0f; + //! @brief The height of the surface in meters in global coordinate, used for + //! debugging and visualization. + std::optional height = 0.0f; + //! @brief Discrete traversability state for of the voxel, computed as a function of // traversability and confidence. spark_dsg::TraversabilityState state = spark_dsg::TraversabilityState::UNKNOWN; diff --git a/src/places/traversability_estimator.cpp b/src/places/traversability_estimator.cpp index 20c93a6b..dfb16fa6 100644 --- a/src/places/traversability_estimator.cpp +++ b/src/places/traversability_estimator.cpp @@ -381,6 +381,7 @@ void GradientTraversabilityEstimator::computeTraversability( num_neighbors_observed > 0 ? gradient_sum / num_neighbors_observed : 0.0f; trav_voxel.traversability = computeTraversabilityFromGradient(mean_gradient); trav_voxel.confidence = num_neighbors_observed / 8.0f; + trav_voxel.height = center_height; classifyTraversabilityVoxel(trav_voxel); } From 70c166ef4e5027f55cdc7ed3cfe369c64952fce2 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Tue, 23 Jun 2026 19:36:04 -0400 Subject: [PATCH 2/7] clean up by factor out some helper functions. --- .../hydra/places/traversability_estimator.h | 9 - src/places/traversability_estimator.cpp | 199 +++++++++--------- 2 files changed, 99 insertions(+), 109 deletions(-) diff --git a/include/hydra/places/traversability_estimator.h b/include/hydra/places/traversability_estimator.h index 83d6f4ef..cc5465ca 100644 --- a/include/hydra/places/traversability_estimator.h +++ b/include/hydra/places/traversability_estimator.h @@ -170,15 +170,6 @@ class GradientTraversabilityEstimator : public TraversabilityEstimator { void computeTraversability(const ActiveWindowOutput& msg); void classifyTraversabilityVoxel(TraversabilityVoxel& voxel) const; - // Helper functions. - BlockIndexSet get2DBlockIndices(const BlockIndices& blocks) const; - - std::optional extractSurfaceHeight(const BlockIndex& block_2d_index, - const VoxelIndex& local_2d, - float robot_z) const; - - float computeHorizontalDistance(const Index2D& offset) const; - float computeTraversabilityFromGradient(float gradient) const; private: diff --git a/src/places/traversability_estimator.cpp b/src/places/traversability_estimator.cpp index dfb16fa6..84741c89 100644 --- a/src/places/traversability_estimator.cpp +++ b/src/places/traversability_estimator.cpp @@ -52,6 +52,96 @@ static const auto gradient_registration_ = GradientTraversabilityEstimator, GradientTraversabilityEstimator::Config>( "GradientTraversabilityEstimator"); + +// Box-filter smoothing of a 2D height map: each cell becomes the average of +// itself and its observed neighbors. Removes projective TSDF radial bias +// (ripple artifact). +Index2DMap smoothHeightMap(const Index2DMap& height_map, + const std::array& neighbor_offsets) { + Index2DMap smoothed; + smoothed.reserve(height_map.size()); + for (const auto& [center_idx, center_height] : height_map) { + float height_sum = center_height; + int count = 1; + for (const auto& offset : neighbor_offsets) { + auto it = height_map.find(center_idx + offset); + if (it != height_map.end()) { + height_sum += it->second; + count++; + } + } + smoothed[center_idx] = height_sum / count; + } + return smoothed; +} + +BlockIndexSet get2DBlockIndices(const BlockIndices& blocks) { + BlockIndexSet block_indices; + for (const auto& block : blocks) { + block_indices.emplace(BlockIndex(block.x(), block.y(), 0)); + } + return block_indices; +} + +// Build a 2D height map from a TSDF layer by scanning each vertical column of +// voxels for the highest surface voxel within the robot's vertical scan window. +// Uses block/local index access directly to avoid the signed/unsigned division +// bug in spatial_hash::blockIndexFromGlobalIndex that corrupts getVoxelPtr +// lookups for negative world coordinates. +Index2DMap extractHeightMap(const TsdfLayer& tsdf_layer, + const BlockIndexSet& blocks_2d, + float robot_z, + float height_below, + float height_above, + float min_weight) { + const float voxel_size = tsdf_layer.voxel_size; + const int vps = static_cast(tsdf_layer.voxels_per_side); + + const VoxelKey min_key = tsdf_layer.getVoxelKey(Point(0, 0, robot_z - height_below)); + const VoxelKey max_key = tsdf_layer.getVoxelKey(Point(0, 0, robot_z + height_above)); + + Index2DMap height_map; + for (const auto& block_idx_2d : blocks_2d) { + for (int x = 0; x < vps; ++x) { + for (int y = 0; y < vps; ++y) { + // Scan the vertical column top-to-bottom and record the highest surface. + for (int block_z = max_key.first.z(); block_z >= min_key.first.z(); --block_z) { + const auto tsdf_block = tsdf_layer.getBlockPtr( + BlockIndex(block_idx_2d.x(), block_idx_2d.y(), block_z)); + if (!tsdf_block) { + continue; + } + + const int min_voxel_z = block_z == min_key.first.z() ? min_key.second.z() : 0; + const int max_voxel_z = + block_z == max_key.first.z() ? max_key.second.z() : vps - 1; + + bool found = false; + for (int z = max_voxel_z; z >= min_voxel_z; --z) { + const auto& voxel = tsdf_block->getVoxel(VoxelIndex(x, y, z)); + if (voxel.weight < min_weight) { + continue; + } + if (voxel.distance < voxel_size) { + const VoxelKey key( + BlockIndex(block_idx_2d.x(), block_idx_2d.y(), block_z), + VoxelIndex(x, y, z)); + const Index2D map_key(block_idx_2d.x() * vps + x, + block_idx_2d.y() * vps + y); + height_map[map_key] = tsdf_layer.getVoxelPosition(key).z(); + found = true; + break; + } + } + if (found) { + break; + } + } + } + } + } + return height_map; +} } // namespace using spark_dsg::TraversabilityState; @@ -289,46 +379,19 @@ void GradientTraversabilityEstimator::computeTraversability( // PASS 1: Extract heights from ALL currently allocated TSDF blocks (not just // previously-processed traversability blocks), so newly-seen areas are included. - Index2DMap height_map; - const auto all_blocks_2d = get2DBlockIndices(tsdf_layer_->allocatedBlockIndices()); - for (const auto& block_idx_2d : all_blocks_2d) { - for (int x = 0; x < voxels_per_side; ++x) { - for (int y = 0; y < voxels_per_side; ++y) { - std::optional surface_height = - extractSurfaceHeight(block_idx_2d, VoxelIndex(x, y, 0), robot_z); - - if (surface_height) { - const Index2D key(block_idx_2d.x() * voxels_per_side + x, - block_idx_2d.y() * voxels_per_side + y); - height_map[key] = *surface_height; - } - } - } - } + const Index2DMap height_map = extractHeightMap(*tsdf_layer_, + all_blocks_2d, + robot_z, + config.height_below, + config.height_above, + config.min_weight); // Optionally smooth height map to remove projective TSDF radial bias (ripple // artifact). For each voxel, replace its height with the average of itself and // observed neighbors. - Index2DMap smoothed_height_map; - if (config.smoothing) { - smoothed_height_map.reserve(height_map.size()); - for (const auto& [center_idx, center_height] : height_map) { - float height_sum = center_height; - int count = 1; - for (const auto& offset : kNeighborOffsets) { - auto it = height_map.find( - Index2D(center_idx.x() + offset.x(), center_idx.y() + offset.y())); - if (it != height_map.end()) { - height_sum += it->second; - count++; - } - } - smoothed_height_map[center_idx] = height_sum / count; - } - } const Index2DMap& grad_height_map = - config.smoothing ? smoothed_height_map : height_map; + config.smoothing ? smoothHeightMap(height_map, kNeighborOffsets) : height_map; // PASS 2: Update ONLY the blocks that were updated in TSDF. for (auto& block_idx_2d : updated_blocks_2d) { @@ -373,7 +436,8 @@ void GradientTraversabilityEstimator::computeTraversability( num_neighbors_observed++; const float height_diff = std::abs(neighbor_it->second - center_height); - const float horiz_dist = computeHorizontalDistance(offset); + const float horiz_dist = + tsdf_layer_->voxel_size * offset.cast().norm(); gradient_sum += height_diff / horiz_dist; } @@ -408,71 +472,6 @@ void GradientTraversabilityEstimator::classifyTraversabilityVoxel( } } -BlockIndexSet GradientTraversabilityEstimator::get2DBlockIndices( - const BlockIndices& blocks) const { - BlockIndexSet block_indices; - for (const auto& block : blocks) { - block_indices.emplace(BlockIndex(block.x(), block.y(), 0)); - } - return block_indices; -} - -std::optional GradientTraversabilityEstimator::extractSurfaceHeight( - const BlockIndex& block_2d_index, const VoxelIndex& local_2d, float robot_z) const { - // Find the highest surface voxel in the vertical column by scanning top to bottom. - // Uses block/local index access directly (same pattern as - // HeightTraversabilityEstimator) to avoid the signed/unsigned division bug in - // spatial_hash::blockIndexFromGlobalIndex, which corrupts getVoxelPtr lookups for any - // negative world coordinate. - const float voxel_size = tsdf_layer_->voxel_size; - const int vps = static_cast(tsdf_layer_->voxels_per_side); - - const VoxelKey min_key = - tsdf_layer_->getVoxelKey(Point(0, 0, robot_z - config.height_below)); - const VoxelKey max_key = - tsdf_layer_->getVoxelKey(Point(0, 0, robot_z + config.height_above)); - - for (int block_z = max_key.first.z(); block_z >= min_key.first.z(); --block_z) { - const auto tsdf_block = tsdf_layer_->getBlockPtr( - BlockIndex(block_2d_index.x(), block_2d_index.y(), block_z)); - if (!tsdf_block) { - continue; - } - - const int min_voxel_z = block_z == min_key.first.z() ? min_key.second.z() : 0; - const int max_voxel_z = block_z == max_key.first.z() ? max_key.second.z() : vps - 1; - - for (int z = max_voxel_z; z >= min_voxel_z; --z) { - const auto& voxel = - tsdf_block->getVoxel(VoxelIndex(local_2d.x(), local_2d.y(), z)); - if (voxel.weight < config.min_weight) { - continue; - } - - if (voxel.distance < voxel_size) { - const VoxelKey key(BlockIndex(block_2d_index.x(), block_2d_index.y(), block_z), - VoxelIndex(local_2d.x(), local_2d.y(), z)); - return tsdf_layer_->getVoxelPosition(key).z(); - } - } - } - - return std::nullopt; -} - -float GradientTraversabilityEstimator::computeHorizontalDistance( - const Index2D& offset) const { - const float voxel_size = tsdf_layer_->voxel_size; - - // Diagonal: sqrt(2) * voxel_size. - if (offset.x() != 0 && offset.y() != 0) { - return voxel_size * std::sqrt(2.0f); - } - - // Cardinal: voxel_size. - return voxel_size; -} - float GradientTraversabilityEstimator::computeTraversabilityFromGradient( float gradient) const { if (gradient >= config.gradient_threshold) { From f75839aa7bb7ac1765d05d1a2854b463fd0c0dc4 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Tue, 23 Jun 2026 19:51:00 -0400 Subject: [PATCH 3/7] factor out common traversability classification. --- .../hydra/places/traversability_estimator.h | 15 +++- src/places/traversability_estimator.cpp | 75 ++++++++----------- 2 files changed, 44 insertions(+), 46 deletions(-) diff --git a/include/hydra/places/traversability_estimator.h b/include/hydra/places/traversability_estimator.h index cc5465ca..24a588fb 100644 --- a/include/hydra/places/traversability_estimator.h +++ b/include/hydra/places/traversability_estimator.h @@ -64,8 +64,21 @@ class TraversabilityEstimator { return *traversability_layer_; } + /** + * @brief Classify a traversability voxel based on its confidence and traversability. + * @note Default implementation uses min_confidence_, min_traversability_, and + * pessimistic_ set by derived class constructors. Subclasses may override for custom + * logic. + */ + virtual void classifyTraversabilityVoxel(TraversabilityVoxel& voxel) const; + protected: std::unique_ptr traversability_layer_; + + //! Thresholds for classifyTraversabilityVoxel(), set by derived class constructors. + float min_confidence_ = 1.0f; + float min_traversability_ = 1.0f; + bool pessimistic_ = true; }; /** @@ -108,7 +121,6 @@ class HeightTraversabilityEstimator : public TraversabilityEstimator { // Processing steps. void updateTsdf(const ActiveWindowOutput& msg); void computeTraversability(const ActiveWindowOutput& msg); - void classifyTraversabilityVoxel(TraversabilityVoxel& voxel) const; // Helper functions. BlockIndexSet get2DBlockIndices(const BlockIndices& blocks) const; @@ -168,7 +180,6 @@ class GradientTraversabilityEstimator : public TraversabilityEstimator { // Processing steps (reuse updateTsdf from HeightTraversabilityEstimator). void updateTsdf(const ActiveWindowOutput& msg); void computeTraversability(const ActiveWindowOutput& msg); - void classifyTraversabilityVoxel(TraversabilityVoxel& voxel) const; float computeTraversabilityFromGradient(float gradient) const; diff --git a/src/places/traversability_estimator.cpp b/src/places/traversability_estimator.cpp index 84741c89..05e43011 100644 --- a/src/places/traversability_estimator.cpp +++ b/src/places/traversability_estimator.cpp @@ -142,10 +142,30 @@ Index2DMap extractHeightMap(const TsdfLayer& tsdf_layer, } return height_map; } +// Where to put these helper functions? } // namespace using spark_dsg::TraversabilityState; +void TraversabilityEstimator::classifyTraversabilityVoxel( + TraversabilityVoxel& voxel) const { + if (voxel.confidence <= 0.0f) { + voxel.state = TraversabilityState::UNKNOWN; + return; + } + if (voxel.confidence >= min_confidence_) { + if (voxel.traversability >= min_traversability_) { + voxel.state = TraversabilityState::TRAVERSABLE; + } else { + voxel.state = TraversabilityState::INTRAVERSABLE; + } + } else if (pessimistic_ && voxel.traversability < min_traversability_) { + voxel.state = TraversabilityState::INTRAVERSABLE; + } else { + voxel.state = TraversabilityState::UNKNOWN; + } +} + const std::array GradientTraversabilityEstimator::kNeighborOffsets = {{ {0, -1}, // bottom {-1, 0}, // left @@ -172,7 +192,11 @@ void declare_config(HeightTraversabilityEstimator::Config& config) { } HeightTraversabilityEstimator::HeightTraversabilityEstimator(const Config& config) - : config(config::checkValid(config)) {} + : config(config::checkValid(config)) { + min_confidence_ = config.min_confidence; + min_traversability_ = config.min_traversability; + pessimistic_ = config.pessimistic; +} void HeightTraversabilityEstimator::updateTraversability( const ActiveWindowOutput& msg) { @@ -278,25 +302,6 @@ void HeightTraversabilityEstimator::computeTraversability( } } -void HeightTraversabilityEstimator::classifyTraversabilityVoxel( - TraversabilityVoxel& voxel) const { - if (voxel.confidence <= 0.0f) { - voxel.state = TraversabilityState::UNKNOWN; - return; - } - if (voxel.confidence >= config.min_confidence) { - if (voxel.traversability >= config.min_traversability) { - voxel.state = TraversabilityState::TRAVERSABLE; - } else { - voxel.state = TraversabilityState::INTRAVERSABLE; - } - } else if (config.pessimistic && voxel.traversability < config.min_traversability) { - voxel.state = TraversabilityState::INTRAVERSABLE; - } else { - voxel.state = TraversabilityState::UNKNOWN; - } -} - BlockIndexSet HeightTraversabilityEstimator::get2DBlockIndices( const BlockIndices& blocks) const { BlockIndexSet block_indices; @@ -327,7 +332,11 @@ void declare_config(GradientTraversabilityEstimator::Config& config) { } GradientTraversabilityEstimator::GradientTraversabilityEstimator(const Config& config) - : config(config::checkValid(config)) {} + : config(config::checkValid(config)) { + min_confidence_ = config.min_confidence; + min_traversability_ = config.min_traversability; + pessimistic_ = config.pessimistic; +} void GradientTraversabilityEstimator::updateTraversability( const ActiveWindowOutput& msg) { @@ -425,10 +434,7 @@ void GradientTraversabilityEstimator::computeTraversability( int num_neighbors_observed = 0; for (const auto& offset : kNeighborOffsets) { - const Index2D neighbor_idx(center_idx.x() + offset.x(), - center_idx.y() + offset.y()); - - auto neighbor_it = grad_height_map.find(neighbor_idx); + auto neighbor_it = grad_height_map.find(center_idx + offset); if (neighbor_it == grad_height_map.end()) { continue; } @@ -453,25 +459,6 @@ void GradientTraversabilityEstimator::computeTraversability( } } -void GradientTraversabilityEstimator::classifyTraversabilityVoxel( - TraversabilityVoxel& voxel) const { - if (voxel.confidence <= 0.0f) { - voxel.state = TraversabilityState::UNKNOWN; - return; - } - if (voxel.confidence >= config.min_confidence) { - if (voxel.traversability >= config.min_traversability) { - voxel.state = TraversabilityState::TRAVERSABLE; - } else { - voxel.state = TraversabilityState::INTRAVERSABLE; - } - } else if (config.pessimistic && voxel.traversability < config.min_traversability) { - voxel.state = TraversabilityState::INTRAVERSABLE; - } else { - voxel.state = TraversabilityState::UNKNOWN; - } -} - float GradientTraversabilityEstimator::computeTraversabilityFromGradient( float gradient) const { if (gradient >= config.gradient_threshold) { From cee6265a1f05f815cc3024dc648e3e6856417cf8 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Thu, 25 Jun 2026 16:21:12 -0400 Subject: [PATCH 4/7] Add base config to traversability --- .../hydra/places/traversability_estimator.h | 48 +++++++------------ src/places/traversability_estimator.cpp | 40 +++++++--------- 2 files changed, 36 insertions(+), 52 deletions(-) diff --git a/include/hydra/places/traversability_estimator.h b/include/hydra/places/traversability_estimator.h index 24a588fb..5c7d9eec 100644 --- a/include/hydra/places/traversability_estimator.h +++ b/include/hydra/places/traversability_estimator.h @@ -49,8 +49,19 @@ class TraversabilityEstimator { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; + struct Config { + //! @brief Minimum confidence for a voxel to be considered observed. + float min_confidence = 1.0f; + + //! @brief Minimum traversability for a voxel to be considered traversable. + float min_traversability = 1.0f; + + //! @brief If true, mark voxels as intraversable if they do not meet the + //! min_traversability threshold, even if the confidence is below min_confidence. If + //! false, mark these voxels as unknown instead. + bool pessimistic = true; + } const config; - TraversabilityEstimator() = default; virtual ~TraversabilityEstimator() = default; /** @@ -73,14 +84,13 @@ class TraversabilityEstimator { virtual void classifyTraversabilityVoxel(TraversabilityVoxel& voxel) const; protected: - std::unique_ptr traversability_layer_; + explicit TraversabilityEstimator(const Config& config) : config(config) {} - //! Thresholds for classifyTraversabilityVoxel(), set by derived class constructors. - float min_confidence_ = 1.0f; - float min_traversability_ = 1.0f; - bool pessimistic_ = true; + std::unique_ptr traversability_layer_; }; +void declare_config(TraversabilityEstimator::Config& config); + /** * @brief Simple traversability estimator which checks a specified volume in the TSDF * map for obstacles. @@ -89,23 +99,12 @@ class TraversabilityEstimator { */ class HeightTraversabilityEstimator : public TraversabilityEstimator { public: - struct Config { + struct Config : public TraversabilityEstimator::Config { //! @brief The height above the robot body to consider for traversability in meters. float height_above = 0.5f; //! @brief The height below the robot body to consider for traversability in meters. float height_below = 0.5f; - - //! @brief Minimum confidence for a voxel to be considered observed. - float min_confidence = 1.0f; - - //! @brief Minimum traversability for a voxel to be considered traversable. - float min_traversability = 1.0f; - - //! @brief If true, mark voxels as intraversable if they do not meet the - //! min_traversability threshold, even if the confidence is below min_confidence. If - //! false, mark these voxels as unknown instead. - bool pessimistic = true; }; HeightTraversabilityEstimator(const Config& config); @@ -136,7 +135,7 @@ void declare_config(HeightTraversabilityEstimator::Config& config); */ class GradientTraversabilityEstimator : public TraversabilityEstimator { public: - struct Config { + struct Config : public TraversabilityEstimator::Config { //! @brief Maximum traversable gradient (m/m). Gradient >= threshold → //! traversability = 0. Gradient = 0 → traversability = 1. Linear interpolation //! between. @@ -151,17 +150,6 @@ class GradientTraversabilityEstimator : public TraversabilityEstimator { //! @brief Minimum TSDF weight to consider voxel observed. float min_weight = 1.0e-6f; - //! @brief Minimum confidence for a voxel to be considered observed. - float min_confidence = 0.5f; - - //! @brief Minimum traversability for a voxel to be considered traversable. - float min_traversability = 0.5f; - - //! @brief If true, mark voxels as intraversable if they do not meet the - //! min_traversability threshold, even if the confidence is below min_confidence. If - //! false, mark these voxels as unknown instead. - bool pessimistic = true; - //! @brief If true, smooth the height map with a box filter before computing //! gradients, reducing the ripple artifact caused by projective TSDF radial bias. bool smoothing = true; diff --git a/src/places/traversability_estimator.cpp b/src/places/traversability_estimator.cpp index 05e43011..d6f5d73a 100644 --- a/src/places/traversability_estimator.cpp +++ b/src/places/traversability_estimator.cpp @@ -153,13 +153,13 @@ void TraversabilityEstimator::classifyTraversabilityVoxel( voxel.state = TraversabilityState::UNKNOWN; return; } - if (voxel.confidence >= min_confidence_) { - if (voxel.traversability >= min_traversability_) { + if (voxel.confidence >= config.min_confidence) { + if (voxel.traversability >= config.min_traversability) { voxel.state = TraversabilityState::TRAVERSABLE; } else { voxel.state = TraversabilityState::INTRAVERSABLE; } - } else if (pessimistic_ && voxel.traversability < min_traversability_) { + } else if (config.pessimistic && voxel.traversability < config.min_traversability) { voxel.state = TraversabilityState::INTRAVERSABLE; } else { voxel.state = TraversabilityState::UNKNOWN; @@ -177,26 +177,27 @@ const std::array GradientTraversabilityEstimator::kNeighborOffsets = {1, -1} // bottom-right }}; +void declare_config(TraversabilityEstimator::Config& config) { + using namespace config; + field(config.min_confidence, "min_confidence"); + field(config.min_traversability, "min_traversability"); + field(config.pessimistic, "pessimistic"); + checkInRange(config.min_confidence, 0.0f, 1.0f, "min_confidence"); + checkInRange(config.min_traversability, 0.0f, 1.0f, "min_traversability"); +} + void declare_config(HeightTraversabilityEstimator::Config& config) { using namespace config; name("HeightTraversabilityEstimator::Config"); + base(config); field(config.height_above, "height_above", "m"); field(config.height_below, "height_below", "m"); - field(config.min_confidence, "min_confidence"); - field(config.min_traversability, "min_traversability"); - field(config.pessimistic, "pessimistic"); checkCondition(config.height_above >= -config.height_below, "'height_above' and 'height_below' don't span any volume"); - checkInRange(config.min_confidence, 0.0f, 1.0f, "min_confidence"); - checkInRange(config.min_traversability, 0.0f, 1.0f, "min_traversability"); } HeightTraversabilityEstimator::HeightTraversabilityEstimator(const Config& config) - : config(config::checkValid(config)) { - min_confidence_ = config.min_confidence; - min_traversability_ = config.min_traversability; - pessimistic_ = config.pessimistic; -} + : TraversabilityEstimator(config), config(config::checkValid(config)) {} void HeightTraversabilityEstimator::updateTraversability( const ActiveWindowOutput& msg) { @@ -314,28 +315,23 @@ BlockIndexSet HeightTraversabilityEstimator::get2DBlockIndices( void declare_config(GradientTraversabilityEstimator::Config& config) { using namespace config; name("GradientTraversabilityEstimator::Config"); + base(config); field(config.gradient_threshold, "gradient_threshold"); field(config.height_above, "height_above", "m"); field(config.height_below, "height_below", "m"); field(config.min_weight, "min_weight"); - field(config.min_confidence, "min_confidence"); - field(config.min_traversability, "min_traversability"); - field(config.pessimistic, "pessimistic"); field(config.smoothing, "smoothing"); checkCondition(config.gradient_threshold > 0.0f, "gradient_threshold must be positive"); checkCondition(config.height_above >= -config.height_below, "'height_above' and 'height_below' don't span any volume"); - checkInRange(config.min_confidence, 0.0f, 1.0f, "min_confidence"); - checkInRange(config.min_traversability, 0.0f, 1.0f, "min_traversability"); } GradientTraversabilityEstimator::GradientTraversabilityEstimator(const Config& config) - : config(config::checkValid(config)) { - min_confidence_ = config.min_confidence; - min_traversability_ = config.min_traversability; - pessimistic_ = config.pessimistic; + : TraversabilityEstimator(config), config(config::checkValid(config)) { + LOG(INFO) << "Created GradientTraversabilityEstimator with min traversability: " + << config.min_traversability; } void GradientTraversabilityEstimator::updateTraversability( From fe370bea96bc3a109b8421b0d1155a3613006fbe Mon Sep 17 00:00:00 2001 From: Multyxu Date: Thu, 25 Jun 2026 18:26:51 -0400 Subject: [PATCH 5/7] use some strcuture form hydra_ros to merge the two gradient computation. --- .../hydra/places/traversability_estimator.h | 51 +++- src/places/traversability_estimator.cpp | 273 +++++++++--------- 2 files changed, 184 insertions(+), 140 deletions(-) diff --git a/include/hydra/places/traversability_estimator.h b/include/hydra/places/traversability_estimator.h index 5c7d9eec..193f249e 100644 --- a/include/hydra/places/traversability_estimator.h +++ b/include/hydra/places/traversability_estimator.h @@ -34,10 +34,12 @@ * -------------------------------------------------------------------------- */ #pragma once +#include #include #include #include +#include #include "hydra/active_window/active_window_output.h" #include "hydra/places/traversability_layer.h" @@ -45,6 +47,42 @@ namespace hydra::places { +struct GradientInfo { + float gradient = 0.0f; // mean gradient magnitude + float confidence = 0.0f; // num_neighbors / 8.0 +}; + +using HeightMap = Index2DMap; +using GradientMap = Index2DMap; + +// Scan one vertical column for the highest surface voxel within [min_z, max_z]. +// Uses block/local index access to avoid the signed/unsigned division bug in +// spatial_hash::blockIndexFromGlobalIndex for negative world coordinates. +// Returns the voxel height if found (weight >= min_weight && distance < voxel_size). +std::optional extractSurfaceHeight(const TsdfLayer& layer, + const BlockIndex& block_2d_index, + const VoxelIndex& local_2d, + float min_z, + float max_z, + float min_weight); + +// Extract terrain surface heights from TSDF within a vertical window around robot_z. +HeightMap extractHeightMap(const TsdfLayer& layer, + const BlockIndexSet& blocks_2d, + float robot_z, + float height_below, + float height_above, + float min_weight); + +// Box-filter smoothing to reduce projective TSDF radial bias (ripple artifact). +HeightMap smoothHeightMap(const HeightMap& height_map); + +// Compute mean 8-way gradient magnitude and neighbor confidence for each cell. +GradientMap computeGradientMap(const HeightMap& height_map, float voxel_size); + +// Linear interpolation: 1.0 at gradient=0, 0.0 at gradient=threshold. +float computeTraversabilityFromGradient(float gradient, float gradient_threshold); + class TraversabilityEstimator { public: using Ptr = std::shared_ptr; @@ -135,6 +173,9 @@ void declare_config(HeightTraversabilityEstimator::Config& config); */ class GradientTraversabilityEstimator : public TraversabilityEstimator { public: + using Sink = hydra:: + OutputSink; + struct Config : public TraversabilityEstimator::Config { //! @brief Maximum traversable gradient (m/m). Gradient >= threshold → //! traversability = 0. Gradient = 0 → traversability = 1. Linear interpolation @@ -153,6 +194,9 @@ class GradientTraversabilityEstimator : public TraversabilityEstimator { //! @brief If true, smooth the height map with a box filter before computing //! gradients, reducing the ripple artifact caused by projective TSDF radial bias. bool smoothing = true; + + //! @brief Downstream consumers of the height map and gradient map. + std::vector sinks; }; GradientTraversabilityEstimator(const Config& config); @@ -161,6 +205,7 @@ class GradientTraversabilityEstimator : public TraversabilityEstimator { void updateTraversability(const ActiveWindowOutput& msg) override; const Config config; + Sink::List sinks_; protected: TsdfLayer::Ptr tsdf_layer_; @@ -168,12 +213,6 @@ class GradientTraversabilityEstimator : public TraversabilityEstimator { // Processing steps (reuse updateTsdf from HeightTraversabilityEstimator). void updateTsdf(const ActiveWindowOutput& msg); void computeTraversability(const ActiveWindowOutput& msg); - - float computeTraversabilityFromGradient(float gradient) const; - - private: - // 8-way neighbor offsets. - static const std::array kNeighborOffsets; }; void declare_config(GradientTraversabilityEstimator::Config& config); diff --git a/src/places/traversability_estimator.cpp b/src/places/traversability_estimator.cpp index d6f5d73a..700440b4 100644 --- a/src/places/traversability_estimator.cpp +++ b/src/places/traversability_estimator.cpp @@ -53,17 +53,97 @@ static const auto gradient_registration_ = GradientTraversabilityEstimator::Config>( "GradientTraversabilityEstimator"); -// Box-filter smoothing of a 2D height map: each cell becomes the average of -// itself and its observed neighbors. Removes projective TSDF radial bias -// (ripple artifact). -Index2DMap smoothHeightMap(const Index2DMap& height_map, - const std::array& neighbor_offsets) { - Index2DMap smoothed; +BlockIndexSet get2DBlockIndices(const BlockIndices& blocks) { + BlockIndexSet block_indices; + for (const auto& block : blocks) { + block_indices.emplace(BlockIndex(block.x(), block.y(), 0)); + } + return block_indices; +} + +static const std::array kNeighborOffsets = {{ + {0, -1}, // bottom + {-1, 0}, // left + {0, 1}, // top + {1, 0}, // right + {-1, -1}, // bottom-left + {-1, 1}, // top-left + {1, 1}, // top-right + {1, -1} // bottom-right +}}; + +} // namespace + +std::optional extractSurfaceHeight(const TsdfLayer& layer, + const BlockIndex& block_2d_index, + const VoxelIndex& local_2d, + float min_z, + float max_z, + float min_weight) { + const float voxel_size = layer.voxel_size; + const int vps = static_cast(layer.voxels_per_side); + const auto min_key = layer.getVoxelKey(Point(0, 0, min_z)); + const auto max_key = layer.getVoxelKey(Point(0, 0, max_z)); + + for (int block_z = max_key.first.z(); block_z >= min_key.first.z(); --block_z) { + const auto tsdf_block = + layer.getBlockPtr(BlockIndex(block_2d_index.x(), block_2d_index.y(), block_z)); + if (!tsdf_block) { + continue; + } + + const int min_voxel_z = block_z == min_key.first.z() ? min_key.second.z() : 0; + const int max_voxel_z = block_z == max_key.first.z() ? max_key.second.z() : vps - 1; + + for (int z = max_voxel_z; z >= min_voxel_z; --z) { + const auto& voxel = + tsdf_block->getVoxel(VoxelIndex(local_2d.x(), local_2d.y(), z)); + if (voxel.weight < min_weight) { + continue; + } + if (voxel.distance < voxel_size) { + const VoxelKey key(BlockIndex(block_2d_index.x(), block_2d_index.y(), block_z), + VoxelIndex(local_2d.x(), local_2d.y(), z)); + return layer.getVoxelPosition(key).z(); + } + } + } + return std::nullopt; +} + +HeightMap extractHeightMap(const TsdfLayer& layer, + const BlockIndexSet& blocks_2d, + float robot_z, + float height_below, + float height_above, + float min_weight) { + const float min_z = robot_z - height_below; + const float max_z = robot_z + height_above; + const int vps = static_cast(layer.voxels_per_side); + + HeightMap height_map; + for (const auto& block_idx_2d : blocks_2d) { + for (int x = 0; x < vps; ++x) { + for (int y = 0; y < vps; ++y) { + auto surface_height = extractSurfaceHeight( + layer, block_idx_2d, VoxelIndex(x, y, 0), min_z, max_z, min_weight); + if (surface_height) { + const Index2D map_key(block_idx_2d.x() * vps + x, block_idx_2d.y() * vps + y); + height_map[map_key] = *surface_height; + } + } + } + } + return height_map; +} + +HeightMap smoothHeightMap(const HeightMap& height_map) { + HeightMap smoothed; smoothed.reserve(height_map.size()); for (const auto& [center_idx, center_height] : height_map) { float height_sum = center_height; int count = 1; - for (const auto& offset : neighbor_offsets) { + for (const auto& offset : kNeighborOffsets) { auto it = height_map.find(center_idx + offset); if (it != height_map.end()) { height_sum += it->second; @@ -75,75 +155,36 @@ Index2DMap smoothHeightMap(const Index2DMap& height_map, return smoothed; } -BlockIndexSet get2DBlockIndices(const BlockIndices& blocks) { - BlockIndexSet block_indices; - for (const auto& block : blocks) { - block_indices.emplace(BlockIndex(block.x(), block.y(), 0)); +float computeTraversabilityFromGradient(float gradient, float gradient_threshold) { + if (gradient >= gradient_threshold) { + return 0.0f; } - return block_indices; + return 1.0f - (gradient / gradient_threshold); } -// Build a 2D height map from a TSDF layer by scanning each vertical column of -// voxels for the highest surface voxel within the robot's vertical scan window. -// Uses block/local index access directly to avoid the signed/unsigned division -// bug in spatial_hash::blockIndexFromGlobalIndex that corrupts getVoxelPtr -// lookups for negative world coordinates. -Index2DMap extractHeightMap(const TsdfLayer& tsdf_layer, - const BlockIndexSet& blocks_2d, - float robot_z, - float height_below, - float height_above, - float min_weight) { - const float voxel_size = tsdf_layer.voxel_size; - const int vps = static_cast(tsdf_layer.voxels_per_side); - - const VoxelKey min_key = tsdf_layer.getVoxelKey(Point(0, 0, robot_z - height_below)); - const VoxelKey max_key = tsdf_layer.getVoxelKey(Point(0, 0, robot_z + height_above)); - - Index2DMap height_map; - for (const auto& block_idx_2d : blocks_2d) { - for (int x = 0; x < vps; ++x) { - for (int y = 0; y < vps; ++y) { - // Scan the vertical column top-to-bottom and record the highest surface. - for (int block_z = max_key.first.z(); block_z >= min_key.first.z(); --block_z) { - const auto tsdf_block = tsdf_layer.getBlockPtr( - BlockIndex(block_idx_2d.x(), block_idx_2d.y(), block_z)); - if (!tsdf_block) { - continue; - } - - const int min_voxel_z = block_z == min_key.first.z() ? min_key.second.z() : 0; - const int max_voxel_z = - block_z == max_key.first.z() ? max_key.second.z() : vps - 1; - - bool found = false; - for (int z = max_voxel_z; z >= min_voxel_z; --z) { - const auto& voxel = tsdf_block->getVoxel(VoxelIndex(x, y, z)); - if (voxel.weight < min_weight) { - continue; - } - if (voxel.distance < voxel_size) { - const VoxelKey key( - BlockIndex(block_idx_2d.x(), block_idx_2d.y(), block_z), - VoxelIndex(x, y, z)); - const Index2D map_key(block_idx_2d.x() * vps + x, - block_idx_2d.y() * vps + y); - height_map[map_key] = tsdf_layer.getVoxelPosition(key).z(); - found = true; - break; - } - } - if (found) { - break; - } - } +GradientMap computeGradientMap(const HeightMap& height_map, float voxel_size) { + GradientMap gradient_map; + gradient_map.reserve(height_map.size()); + for (const auto& [center_idx, center_height] : height_map) { + float gradient_sum = 0.0f; + int num_neighbors_observed = 0; + for (const auto& offset : kNeighborOffsets) { + auto it = height_map.find(center_idx + offset); + if (it == height_map.end()) { + continue; } + num_neighbors_observed++; + const float height_diff = std::abs(it->second - center_height); + const float horiz_dist = voxel_size * offset.cast().norm(); + gradient_sum += height_diff / horiz_dist; + } + if (num_neighbors_observed > 0) { + gradient_map[center_idx] = {gradient_sum / num_neighbors_observed, + num_neighbors_observed / 8.0f}; } } - return height_map; + return gradient_map; } -// Where to put these helper functions? -} // namespace using spark_dsg::TraversabilityState; @@ -166,17 +207,6 @@ void TraversabilityEstimator::classifyTraversabilityVoxel( } } -const std::array GradientTraversabilityEstimator::kNeighborOffsets = {{ - {0, -1}, // bottom - {-1, 0}, // left - {0, 1}, // top - {1, 0}, // right - {-1, -1}, // bottom-left - {-1, 1}, // top-left - {1, 1}, // top-right - {1, -1} // bottom-right -}}; - void declare_config(TraversabilityEstimator::Config& config) { using namespace config; field(config.min_confidence, "min_confidence"); @@ -321,6 +351,7 @@ void declare_config(GradientTraversabilityEstimator::Config& config) { field(config.height_below, "height_below", "m"); field(config.min_weight, "min_weight"); field(config.smoothing, "smoothing"); + field(config.sinks, "sinks"); checkCondition(config.gradient_threshold > 0.0f, "gradient_threshold must be positive"); @@ -329,7 +360,9 @@ void declare_config(GradientTraversabilityEstimator::Config& config) { } GradientTraversabilityEstimator::GradientTraversabilityEstimator(const Config& config) - : TraversabilityEstimator(config), config(config::checkValid(config)) { + : TraversabilityEstimator(config), + config(config::checkValid(config)), + sinks_(Sink::instantiate(config.sinks)) { LOG(INFO) << "Created GradientTraversabilityEstimator with min traversability: " << config.min_traversability; } @@ -385,24 +418,24 @@ void GradientTraversabilityEstimator::computeTraversability( // PASS 1: Extract heights from ALL currently allocated TSDF blocks (not just // previously-processed traversability blocks), so newly-seen areas are included. const auto all_blocks_2d = get2DBlockIndices(tsdf_layer_->allocatedBlockIndices()); - const Index2DMap height_map = extractHeightMap(*tsdf_layer_, - all_blocks_2d, - robot_z, - config.height_below, - config.height_above, - config.min_weight); - - // Optionally smooth height map to remove projective TSDF radial bias (ripple - // artifact). For each voxel, replace its height with the average of itself and - // observed neighbors. - const Index2DMap& grad_height_map = - config.smoothing ? smoothHeightMap(height_map, kNeighborOffsets) : height_map; - - // PASS 2: Update ONLY the blocks that were updated in TSDF. + const HeightMap height_map = extractHeightMap(*tsdf_layer_, + all_blocks_2d, + robot_z, + config.height_below, + config.height_above, + config.min_weight); + + // PASS 2: Optionally smooth, then compute gradient map. + const HeightMap& smooth_map = + config.smoothing ? smoothHeightMap(height_map) : height_map; + const GradientMap gradient_map = + computeGradientMap(smooth_map, tsdf_layer_->voxel_size); + + // PASS 3: Update traversability voxels for updated blocks using the gradient map. for (auto& block_idx_2d : updated_blocks_2d) { auto& trav_block = traversability_layer_->allocateBlock(block_idx_2d, voxels_per_side); - trav_block.reset(); // Reset now, in Pass 2. + trav_block.reset(); trav_block.updated = true; for (int x = 0; x < voxels_per_side; ++x) { @@ -411,58 +444,30 @@ void GradientTraversabilityEstimator::computeTraversability( const BlockIndex global_2d = trav_block.globalFromLocalIndex(Index2D(x, y)); const Index2D center_idx(global_2d.x(), global_2d.y()); - // Check if center has surface. - auto center_it = grad_height_map.find(center_idx); - if (center_it == grad_height_map.end()) { + auto grad_it = gradient_map.find(center_idx); + if (grad_it == gradient_map.end()) { trav_voxel.confidence = 0.0f; trav_voxel.traversability = 0.0f; classifyTraversabilityVoxel(trav_voxel); continue; } - const float center_height = center_it->second; - - // Compute mean gradient over observed neighbors. Skip missing neighbors — - // the confidence field already captures partial coverage; treating missing - // neighbors as infinite gradient makes boundary voxels intraversable even - // on flat terrain, and prevents intermediate (yellow) traversability values. - float gradient_sum = 0.0f; - int num_neighbors_observed = 0; - - for (const auto& offset : kNeighborOffsets) { - auto neighbor_it = grad_height_map.find(center_idx + offset); - if (neighbor_it == grad_height_map.end()) { - continue; - } - - num_neighbors_observed++; + trav_voxel.traversability = computeTraversabilityFromGradient( + grad_it->second.gradient, config.gradient_threshold); + trav_voxel.confidence = grad_it->second.confidence; - const float height_diff = std::abs(neighbor_it->second - center_height); - const float horiz_dist = - tsdf_layer_->voxel_size * offset.cast().norm(); - gradient_sum += height_diff / horiz_dist; + auto height_it = smooth_map.find(center_idx); + if (height_it != smooth_map.end()) { + trav_voxel.height = height_it->second; } - const float mean_gradient = - num_neighbors_observed > 0 ? gradient_sum / num_neighbors_observed : 0.0f; - trav_voxel.traversability = computeTraversabilityFromGradient(mean_gradient); - trav_voxel.confidence = num_neighbors_observed / 8.0f; - trav_voxel.height = center_height; - classifyTraversabilityVoxel(trav_voxel); } } } -} - -float GradientTraversabilityEstimator::computeTraversabilityFromGradient( - float gradient) const { - if (gradient >= config.gradient_threshold) { - return 0.0f; // Intraversable. - } - // Linear interpolation: 1.0 at gradient=0, 0.0 at gradient=threshold. - return 1.0f - (gradient / config.gradient_threshold); + // PASS 4: Dispatch to sinks. + Sink::callAll(sinks_, height_map, gradient_map, msg); } } // namespace hydra::places From 80cd8e2f08032a76d801a13a963faaf01ef4af7a Mon Sep 17 00:00:00 2001 From: Multyxu Date: Thu, 25 Jun 2026 18:59:51 -0400 Subject: [PATCH 6/7] Pass additional tsdf_layer_ to the sink to let it figure out the active window size. --- include/hydra/places/traversability_estimator.h | 6 ++++-- src/places/traversability_estimator.cpp | 4 ++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/include/hydra/places/traversability_estimator.h b/include/hydra/places/traversability_estimator.h index 193f249e..e995935c 100644 --- a/include/hydra/places/traversability_estimator.h +++ b/include/hydra/places/traversability_estimator.h @@ -173,8 +173,10 @@ void declare_config(HeightTraversabilityEstimator::Config& config); */ class GradientTraversabilityEstimator : public TraversabilityEstimator { public: - using Sink = hydra:: - OutputSink; + using Sink = hydra::OutputSink; struct Config : public TraversabilityEstimator::Config { //! @brief Maximum traversable gradient (m/m). Gradient >= threshold → diff --git a/src/places/traversability_estimator.cpp b/src/places/traversability_estimator.cpp index 700440b4..e32f77d9 100644 --- a/src/places/traversability_estimator.cpp +++ b/src/places/traversability_estimator.cpp @@ -466,8 +466,8 @@ void GradientTraversabilityEstimator::computeTraversability( } } - // PASS 4: Dispatch to sinks. - Sink::callAll(sinks_, height_map, gradient_map, msg); + // PASS 4: Dispatch to sinks with the accumulated TSDF layer for bounds computation. + Sink::callAll(sinks_, height_map, gradient_map, msg, *tsdf_layer_); } } // namespace hydra::places From 4caed1695a50172e0aa2c20b9462ef511bf74af8 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Thu, 25 Jun 2026 19:09:02 -0400 Subject: [PATCH 7/7] remove from protected --- include/hydra/places/traversability_estimator.h | 3 +-- src/places/traversability_estimator.cpp | 5 +++++ 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/include/hydra/places/traversability_estimator.h b/include/hydra/places/traversability_estimator.h index e995935c..06cecb41 100644 --- a/include/hydra/places/traversability_estimator.h +++ b/include/hydra/places/traversability_estimator.h @@ -100,6 +100,7 @@ class TraversabilityEstimator { bool pessimistic = true; } const config; + explicit TraversabilityEstimator(const Config& config) : config(config) {} virtual ~TraversabilityEstimator() = default; /** @@ -122,8 +123,6 @@ class TraversabilityEstimator { virtual void classifyTraversabilityVoxel(TraversabilityVoxel& voxel) const; protected: - explicit TraversabilityEstimator(const Config& config) : config(config) {} - std::unique_ptr traversability_layer_; }; diff --git a/src/places/traversability_estimator.cpp b/src/places/traversability_estimator.cpp index e32f77d9..f29e2025 100644 --- a/src/places/traversability_estimator.cpp +++ b/src/places/traversability_estimator.cpp @@ -111,6 +111,11 @@ std::optional extractSurfaceHeight(const TsdfLayer& layer, return std::nullopt; } +// Build a 2D height map from a TSDF layer by scanning each vertical column of +// voxels for the highest surface voxel within the robot's vertical scan window. +// Uses block/local index access directly to avoid the signed/unsigned division +// bug in spatial_hash::blockIndexFromGlobalIndex that corrupts getVoxelPtr +// lookups for negative world coordinates. HeightMap extractHeightMap(const TsdfLayer& layer, const BlockIndexSet& blocks_2d, float robot_z,