diff --git a/include/hydra/places/traversability_estimator.h b/include/hydra/places/traversability_estimator.h index 83d6f4ef..06cecb41 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,12 +47,60 @@ 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; using ConstPtr = std::shared_ptr; + struct Config { + //! @brief Minimum confidence for a voxel to be considered observed. + float min_confidence = 1.0f; - TraversabilityEstimator() = default; + //! @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; + + explicit TraversabilityEstimator(const Config& config) : config(config) {} virtual ~TraversabilityEstimator() = default; /** @@ -64,10 +114,20 @@ 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_; }; +void declare_config(TraversabilityEstimator::Config& config); + /** * @brief Simple traversability estimator which checks a specified volume in the TSDF * map for obstacles. @@ -76,23 +136,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); @@ -108,7 +157,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; @@ -124,7 +172,12 @@ void declare_config(HeightTraversabilityEstimator::Config& config); */ class GradientTraversabilityEstimator : public TraversabilityEstimator { public: - struct Config { + 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 //! between. @@ -139,20 +192,12 @@ 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; + + //! @brief Downstream consumers of the height map and gradient map. + std::vector sinks; }; GradientTraversabilityEstimator(const Config& config); @@ -161,6 +206,7 @@ class GradientTraversabilityEstimator : public TraversabilityEstimator { void updateTraversability(const ActiveWindowOutput& msg) override; const Config config; + Sink::List sinks_; protected: TsdfLayer::Ptr tsdf_layer_; @@ -168,22 +214,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; - - // 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: - // 8-way neighbor offsets. - static const std::array kNeighborOffsets; }; void declare_config(GradientTraversabilityEstimator::Config& config); 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..f29e2025 100644 --- a/src/places/traversability_estimator.cpp +++ b/src/places/traversability_estimator.cpp @@ -52,11 +52,16 @@ static const auto gradient_registration_ = GradientTraversabilityEstimator, GradientTraversabilityEstimator::Config>( "GradientTraversabilityEstimator"); -} // namespace -using spark_dsg::TraversabilityState; +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; +} -const std::array GradientTraversabilityEstimator::kNeighborOffsets = {{ +static const std::array kNeighborOffsets = {{ {0, -1}, // bottom {-1, 0}, // left {0, 1}, // top @@ -67,22 +72,167 @@ const std::array GradientTraversabilityEstimator::kNeighborOffsets = {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; +} + +// 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, + 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 : kNeighborOffsets) { + 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; +} + +float computeTraversabilityFromGradient(float gradient, float gradient_threshold) { + if (gradient >= gradient_threshold) { + return 0.0f; + } + return 1.0f - (gradient / gradient_threshold); +} + +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 gradient_map; +} + +using spark_dsg::TraversabilityState; + +void TraversabilityEstimator::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; + } +} + +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)) {} + : TraversabilityEstimator(config), config(config::checkValid(config)) {} void HeightTraversabilityEstimator::updateTraversability( const ActiveWindowOutput& msg) { @@ -188,25 +338,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; @@ -219,25 +350,27 @@ 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"); + field(config.sinks, "sinks"); 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)) {} + : TraversabilityEstimator(config), + config(config::checkValid(config)), + sinks_(Sink::instantiate(config.sinks)) { + LOG(INFO) << "Created GradientTraversabilityEstimator with min traversability: " + << config.min_traversability; +} void GradientTraversabilityEstimator::updateTraversability( const ActiveWindowOutput& msg) { @@ -289,52 +422,25 @@ 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; - } - } - } - } - - // 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; - - // 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) { @@ -343,143 +449,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) { - const Index2D neighbor_idx(center_idx.x() + offset.x(), - center_idx.y() + offset.y()); + trav_voxel.traversability = computeTraversabilityFromGradient( + grad_it->second.gradient, config.gradient_threshold); + trav_voxel.confidence = grad_it->second.confidence; - auto neighbor_it = grad_height_map.find(neighbor_idx); - if (neighbor_it == grad_height_map.end()) { - continue; - } - - num_neighbors_observed++; - - const float height_diff = std::abs(neighbor_it->second - center_height); - const float horiz_dist = computeHorizontalDistance(offset); - 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; - classifyTraversabilityVoxel(trav_voxel); } } } -} - -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; - } -} - -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) { - 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 with the accumulated TSDF layer for bounds computation. + Sink::callAll(sinks_, height_map, gradient_map, msg, *tsdf_layer_); } } // namespace hydra::places