From 2f720057d7c3584b9dec710138a2a4eb7140207b Mon Sep 17 00:00:00 2001 From: Multyxu Date: Thu, 23 Apr 2026 15:44:44 -0400 Subject: [PATCH 01/12] optionally draw traversability voxels at their surface height level --- hydra_ros/src/frontend/traversability_visualizer.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/hydra_ros/src/frontend/traversability_visualizer.cpp b/hydra_ros/src/frontend/traversability_visualizer.cpp index 85bd0ff..e2d1e59 100644 --- a/hydra_ros/src/frontend/traversability_visualizer.cpp +++ b/hydra_ros/src/frontend/traversability_visualizer.cpp @@ -144,6 +144,9 @@ void TraversabilityVisualizer::visualizeLayer( continue; // Unobserved voxels. } pos.y = block.origin().y() + (y + 0.5f) * layer.voxel_size; + if (voxel.height) { + pos.z = *voxel.height; + } msg.points.push_back(pos); msg.colors.push_back(visualizer::makeColorMsg( traversability_colormap_->getColor(voxel.traversability), From b486baaf5a62a57d0809a014f515f5fc4d625aae Mon Sep 17 00:00:00 2001 From: Multyxu Date: Tue, 14 Apr 2026 22:47:30 -0400 Subject: [PATCH 02/12] first version of gradient based local grid. --- hydra_ros/CMakeLists.txt | 1 + .../tsdf_gradient_occupancy_publisher.h | 136 ++++++ .../tsdf_gradient_occupancy_publisher.cpp | 391 ++++++++++++++++++ 3 files changed, 528 insertions(+) create mode 100644 hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h create mode 100644 hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp diff --git a/hydra_ros/CMakeLists.txt b/hydra_ros/CMakeLists.txt index 458f3bc..e1018ee 100644 --- a/hydra_ros/CMakeLists.txt +++ b/hydra_ros/CMakeLists.txt @@ -35,6 +35,7 @@ add_library( src/hydra_ros_pipeline.cpp src/active_window/reconstruction_visualizer.cpp src/active_window/tsdf_occupancy_publisher.cpp + src/active_window/tsdf_gradient_occupancy_publisher.cpp src/backend/ros_backend_publisher.cpp src/backend/gt_room_publisher.cpp src/frontend/gvd_occupancy_publisher.cpp diff --git a/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h b/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h new file mode 100644 index 0000000..a51fe8c --- /dev/null +++ b/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h @@ -0,0 +1,136 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#pragma once +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace hydra { + +using Index2D = Eigen::Vector2i; + +struct Index2DHash { + inline static const auto s = Index2D(1, 1290); + int operator()(const Index2D& index) const { return index.dot(s); } +}; + +template +using Index2DMap = + std::unordered_map, + Eigen::aligned_allocator>>; + +struct GradientInfo { + float gradient = 0.0f; // mean gradient magnitude + float confidence = 0.0f; // num_neighbors / 8.0 +}; + +class TsdfGradientOccupancyPublisher : public ReconstructionModule::Sink { + public: + struct Config { + // Base occupancy config fields + std::string ns = "~/tsdf_gradient"; + bool collate = false; + bool use_relative_height = true; + double slice_height = -1.0; + size_t num_slices = 20; // if if 10 cm, we want 2 m from -1 m to +1 m + bool add_robot_footprint = false; + Eigen::Vector3f footprint_min = Eigen::Vector3f::Zero(); + Eigen::Vector3f footprint_max = Eigen::Vector3f::Zero(); + + // Gradient-specific parameters + float gradient_threshold = 0.5f; // m/m - max traversable gradient + float min_weight = 1.0e-6f; // min TSDF weight for observed voxel + float min_confidence = 0.5f; // min confidence (neighbors/8) for valid cell + bool smoothing = true; // apply box filter to reduce TSDF ripple + bool probabilistic = false; // continuous vs binary occupancy + } const config; + + explicit TsdfGradientOccupancyPublisher(const Config& config); + + virtual ~TsdfGradientOccupancyPublisher() = default; + + std::string printInfo() const override; + + void call(uint64_t timestamp_ns, + const VolumetricMap& map, + const ActiveWindowOutput& output) const override; + + private: + rclcpp::Publisher::SharedPtr pub_; + + // Helper functions + std::optional extractSurfaceHeight(const TsdfLayer& layer, + const Index2D& global_2d, + float min_z, + float max_z) const; + + void buildHeightMap(const TsdfLayer& layer, + float min_z, + float max_z, + Index2DMap& height_map) const; + + void computeGradientMap(const Index2DMap& height_map, + float voxel_size, + Index2DMap& gradient_map) const; + + void fillOccupancyGrid(const Index2DMap& gradient_map, + const Eigen::Isometry3d& world_T_sensor, + const TsdfLayer& layer, + nav_msgs::msg::OccupancyGrid& msg) const; + + float computeHorizontalDistance(const Index2D& offset, + float voxel_size) const; + + float computeTraversabilityFromGradient(float gradient) const; + + int8_t gradientToOccupancy(float gradient, float confidence) const; + + // 8-way neighbor offsets + static const std::array kNeighborOffsets; +}; + +void declare_config(TsdfGradientOccupancyPublisher::Config& config); + +} // namespace hydra diff --git a/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp new file mode 100644 index 0000000..6142f7e --- /dev/null +++ b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp @@ -0,0 +1,391 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include "hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h" + +#include +#include +#include +#include +#include + +#include + +namespace hydra { + +namespace { + +// 8-way neighbor offsets (cardinal + diagonal) +const std::array kNeighborOffsetsArray = {{ + {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 + +const std::array + TsdfGradientOccupancyPublisher::kNeighborOffsets = kNeighborOffsetsArray; + +void declare_config(TsdfGradientOccupancyPublisher::Config& config) { + using namespace config; + name("TsdfGradientOccupancyPublisher::Config"); + field(config.ns, "ns"); + field(config.collate, "collate"); + field(config.use_relative_height, "use_relative_height"); + field(config.slice_height, "slice_height", "m"); + field(config.num_slices, "num_slices"); + field(config.add_robot_footprint, "add_robot_footprint"); + field(config.footprint_min, "footprint_min"); + field(config.footprint_max, "footprint_max"); + field(config.gradient_threshold, "gradient_threshold"); + field(config.min_weight, "min_weight"); + field(config.min_confidence, "min_confidence"); + field(config.smoothing, "smoothing"); + field(config.probabilistic, "probabilistic"); + + checkCondition(config.gradient_threshold > 0.0f, + "gradient_threshold must be positive"); + checkInRange(config.min_confidence, 0.0f, 1.0f, "min_confidence"); +} + +TsdfGradientOccupancyPublisher::TsdfGradientOccupancyPublisher(const Config& config) + : config(config::checkValid(config)), + pub_(ianvs::NodeHandle::this_node(config.ns) + .create_publisher( + "occupancy", + rclcpp::QoS(1).transient_local())) {} + +std::string TsdfGradientOccupancyPublisher::printInfo() const { + return config::toString(config); +} + +void TsdfGradientOccupancyPublisher::call(uint64_t timestamp_ns, + const VolumetricMap& map, + const ActiveWindowOutput& output) const { + if (!pub_->get_subscription_count()) { + return; + } + + const auto& tsdf_layer = map.getTsdfLayer(); + const auto& world_T_body = output.world_T_body(); + + // Compute vertical range + double base_z = config.slice_height; + if (config.use_relative_height) { + base_z += world_T_body.translation().z(); + } + + const float voxel_size = tsdf_layer.voxel_size; + const float min_z = static_cast(base_z); + const float max_z = static_cast(base_z + config.num_slices * voxel_size); + + // Build height map (Pass 1) + Index2DMap height_map; + buildHeightMap(tsdf_layer, min_z, max_z, height_map); + + // Compute gradient map (Pass 2) + Index2DMap gradient_map; + computeGradientMap(height_map, voxel_size, gradient_map); + + // Fill and publish occupancy grid (Pass 3) + nav_msgs::msg::OccupancyGrid msg; + msg.header.frame_id = GlobalInfo::instance().getFrames().map; + msg.header.stamp = rclcpp::Time(timestamp_ns); + msg.info.map_load_time = msg.header.stamp; + + fillOccupancyGrid(gradient_map, world_T_body, tsdf_layer, msg); + pub_->publish(msg); +} + +std::optional TsdfGradientOccupancyPublisher::extractSurfaceHeight( + const TsdfLayer& layer, + const Index2D& global_2d, + float min_z, + float max_z) const { + const float voxel_size = layer.voxel_size; + const int vps = static_cast(layer.voxels_per_side); + + // Convert global 2D index to block coordinates + const int block_x = global_2d.x() / vps; + const int block_y = global_2d.y() / vps; + const int local_x = global_2d.x() % vps; + const int local_y = global_2d.y() % vps; + + // Get vertical range in voxel coordinates + const auto min_key = layer.getVoxelKey(spatial_hash::Point(0, 0, min_z)); + const auto max_key = layer.getVoxelKey(spatial_hash::Point(0, 0, max_z)); + + // Scan from top to bottom to find highest surface + for (int block_z = max_key.first.z(); block_z >= min_key.first.z(); --block_z) { + const auto tsdf_block = layer.getBlockPtr(BlockIndex(block_x, block_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_x, local_y, z)); + + if (voxel.weight < config.min_weight) { + continue; + } + + if (voxel.distance < voxel_size) { + const VoxelKey key(BlockIndex(block_x, block_y, block_z), + VoxelIndex(local_x, local_y, z)); + return layer.getVoxelPosition(key).z(); + } + } + } + + return std::nullopt; +} + +void TsdfGradientOccupancyPublisher::buildHeightMap( + const TsdfLayer& layer, + float min_z, + float max_z, + Index2DMap& height_map) const { + const int vps = static_cast(layer.voxels_per_side); + + // Iterate over all allocated TSDF blocks + for (const auto& block : layer) { + // For each 2D column in the block + for (int local_x = 0; local_x < vps; ++local_x) { + for (int local_y = 0; local_y < vps; ++local_y) { + // Compute global 2D index + const Index2D global_2d(block.index.x() * vps + local_x, + block.index.y() * vps + local_y); + + // Extract surface height for this column + auto surface_height = extractSurfaceHeight(layer, global_2d, min_z, max_z); + if (surface_height) { + height_map[global_2d] = *surface_height; + } + } + } + } +} + +void TsdfGradientOccupancyPublisher::computeGradientMap( + const Index2DMap& height_map, + float voxel_size, + Index2DMap& gradient_map) const { + // Optional smoothing pass + 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) { + const Index2D neighbor_idx(center_idx.x() + offset.x(), + center_idx.y() + offset.y()); + auto it = height_map.find(neighbor_idx); + if (it != height_map.end()) { + height_sum += it->second; + count++; + } + } + + smoothed_height_map[center_idx] = height_sum / count; + } + } + + const auto& grad_height_map = config.smoothing ? smoothed_height_map : height_map; + + // Compute gradients + gradient_map.reserve(grad_height_map.size()); + for (const auto& [center_idx, center_height] : grad_height_map) { + 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()); + + 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, voxel_size); + gradient_sum += height_diff / horiz_dist; + } + + GradientInfo info; + info.gradient = + num_neighbors_observed > 0 ? gradient_sum / num_neighbors_observed : 0.0f; + info.confidence = num_neighbors_observed / 8.0f; + gradient_map[center_idx] = info; + } +} + +void TsdfGradientOccupancyPublisher::fillOccupancyGrid( + const Index2DMap& gradient_map, + const Eigen::Isometry3d& world_T_sensor, + const TsdfLayer& layer, + nav_msgs::msg::OccupancyGrid& msg) const { + if (gradient_map.empty()) { + return; + } + + // Compute bounds + Eigen::Vector2f x_min = Eigen::Vector2f::Constant(std::numeric_limits::max()); + Eigen::Vector2f x_max = + Eigen::Vector2f::Constant(std::numeric_limits::lowest()); + + const float voxel_size = layer.voxel_size; + for (const auto& [idx, _] : gradient_map) { + const Eigen::Vector2f pos = idx.cast() * voxel_size; + x_min = x_min.array().min(pos.array()); + x_max = x_max.array().max(pos.array()); + } + + // Add one voxel margin + x_min -= Eigen::Vector2f::Constant(voxel_size); + x_max += Eigen::Vector2f::Constant(voxel_size); + + const Eigen::Vector2f dims = (x_max - x_min) / voxel_size; + + // Initialize grid + msg.info.resolution = voxel_size; + msg.info.width = std::ceil(dims.x()); + msg.info.height = std::ceil(dims.y()); + msg.info.origin.position.x = x_min.x(); + msg.info.origin.position.y = x_min.y(); + msg.info.origin.position.z = config.slice_height; + msg.info.origin.orientation.w = 1.0; + msg.data.resize(msg.info.width * msg.info.height, -1); + + // Setup robot footprint if needed + spark_dsg::BoundingBox bbox; + Eigen::Isometry3f sensor_T_world; + if (config.add_robot_footprint) { + bbox = spark_dsg::BoundingBox(config.footprint_min, config.footprint_max); + sensor_T_world = world_T_sensor.inverse().cast(); + } + + // Fill occupancy grid + for (const auto& [global_idx, gradient_info] : gradient_map) { + const Eigen::Vector2f pos = global_idx.cast() * voxel_size; + const Eigen::Vector2f rel_pos = pos - x_min; + + const auto r = std::floor(rel_pos.y() / voxel_size); + const auto c = std::floor(rel_pos.x() / voxel_size); + const size_t index = r * msg.info.width + c; + + if (index >= msg.data.size()) { + continue; + } + + // Check robot footprint + if (config.add_robot_footprint) { + const Eigen::Vector3f pos_3d(pos.x(), pos.y(), 0.0f); + if (bbox.contains((sensor_T_world * pos_3d).eval())) { + msg.data[index] = 0; + continue; + } + } + + // Map gradient to occupancy + msg.data[index] = + gradientToOccupancy(gradient_info.gradient, gradient_info.confidence); + } +} + +float TsdfGradientOccupancyPublisher::computeHorizontalDistance( + const Index2D& offset, + float voxel_size) const { + // 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 TsdfGradientOccupancyPublisher::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); +} + +int8_t TsdfGradientOccupancyPublisher::gradientToOccupancy(float gradient, + float confidence) const { + // Check confidence threshold + if (confidence < config.min_confidence) { + return -1; // Unknown + } + + if (config.probabilistic) { + // Continuous mode: map traversability to occupancy + const float traversability = computeTraversabilityFromGradient(gradient); + const float occupancy_float = (1.0f - traversability) * 100.0f; + return static_cast(std::clamp(occupancy_float, 0.0f, 100.0f)); + } else { + // Binary mode: threshold-based + return gradient >= config.gradient_threshold ? 100 : 0; + } +} + +namespace { + +static const auto registration_ = + config::RegistrationWithConfig( + "TsdfGradientOccupancyPublisher"); + +} // namespace + +} // namespace hydra From 58859fbd5998d1960de14003eb4abb9e47edb26b Mon Sep 17 00:00:00 2001 From: Multyxu Date: Tue, 14 Apr 2026 23:23:04 -0400 Subject: [PATCH 03/12] visualize height and gradient map --- .../tsdf_gradient_occupancy_publisher.h | 10 ++ .../tsdf_gradient_occupancy_publisher.cpp | 160 +++++++++++++++++- 2 files changed, 167 insertions(+), 3 deletions(-) diff --git a/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h b/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h index a51fe8c..ad4cf04 100644 --- a/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h +++ b/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h @@ -99,6 +99,8 @@ class TsdfGradientOccupancyPublisher : public ReconstructionModule::Sink { private: rclcpp::Publisher::SharedPtr pub_; + rclcpp::Publisher::SharedPtr height_map_pub_; + rclcpp::Publisher::SharedPtr gradient_map_pub_; // Helper functions std::optional extractSurfaceHeight(const TsdfLayer& layer, @@ -120,6 +122,14 @@ class TsdfGradientOccupancyPublisher : public ReconstructionModule::Sink { const TsdfLayer& layer, nav_msgs::msg::OccupancyGrid& msg) const; + void publishHeightMapViz(const Index2DMap& height_map, + const TsdfLayer& layer, + uint64_t timestamp_ns) const; + + void publishGradientMapViz(const Index2DMap& gradient_map, + const TsdfLayer& layer, + uint64_t timestamp_ns) const; + float computeHorizontalDistance(const Index2D& offset, float voxel_size) const; diff --git a/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp index 6142f7e..41ba152 100644 --- a/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp +++ b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp @@ -90,7 +90,15 @@ TsdfGradientOccupancyPublisher::TsdfGradientOccupancyPublisher(const Config& con pub_(ianvs::NodeHandle::this_node(config.ns) .create_publisher( "occupancy", - rclcpp::QoS(1).transient_local())) {} + rclcpp::QoS(1).transient_local())), + height_map_pub_(ianvs::NodeHandle::this_node(config.ns) + .create_publisher( + "height_map_debug", + rclcpp::QoS(1).transient_local())), + gradient_map_pub_(ianvs::NodeHandle::this_node(config.ns) + .create_publisher( + "gradient_map_debug", + rclcpp::QoS(1).transient_local())) {} std::string TsdfGradientOccupancyPublisher::printInfo() const { return config::toString(config); @@ -99,7 +107,8 @@ std::string TsdfGradientOccupancyPublisher::printInfo() const { void TsdfGradientOccupancyPublisher::call(uint64_t timestamp_ns, const VolumetricMap& map, const ActiveWindowOutput& output) const { - if (!pub_->get_subscription_count()) { + if (!pub_->get_subscription_count() && !height_map_pub_->get_subscription_count() && + !gradient_map_pub_->get_subscription_count()) { return; } @@ -119,10 +128,12 @@ void TsdfGradientOccupancyPublisher::call(uint64_t timestamp_ns, // Build height map (Pass 1) Index2DMap height_map; buildHeightMap(tsdf_layer, min_z, max_z, height_map); + publishHeightMapViz(height_map, tsdf_layer, timestamp_ns); // Compute gradient map (Pass 2) Index2DMap gradient_map; computeGradientMap(height_map, voxel_size, gradient_map); + publishGradientMapViz(gradient_map, tsdf_layer, timestamp_ns); // Fill and publish occupancy grid (Pass 3) nav_msgs::msg::OccupancyGrid msg; @@ -195,7 +206,7 @@ void TsdfGradientOccupancyPublisher::buildHeightMap( for (int local_y = 0; local_y < vps; ++local_y) { // Compute global 2D index const Index2D global_2d(block.index.x() * vps + local_x, - block.index.y() * vps + local_y); + block.index.y() * vps + local_y); // Extract surface height for this column auto surface_height = extractSurfaceHeight(layer, global_2d, min_z, max_z); @@ -378,6 +389,149 @@ int8_t TsdfGradientOccupancyPublisher::gradientToOccupancy(float gradient, } } +void TsdfGradientOccupancyPublisher::publishHeightMapViz( + const Index2DMap& height_map, + const TsdfLayer& layer, + uint64_t timestamp_ns) const { + if (!height_map_pub_->get_subscription_count()) { + return; + } + + if (height_map.empty()) { + return; + } + + // Find min/max heights for normalization + float min_height = std::numeric_limits::max(); + float max_height = std::numeric_limits::lowest(); + for (const auto& [_, height] : height_map) { + min_height = std::min(min_height, height); + max_height = std::max(max_height, height); + } + + // Compute bounds + Eigen::Vector2f x_min = Eigen::Vector2f::Constant(std::numeric_limits::max()); + Eigen::Vector2f x_max = + Eigen::Vector2f::Constant(std::numeric_limits::lowest()); + + const float voxel_size = layer.voxel_size; + for (const auto& [idx, _] : height_map) { + const Eigen::Vector2f pos = idx.cast() * voxel_size; + x_min = x_min.array().min(pos.array()); + x_max = x_max.array().max(pos.array()); + } + + x_min -= Eigen::Vector2f::Constant(voxel_size); + x_max += Eigen::Vector2f::Constant(voxel_size); + + const Eigen::Vector2f dims = (x_max - x_min) / voxel_size; + + // Initialize grid + nav_msgs::msg::OccupancyGrid msg; + msg.header.frame_id = GlobalInfo::instance().getFrames().map; + msg.header.stamp = rclcpp::Time(timestamp_ns); + msg.info.map_load_time = msg.header.stamp; + msg.info.resolution = voxel_size; + msg.info.width = std::ceil(dims.x()); + msg.info.height = std::ceil(dims.y()); + msg.info.origin.position.x = x_min.x(); + msg.info.origin.position.y = x_min.y(); + msg.info.origin.position.z = 0.0; + msg.info.origin.orientation.w = 1.0; + msg.data.resize(msg.info.width * msg.info.height, -1); + + // Fill grid with normalized heights + const float height_range = max_height - min_height; + for (const auto& [global_idx, height] : height_map) { + const Eigen::Vector2f pos = global_idx.cast() * voxel_size; + const Eigen::Vector2f rel_pos = pos - x_min; + + const auto r = std::floor(rel_pos.y() / voxel_size); + const auto c = std::floor(rel_pos.x() / voxel_size); + const size_t index = r * msg.info.width + c; + + if (index >= msg.data.size()) { + continue; + } + + // Normalize height to 0-100 range + if (height_range > 1e-6f) { + const float normalized = (height - min_height) / height_range * 100.0f; + msg.data[index] = static_cast(std::clamp(normalized, 0.0f, 100.0f)); + } else { + msg.data[index] = 50; // All same height -> mid-gray + } + } + + height_map_pub_->publish(msg); +} + +void TsdfGradientOccupancyPublisher::publishGradientMapViz( + const Index2DMap& gradient_map, + const TsdfLayer& layer, + uint64_t timestamp_ns) const { + if (!gradient_map_pub_->get_subscription_count()) { + return; + } + + if (gradient_map.empty()) { + return; + } + + // Compute bounds + Eigen::Vector2f x_min = Eigen::Vector2f::Constant(std::numeric_limits::max()); + Eigen::Vector2f x_max = + Eigen::Vector2f::Constant(std::numeric_limits::lowest()); + + const float voxel_size = layer.voxel_size; + for (const auto& [idx, _] : gradient_map) { + const Eigen::Vector2f pos = idx.cast() * voxel_size; + x_min = x_min.array().min(pos.array()); + x_max = x_max.array().max(pos.array()); + } + + x_min -= Eigen::Vector2f::Constant(voxel_size); + x_max += Eigen::Vector2f::Constant(voxel_size); + + const Eigen::Vector2f dims = (x_max - x_min) / voxel_size; + + // Initialize grid + nav_msgs::msg::OccupancyGrid msg; + msg.header.frame_id = GlobalInfo::instance().getFrames().map; + msg.header.stamp = rclcpp::Time(timestamp_ns); + msg.info.map_load_time = msg.header.stamp; + msg.info.resolution = voxel_size; + msg.info.width = std::ceil(dims.x()); + msg.info.height = std::ceil(dims.y()); + msg.info.origin.position.x = x_min.x(); + msg.info.origin.position.y = x_min.y(); + msg.info.origin.position.z = 0.0; + msg.info.origin.orientation.w = 1.0; + msg.data.resize(msg.info.width * msg.info.height, -1); + + // Fill grid with gradient values + for (const auto& [global_idx, gradient_info] : gradient_map) { + const Eigen::Vector2f pos = global_idx.cast() * voxel_size; + const Eigen::Vector2f rel_pos = pos - x_min; + + const auto r = std::floor(rel_pos.y() / voxel_size); + const auto c = std::floor(rel_pos.x() / voxel_size); + const size_t index = r * msg.info.width + c; + + if (index >= msg.data.size()) { + continue; + } + + // Map gradient to 0-100 (using gradient_threshold as max) + // Low gradient (flat) -> 0 (free), high gradient (steep) -> 100 (occupied) + const float normalized = + std::min(gradient_info.gradient / config.gradient_threshold, 1.0f) * 100.0f; + msg.data[index] = static_cast(normalized); + } + + gradient_map_pub_->publish(msg); +} + namespace { static const auto registration_ = From cd0502d11f7af6e054e407b273da7783f6ec9422 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Tue, 14 Apr 2026 23:56:10 -0400 Subject: [PATCH 04/12] still having checkerboard effect when visualizing the map --- .../tsdf_gradient_occupancy_publisher.h | 3 +- .../tsdf_gradient_occupancy_publisher.cpp | 46 +++++++++++-------- 2 files changed, 30 insertions(+), 19 deletions(-) diff --git a/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h b/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h index ad4cf04..e4220af 100644 --- a/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h +++ b/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h @@ -104,7 +104,8 @@ class TsdfGradientOccupancyPublisher : public ReconstructionModule::Sink { // Helper functions std::optional extractSurfaceHeight(const TsdfLayer& layer, - const Index2D& global_2d, + const BlockIndex& block_2d_index, + const VoxelIndex& local_2d, float min_z, float max_z) const; diff --git a/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp index 41ba152..c356342 100644 --- a/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp +++ b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp @@ -41,6 +41,7 @@ #include #include +#include namespace hydra { @@ -147,25 +148,25 @@ void TsdfGradientOccupancyPublisher::call(uint64_t timestamp_ns, std::optional TsdfGradientOccupancyPublisher::extractSurfaceHeight( const TsdfLayer& layer, - const Index2D& global_2d, + const BlockIndex& block_2d_index, + const VoxelIndex& local_2d, float min_z, float max_z) const { + // Uses block/local index access directly (same pattern as + // GradientTraversabilityEstimator) to avoid the signed/unsigned division bug in + // spatial_hash::blockIndexFromGlobalIndex, which corrupts getVoxelPtr lookups for + // negative world coordinates. const float voxel_size = layer.voxel_size; const int vps = static_cast(layer.voxels_per_side); - // Convert global 2D index to block coordinates - const int block_x = global_2d.x() / vps; - const int block_y = global_2d.y() / vps; - const int local_x = global_2d.x() % vps; - const int local_y = global_2d.y() % vps; - // Get vertical range in voxel coordinates const auto min_key = layer.getVoxelKey(spatial_hash::Point(0, 0, min_z)); const auto max_key = layer.getVoxelKey(spatial_hash::Point(0, 0, max_z)); // Scan from top to bottom to find highest surface for (int block_z = max_key.first.z(); block_z >= min_key.first.z(); --block_z) { - const auto tsdf_block = layer.getBlockPtr(BlockIndex(block_x, block_y, block_z)); + const auto tsdf_block = layer.getBlockPtr( + BlockIndex(block_2d_index.x(), block_2d_index.y(), block_z)); if (!tsdf_block) { continue; } @@ -175,15 +176,16 @@ std::optional TsdfGradientOccupancyPublisher::extractSurfaceHeight( 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_x, local_y, 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_x, block_y, block_z), - VoxelIndex(local_x, local_y, z)); + 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(); } } @@ -199,18 +201,26 @@ void TsdfGradientOccupancyPublisher::buildHeightMap( Index2DMap& height_map) const { const int vps = static_cast(layer.voxels_per_side); - // Iterate over all allocated TSDF blocks + // Deduplicate to unique 2D blocks (matching GradientTraversabilityEstimator pattern) + spatial_hash::IndexSet blocks_2d; for (const auto& block : layer) { - // For each 2D column in the block + blocks_2d.emplace(block.index.x(), block.index.y(), 0); + } + + // Process each 2D column ONCE + for (const auto& block_2d : blocks_2d) { for (int local_x = 0; local_x < vps; ++local_x) { for (int local_y = 0; local_y < vps; ++local_y) { - // Compute global 2D index - const Index2D global_2d(block.index.x() * vps + local_x, - block.index.y() * vps + local_y); + const VoxelIndex local_2d(local_x, local_y, 0); + + // Pass block/local indices directly (avoids division/modulo bug) + auto surface_height = + extractSurfaceHeight(layer, block_2d, local_2d, min_z, max_z); - // Extract surface height for this column - auto surface_height = extractSurfaceHeight(layer, global_2d, min_z, max_z); if (surface_height) { + // Compute global 2D index for storage (matches GradientTraversabilityEstimator:301-302) + const Index2D global_2d(block_2d.x() * vps + local_x, + block_2d.y() * vps + local_y); height_map[global_2d] = *surface_height; } } From cd417d76a5b7451eb88da076de54efc2ea943c8e Mon Sep 17 00:00:00 2001 From: Multyxu Date: Wed, 15 Apr 2026 00:24:08 -0400 Subject: [PATCH 05/12] A fixed state, something wrong with global indexing? --- .../tsdf_gradient_occupancy_publisher.cpp | 89 ++++++++++--------- 1 file changed, 45 insertions(+), 44 deletions(-) diff --git a/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp index c356342..2f7747f 100644 --- a/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp +++ b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp @@ -201,27 +201,23 @@ void TsdfGradientOccupancyPublisher::buildHeightMap( Index2DMap& height_map) const { const int vps = static_cast(layer.voxels_per_side); - // Deduplicate to unique 2D blocks (matching GradientTraversabilityEstimator pattern) + // Deduplicate to unique 2D blocks (exact pattern from GradientTraversabilityEstimator:293) spatial_hash::IndexSet blocks_2d; - for (const auto& block : layer) { - blocks_2d.emplace(block.index.x(), block.index.y(), 0); + for (const auto& block_index : layer.allocatedBlockIndices()) { + blocks_2d.emplace(block_index.x(), block_index.y(), 0); } - // Process each 2D column ONCE - for (const auto& block_2d : blocks_2d) { - for (int local_x = 0; local_x < vps; ++local_x) { - for (int local_y = 0; local_y < vps; ++local_y) { - const VoxelIndex local_2d(local_x, local_y, 0); - - // Pass block/local indices directly (avoids division/modulo bug) - auto surface_height = - extractSurfaceHeight(layer, block_2d, local_2d, min_z, max_z); + // Process each 2D column ONCE (matches GradientTraversabilityEstimator:294-307) + for (const auto& block_idx_2d : blocks_2d) { + for (int x = 0; x < vps; ++x) { + for (int y = 0; y < vps; ++y) { + std::optional surface_height = + extractSurfaceHeight(layer, block_idx_2d, VoxelIndex(x, y, 0), min_z, max_z); if (surface_height) { - // Compute global 2D index for storage (matches GradientTraversabilityEstimator:301-302) - const Index2D global_2d(block_2d.x() * vps + local_x, - block_2d.y() * vps + local_y); - height_map[global_2d] = *surface_height; + const Index2D key(block_idx_2d.x() * vps + x, + block_idx_2d.y() * vps + y); + height_map[key] = *surface_height; } } } @@ -295,22 +291,19 @@ void TsdfGradientOccupancyPublisher::fillOccupancyGrid( return; } - // Compute bounds + // Compute bounds using block origins (matches getLayerBounds pattern) Eigen::Vector2f x_min = Eigen::Vector2f::Constant(std::numeric_limits::max()); Eigen::Vector2f x_max = Eigen::Vector2f::Constant(std::numeric_limits::lowest()); const float voxel_size = layer.voxel_size; - for (const auto& [idx, _] : gradient_map) { - const Eigen::Vector2f pos = idx.cast() * voxel_size; - x_min = x_min.array().min(pos.array()); - x_max = x_max.array().max(pos.array()); + for (const auto& block : layer) { + const auto lower = block.origin(); + const auto upper = lower + spatial_hash::Point::Constant(block.block_size); + x_min = x_min.array().min(lower.template head<2>().array()); + x_max = x_max.array().max(upper.template head<2>().array()); } - // Add one voxel margin - x_min -= Eigen::Vector2f::Constant(voxel_size); - x_max += Eigen::Vector2f::Constant(voxel_size); - const Eigen::Vector2f dims = (x_max - x_min) / voxel_size; // Initialize grid @@ -333,7 +326,11 @@ void TsdfGradientOccupancyPublisher::fillOccupancyGrid( // Fill occupancy grid for (const auto& [global_idx, gradient_info] : gradient_map) { - const Eigen::Vector2f pos = global_idx.cast() * voxel_size; + // Convert global 2D index to 3D, get voxel key, then actual position + const spatial_hash::GlobalIndex global_3d(global_idx.x(), global_idx.y(), 0); + const VoxelKey key = spatial_hash::keyFromGlobalIndex(global_3d, layer.voxels_per_side); + const Eigen::Vector3f pos_3d = layer.getVoxelPosition(key); + const Eigen::Vector2f pos = pos_3d.head<2>(); const Eigen::Vector2f rel_pos = pos - x_min; const auto r = std::floor(rel_pos.y() / voxel_size); @@ -419,21 +416,19 @@ void TsdfGradientOccupancyPublisher::publishHeightMapViz( max_height = std::max(max_height, height); } - // Compute bounds + // Compute bounds using block origins (matches getLayerBounds pattern) Eigen::Vector2f x_min = Eigen::Vector2f::Constant(std::numeric_limits::max()); Eigen::Vector2f x_max = Eigen::Vector2f::Constant(std::numeric_limits::lowest()); const float voxel_size = layer.voxel_size; - for (const auto& [idx, _] : height_map) { - const Eigen::Vector2f pos = idx.cast() * voxel_size; - x_min = x_min.array().min(pos.array()); - x_max = x_max.array().max(pos.array()); + for (const auto& block : layer) { + const auto lower = block.origin(); + const auto upper = lower + spatial_hash::Point::Constant(block.block_size); + x_min = x_min.array().min(lower.template head<2>().array()); + x_max = x_max.array().max(upper.template head<2>().array()); } - x_min -= Eigen::Vector2f::Constant(voxel_size); - x_max += Eigen::Vector2f::Constant(voxel_size); - const Eigen::Vector2f dims = (x_max - x_min) / voxel_size; // Initialize grid @@ -453,7 +448,11 @@ void TsdfGradientOccupancyPublisher::publishHeightMapViz( // Fill grid with normalized heights const float height_range = max_height - min_height; for (const auto& [global_idx, height] : height_map) { - const Eigen::Vector2f pos = global_idx.cast() * voxel_size; + // Convert global 2D index to actual voxel position + const spatial_hash::GlobalIndex global_3d(global_idx.x(), global_idx.y(), 0); + const VoxelKey key = spatial_hash::keyFromGlobalIndex(global_3d, layer.voxels_per_side); + const Eigen::Vector3f pos_3d = layer.getVoxelPosition(key); + const Eigen::Vector2f pos = pos_3d.head<2>(); const Eigen::Vector2f rel_pos = pos - x_min; const auto r = std::floor(rel_pos.y() / voxel_size); @@ -488,21 +487,19 @@ void TsdfGradientOccupancyPublisher::publishGradientMapViz( return; } - // Compute bounds + // Compute bounds using block origins (matches getLayerBounds pattern) Eigen::Vector2f x_min = Eigen::Vector2f::Constant(std::numeric_limits::max()); Eigen::Vector2f x_max = Eigen::Vector2f::Constant(std::numeric_limits::lowest()); const float voxel_size = layer.voxel_size; - for (const auto& [idx, _] : gradient_map) { - const Eigen::Vector2f pos = idx.cast() * voxel_size; - x_min = x_min.array().min(pos.array()); - x_max = x_max.array().max(pos.array()); + for (const auto& block : layer) { + const auto lower = block.origin(); + const auto upper = lower + spatial_hash::Point::Constant(block.block_size); + x_min = x_min.array().min(lower.template head<2>().array()); + x_max = x_max.array().max(upper.template head<2>().array()); } - x_min -= Eigen::Vector2f::Constant(voxel_size); - x_max += Eigen::Vector2f::Constant(voxel_size); - const Eigen::Vector2f dims = (x_max - x_min) / voxel_size; // Initialize grid @@ -521,7 +518,11 @@ void TsdfGradientOccupancyPublisher::publishGradientMapViz( // Fill grid with gradient values for (const auto& [global_idx, gradient_info] : gradient_map) { - const Eigen::Vector2f pos = global_idx.cast() * voxel_size; + // Convert global 2D index to actual voxel position + const spatial_hash::GlobalIndex global_3d(global_idx.x(), global_idx.y(), 0); + const VoxelKey key = spatial_hash::keyFromGlobalIndex(global_3d, layer.voxels_per_side); + const Eigen::Vector3f pos_3d = layer.getVoxelPosition(key); + const Eigen::Vector2f pos = pos_3d.head<2>(); const Eigen::Vector2f rel_pos = pos - x_min; const auto r = std::floor(rel_pos.y() / voxel_size); From 1fd22e6f910e3282c506d9ccd7687314a85a8c47 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Wed, 15 Apr 2026 07:48:44 -0400 Subject: [PATCH 06/12] pre-commit --- .../tsdf_gradient_occupancy_publisher.h | 8 +-- .../tsdf_gradient_occupancy_publisher.cpp | 51 +++++++++---------- 2 files changed, 29 insertions(+), 30 deletions(-) diff --git a/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h b/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h index e4220af..38c8dc0 100644 --- a/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h +++ b/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h @@ -38,11 +38,12 @@ #include #include +#include + #include #include #include -#include namespace hydra { @@ -74,7 +75,7 @@ class TsdfGradientOccupancyPublisher : public ReconstructionModule::Sink { bool collate = false; bool use_relative_height = true; double slice_height = -1.0; - size_t num_slices = 20; // if if 10 cm, we want 2 m from -1 m to +1 m + size_t num_slices = 20; // if if 10 cm, we want 2 m from -1 m to +1 m bool add_robot_footprint = false; Eigen::Vector3f footprint_min = Eigen::Vector3f::Zero(); Eigen::Vector3f footprint_max = Eigen::Vector3f::Zero(); @@ -131,8 +132,7 @@ class TsdfGradientOccupancyPublisher : public ReconstructionModule::Sink { const TsdfLayer& layer, uint64_t timestamp_ns) const; - float computeHorizontalDistance(const Index2D& offset, - float voxel_size) const; + float computeHorizontalDistance(const Index2D& offset, float voxel_size) const; float computeTraversabilityFromGradient(float gradient) const; diff --git a/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp index 2f7747f..91b09b9 100644 --- a/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp +++ b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp @@ -61,8 +61,8 @@ const std::array kNeighborOffsetsArray = {{ } // namespace -const std::array - TsdfGradientOccupancyPublisher::kNeighborOffsets = kNeighborOffsetsArray; +const std::array TsdfGradientOccupancyPublisher::kNeighborOffsets = + kNeighborOffsetsArray; void declare_config(TsdfGradientOccupancyPublisher::Config& config) { using namespace config; @@ -90,16 +90,14 @@ TsdfGradientOccupancyPublisher::TsdfGradientOccupancyPublisher(const Config& con : config(config::checkValid(config)), pub_(ianvs::NodeHandle::this_node(config.ns) .create_publisher( - "occupancy", - rclcpp::QoS(1).transient_local())), + "occupancy", rclcpp::QoS(1).transient_local())), height_map_pub_(ianvs::NodeHandle::this_node(config.ns) .create_publisher( - "height_map_debug", - rclcpp::QoS(1).transient_local())), - gradient_map_pub_(ianvs::NodeHandle::this_node(config.ns) - .create_publisher( - "gradient_map_debug", - rclcpp::QoS(1).transient_local())) {} + "height_map_debug", rclcpp::QoS(1).transient_local())), + gradient_map_pub_( + ianvs::NodeHandle::this_node(config.ns) + .create_publisher( + "gradient_map_debug", rclcpp::QoS(1).transient_local())) {} std::string TsdfGradientOccupancyPublisher::printInfo() const { return config::toString(config); @@ -165,15 +163,14 @@ std::optional TsdfGradientOccupancyPublisher::extractSurfaceHeight( // Scan from top to bottom to find highest surface 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)); + 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; + 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 = @@ -201,7 +198,8 @@ void TsdfGradientOccupancyPublisher::buildHeightMap( Index2DMap& height_map) const { const int vps = static_cast(layer.voxels_per_side); - // Deduplicate to unique 2D blocks (exact pattern from GradientTraversabilityEstimator:293) + // Deduplicate to unique 2D blocks (exact pattern from + // GradientTraversabilityEstimator:293) spatial_hash::IndexSet blocks_2d; for (const auto& block_index : layer.allocatedBlockIndices()) { blocks_2d.emplace(block_index.x(), block_index.y(), 0); @@ -211,12 +209,11 @@ void TsdfGradientOccupancyPublisher::buildHeightMap( for (const auto& block_idx_2d : blocks_2d) { for (int x = 0; x < vps; ++x) { for (int y = 0; y < vps; ++y) { - std::optional surface_height = - extractSurfaceHeight(layer, block_idx_2d, VoxelIndex(x, y, 0), min_z, max_z); + std::optional surface_height = extractSurfaceHeight( + layer, block_idx_2d, VoxelIndex(x, y, 0), min_z, max_z); if (surface_height) { - const Index2D key(block_idx_2d.x() * vps + x, - block_idx_2d.y() * vps + y); + const Index2D key(block_idx_2d.x() * vps + x, block_idx_2d.y() * vps + y); height_map[key] = *surface_height; } } @@ -238,7 +235,7 @@ void TsdfGradientOccupancyPublisher::computeGradientMap( for (const auto& offset : kNeighborOffsets) { const Index2D neighbor_idx(center_idx.x() + offset.x(), - center_idx.y() + offset.y()); + center_idx.y() + offset.y()); auto it = height_map.find(neighbor_idx); if (it != height_map.end()) { height_sum += it->second; @@ -260,7 +257,7 @@ void TsdfGradientOccupancyPublisher::computeGradientMap( for (const auto& offset : kNeighborOffsets) { const Index2D neighbor_idx(center_idx.x() + offset.x(), - center_idx.y() + offset.y()); + center_idx.y() + offset.y()); auto neighbor_it = grad_height_map.find(neighbor_idx); if (neighbor_it == grad_height_map.end()) { @@ -328,7 +325,8 @@ void TsdfGradientOccupancyPublisher::fillOccupancyGrid( for (const auto& [global_idx, gradient_info] : gradient_map) { // Convert global 2D index to 3D, get voxel key, then actual position const spatial_hash::GlobalIndex global_3d(global_idx.x(), global_idx.y(), 0); - const VoxelKey key = spatial_hash::keyFromGlobalIndex(global_3d, layer.voxels_per_side); + const VoxelKey key = + spatial_hash::keyFromGlobalIndex(global_3d, layer.voxels_per_side); const Eigen::Vector3f pos_3d = layer.getVoxelPosition(key); const Eigen::Vector2f pos = pos_3d.head<2>(); const Eigen::Vector2f rel_pos = pos - x_min; @@ -357,8 +355,7 @@ void TsdfGradientOccupancyPublisher::fillOccupancyGrid( } float TsdfGradientOccupancyPublisher::computeHorizontalDistance( - const Index2D& offset, - float voxel_size) const { + const Index2D& offset, float voxel_size) const { // Diagonal: sqrt(2) * voxel_size if (offset.x() != 0 && offset.y() != 0) { return voxel_size * std::sqrt(2.0f); @@ -450,7 +447,8 @@ void TsdfGradientOccupancyPublisher::publishHeightMapViz( for (const auto& [global_idx, height] : height_map) { // Convert global 2D index to actual voxel position const spatial_hash::GlobalIndex global_3d(global_idx.x(), global_idx.y(), 0); - const VoxelKey key = spatial_hash::keyFromGlobalIndex(global_3d, layer.voxels_per_side); + const VoxelKey key = + spatial_hash::keyFromGlobalIndex(global_3d, layer.voxels_per_side); const Eigen::Vector3f pos_3d = layer.getVoxelPosition(key); const Eigen::Vector2f pos = pos_3d.head<2>(); const Eigen::Vector2f rel_pos = pos - x_min; @@ -520,7 +518,8 @@ void TsdfGradientOccupancyPublisher::publishGradientMapViz( for (const auto& [global_idx, gradient_info] : gradient_map) { // Convert global 2D index to actual voxel position const spatial_hash::GlobalIndex global_3d(global_idx.x(), global_idx.y(), 0); - const VoxelKey key = spatial_hash::keyFromGlobalIndex(global_3d, layer.voxels_per_side); + const VoxelKey key = + spatial_hash::keyFromGlobalIndex(global_3d, layer.voxels_per_side); const Eigen::Vector3f pos_3d = layer.getVoxelPosition(key); const Eigen::Vector2f pos = pos_3d.head<2>(); const Eigen::Vector2f rel_pos = pos - x_min; From 231b911c953395779815dae590e6e7105686b184 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Wed, 22 Apr 2026 16:21:34 -0400 Subject: [PATCH 07/12] only consider conntected components as free space. --- .../tsdf_gradient_occupancy_publisher.h | 4 + .../tsdf_gradient_occupancy_publisher.cpp | 84 +++++++++++++++++++ 2 files changed, 88 insertions(+) diff --git a/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h b/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h index 38c8dc0..6ac1bd5 100644 --- a/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h +++ b/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h @@ -86,6 +86,7 @@ class TsdfGradientOccupancyPublisher : public ReconstructionModule::Sink { float min_confidence = 0.5f; // min confidence (neighbors/8) for valid cell bool smoothing = true; // apply box filter to reduce TSDF ripple bool probabilistic = false; // continuous vs binary occupancy + bool filter_disjoint = false; // remove free space not connected to robot } const config; explicit TsdfGradientOccupancyPublisher(const Config& config); @@ -124,6 +125,9 @@ class TsdfGradientOccupancyPublisher : public ReconstructionModule::Sink { const TsdfLayer& layer, nav_msgs::msg::OccupancyGrid& msg) const; + void filterDisjointFreeSpace(nav_msgs::msg::OccupancyGrid& msg, + const Eigen::Isometry3d& world_T_body) const; + void publishHeightMapViz(const Index2DMap& height_map, const TsdfLayer& layer, uint64_t timestamp_ns) const; diff --git a/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp index 91b09b9..594f04f 100644 --- a/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp +++ b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp @@ -41,6 +41,7 @@ #include #include +#include #include namespace hydra { @@ -80,6 +81,7 @@ void declare_config(TsdfGradientOccupancyPublisher::Config& config) { field(config.min_confidence, "min_confidence"); field(config.smoothing, "smoothing"); field(config.probabilistic, "probabilistic"); + field(config.filter_disjoint, "filter_disjoint"); checkCondition(config.gradient_threshold > 0.0f, "gradient_threshold must be positive"); @@ -141,6 +143,9 @@ void TsdfGradientOccupancyPublisher::call(uint64_t timestamp_ns, msg.info.map_load_time = msg.header.stamp; fillOccupancyGrid(gradient_map, world_T_body, tsdf_layer, msg); + if (config.filter_disjoint) { + filterDisjointFreeSpace(msg, world_T_body); + } pub_->publish(msg); } @@ -354,6 +359,85 @@ void TsdfGradientOccupancyPublisher::fillOccupancyGrid( } } +void TsdfGradientOccupancyPublisher::filterDisjointFreeSpace( + nav_msgs::msg::OccupancyGrid& msg, + const Eigen::Isometry3d& world_T_body) const { + if (msg.data.empty()) { + return; + } + + const int width = static_cast(msg.info.width); + const int height = static_cast(msg.info.height); + + // Convert robot world position to grid coordinates + const auto robot_pos_world = world_T_body.translation(); + const float rel_x = + static_cast(robot_pos_world.x() - msg.info.origin.position.x); + const float rel_y = + static_cast(robot_pos_world.y() - msg.info.origin.position.y); + const int robot_r = static_cast(std::floor(rel_y / msg.info.resolution)); + const int robot_c = static_cast(std::floor(rel_x / msg.info.resolution)); + + // Check if robot is within grid bounds + if (robot_r < 0 || robot_r >= height || robot_c < 0 || robot_c >= width) { + return; // Robot outside grid - skip filtering + } + + const size_t robot_index = robot_r * width + robot_c; + + // Only filter if robot is in free space + if (msg.data[robot_index] != 0) { + return; // Robot not in free space - skip filtering + } + + // BFS flood fill from robot position + std::vector visited(msg.data.size(), false); + std::queue to_visit; + + to_visit.push(robot_index); + visited[robot_index] = true; + + while (!to_visit.empty()) { + const size_t current_index = to_visit.front(); + to_visit.pop(); + + const int r = current_index / width; + const int c = current_index % width; + + // Check 4-connected neighbors (up, down, left, right) + const std::array, 4> neighbors = {{ + {r - 1, c}, {r + 1, c}, {r, c - 1}, {r, c + 1} + }}; + + for (const auto& [nr, nc] : neighbors) { + // Check bounds + if (nr < 0 || nr >= height || nc < 0 || nc >= width) { + continue; + } + + const size_t neighbor_index = nr * width + nc; + + // Skip if already visited + if (visited[neighbor_index]) { + continue; + } + + // Only expand through free cells (value 0) + if (msg.data[neighbor_index] == 0) { + visited[neighbor_index] = true; + to_visit.push(neighbor_index); + } + } + } + + // Mark unvisited free cells as occupied + for (size_t i = 0; i < msg.data.size(); ++i) { + if (msg.data[i] == 0 && !visited[i]) { + msg.data[i] = 100; // Mark as occupied + } + } +} + float TsdfGradientOccupancyPublisher::computeHorizontalDistance( const Index2D& offset, float voxel_size) const { // Diagonal: sqrt(2) * voxel_size From 50928444043384929b0be81c589b1aba469b46d5 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Wed, 22 Apr 2026 16:22:18 -0400 Subject: [PATCH 08/12] pre-commit --- .../active_window/tsdf_gradient_occupancy_publisher.h | 2 +- .../active_window/tsdf_gradient_occupancy_publisher.cpp | 8 +++----- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h b/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h index 6ac1bd5..b2fc137 100644 --- a/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h +++ b/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h @@ -86,7 +86,7 @@ class TsdfGradientOccupancyPublisher : public ReconstructionModule::Sink { float min_confidence = 0.5f; // min confidence (neighbors/8) for valid cell bool smoothing = true; // apply box filter to reduce TSDF ripple bool probabilistic = false; // continuous vs binary occupancy - bool filter_disjoint = false; // remove free space not connected to robot + bool filter_disjoint = false; // remove free space not connected to robot } const config; explicit TsdfGradientOccupancyPublisher(const Config& config); diff --git a/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp index 594f04f..8835bbb 100644 --- a/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp +++ b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp @@ -360,8 +360,7 @@ void TsdfGradientOccupancyPublisher::fillOccupancyGrid( } void TsdfGradientOccupancyPublisher::filterDisjointFreeSpace( - nav_msgs::msg::OccupancyGrid& msg, - const Eigen::Isometry3d& world_T_body) const { + nav_msgs::msg::OccupancyGrid& msg, const Eigen::Isometry3d& world_T_body) const { if (msg.data.empty()) { return; } @@ -405,9 +404,8 @@ void TsdfGradientOccupancyPublisher::filterDisjointFreeSpace( const int c = current_index % width; // Check 4-connected neighbors (up, down, left, right) - const std::array, 4> neighbors = {{ - {r - 1, c}, {r + 1, c}, {r, c - 1}, {r, c + 1} - }}; + const std::array, 4> neighbors = { + {{r - 1, c}, {r + 1, c}, {r, c - 1}, {r, c + 1}}}; for (const auto& [nr, nc] : neighbors) { // Check bounds From 660f905d54c8b3b9ce78421cec07986d91eb7b07 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Thu, 25 Jun 2026 16:21:15 -0400 Subject: [PATCH 09/12] Add base config to traversability --- hydra_ros/src/places/external_traversability_estimator.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/hydra_ros/src/places/external_traversability_estimator.cpp b/hydra_ros/src/places/external_traversability_estimator.cpp index f331d04..1cf52fa 100644 --- a/hydra_ros/src/places/external_traversability_estimator.cpp +++ b/hydra_ros/src/places/external_traversability_estimator.cpp @@ -58,7 +58,8 @@ void declare_config(ExternalTraversabilityEstimator::Config& config) { } ExternalTraversabilityEstimator::ExternalTraversabilityEstimator(const Config& config) - : config(config::checkValid(config)) { + : TraversabilityEstimator(TraversabilityEstimator::Config{}), + config(config::checkValid(config)) { auto nh = ianvs::NodeHandle::this_node(); sub_ = nh.create_subscription( config.input_topic, From 28077e46b86a3923ac6e29d70e612ef0c08a8f84 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Thu, 25 Jun 2026 18:25:52 -0400 Subject: [PATCH 10/12] Use publisher as traversability sink, but the map is not the same size as active window. --- .../tsdf_gradient_occupancy_publisher.h | 69 +---- .../tsdf_gradient_occupancy_publisher.cpp | 258 ++---------------- 2 files changed, 31 insertions(+), 296 deletions(-) diff --git a/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h b/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h index b2fc137..b8cdf45 100644 --- a/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h +++ b/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h @@ -33,13 +33,10 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include -#include +#include #include #include -#include - #include #include @@ -47,44 +44,19 @@ namespace hydra { -using Index2D = Eigen::Vector2i; - -struct Index2DHash { - inline static const auto s = Index2D(1, 1290); - int operator()(const Index2D& index) const { return index.dot(s); } -}; - -template -using Index2DMap = - std::unordered_map, - Eigen::aligned_allocator>>; - -struct GradientInfo { - float gradient = 0.0f; // mean gradient magnitude - float confidence = 0.0f; // num_neighbors / 8.0 -}; - -class TsdfGradientOccupancyPublisher : public ReconstructionModule::Sink { +class TsdfGradientOccupancyPublisher + : public hydra::places::GradientTraversabilityEstimator::Sink { public: struct Config { - // Base occupancy config fields std::string ns = "~/tsdf_gradient"; bool collate = false; bool use_relative_height = true; - double slice_height = -1.0; - size_t num_slices = 20; // if if 10 cm, we want 2 m from -1 m to +1 m - bool add_robot_footprint = false; + bool add_robot_footprint = false; // force voxels around robot to be free Eigen::Vector3f footprint_min = Eigen::Vector3f::Zero(); Eigen::Vector3f footprint_max = Eigen::Vector3f::Zero(); - // Gradient-specific parameters float gradient_threshold = 0.5f; // m/m - max traversable gradient - float min_weight = 1.0e-6f; // min TSDF weight for observed voxel float min_confidence = 0.5f; // min confidence (neighbors/8) for valid cell - bool smoothing = true; // apply box filter to reduce TSDF ripple bool probabilistic = false; // continuous vs binary occupancy bool filter_disjoint = false; // remove free space not connected to robot } const config; @@ -95,8 +67,8 @@ class TsdfGradientOccupancyPublisher : public ReconstructionModule::Sink { std::string printInfo() const override; - void call(uint64_t timestamp_ns, - const VolumetricMap& map, + void call(const hydra::places::HeightMap& height_map, + const hydra::places::GradientMap& gradient_map, const ActiveWindowOutput& output) const override; private: @@ -104,23 +76,7 @@ class TsdfGradientOccupancyPublisher : public ReconstructionModule::Sink { rclcpp::Publisher::SharedPtr height_map_pub_; rclcpp::Publisher::SharedPtr gradient_map_pub_; - // Helper functions - std::optional extractSurfaceHeight(const TsdfLayer& layer, - const BlockIndex& block_2d_index, - const VoxelIndex& local_2d, - float min_z, - float max_z) const; - - void buildHeightMap(const TsdfLayer& layer, - float min_z, - float max_z, - Index2DMap& height_map) const; - - void computeGradientMap(const Index2DMap& height_map, - float voxel_size, - Index2DMap& gradient_map) const; - - void fillOccupancyGrid(const Index2DMap& gradient_map, + void fillOccupancyGrid(const hydra::places::GradientMap& gradient_map, const Eigen::Isometry3d& world_T_sensor, const TsdfLayer& layer, nav_msgs::msg::OccupancyGrid& msg) const; @@ -128,22 +84,15 @@ class TsdfGradientOccupancyPublisher : public ReconstructionModule::Sink { void filterDisjointFreeSpace(nav_msgs::msg::OccupancyGrid& msg, const Eigen::Isometry3d& world_T_body) const; - void publishHeightMapViz(const Index2DMap& height_map, + void publishHeightMapViz(const hydra::places::HeightMap& height_map, const TsdfLayer& layer, uint64_t timestamp_ns) const; - void publishGradientMapViz(const Index2DMap& gradient_map, + void publishGradientMapViz(const hydra::places::GradientMap& gradient_map, const TsdfLayer& layer, uint64_t timestamp_ns) const; - float computeHorizontalDistance(const Index2D& offset, float voxel_size) const; - - float computeTraversabilityFromGradient(float gradient) const; - int8_t gradientToOccupancy(float gradient, float confidence) const; - - // 8-way neighbor offsets - static const std::array kNeighborOffsets; }; void declare_config(TsdfGradientOccupancyPublisher::Config& config); diff --git a/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp index 8835bbb..5571351 100644 --- a/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp +++ b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp @@ -46,24 +46,10 @@ namespace hydra { -namespace { - -// 8-way neighbor offsets (cardinal + diagonal) -const std::array kNeighborOffsetsArray = {{ - {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 - -const std::array TsdfGradientOccupancyPublisher::kNeighborOffsets = - kNeighborOffsetsArray; +using hydra::places::GradientInfo; +using hydra::places::GradientMap; +using hydra::places::HeightMap; +using hydra::places::Index2D; void declare_config(TsdfGradientOccupancyPublisher::Config& config) { using namespace config; @@ -71,15 +57,11 @@ void declare_config(TsdfGradientOccupancyPublisher::Config& config) { field(config.ns, "ns"); field(config.collate, "collate"); field(config.use_relative_height, "use_relative_height"); - field(config.slice_height, "slice_height", "m"); - field(config.num_slices, "num_slices"); field(config.add_robot_footprint, "add_robot_footprint"); field(config.footprint_min, "footprint_min"); field(config.footprint_max, "footprint_max"); field(config.gradient_threshold, "gradient_threshold"); - field(config.min_weight, "min_weight"); field(config.min_confidence, "min_confidence"); - field(config.smoothing, "smoothing"); field(config.probabilistic, "probabilistic"); field(config.filter_disjoint, "filter_disjoint"); @@ -105,43 +87,25 @@ std::string TsdfGradientOccupancyPublisher::printInfo() const { return config::toString(config); } -void TsdfGradientOccupancyPublisher::call(uint64_t timestamp_ns, - const VolumetricMap& map, +void TsdfGradientOccupancyPublisher::call(const HeightMap& height_map, + const GradientMap& gradient_map, const ActiveWindowOutput& output) const { if (!pub_->get_subscription_count() && !height_map_pub_->get_subscription_count() && !gradient_map_pub_->get_subscription_count()) { return; } - const auto& tsdf_layer = map.getTsdfLayer(); + const uint64_t timestamp_ns = output.timestamp_ns; + const auto& tsdf_layer = output.map().getTsdfLayer(); const auto& world_T_body = output.world_T_body(); - // Compute vertical range - double base_z = config.slice_height; - if (config.use_relative_height) { - base_z += world_T_body.translation().z(); - } - - const float voxel_size = tsdf_layer.voxel_size; - const float min_z = static_cast(base_z); - const float max_z = static_cast(base_z + config.num_slices * voxel_size); - - // Build height map (Pass 1) - Index2DMap height_map; - buildHeightMap(tsdf_layer, min_z, max_z, height_map); publishHeightMapViz(height_map, tsdf_layer, timestamp_ns); - - // Compute gradient map (Pass 2) - Index2DMap gradient_map; - computeGradientMap(height_map, voxel_size, gradient_map); publishGradientMapViz(gradient_map, tsdf_layer, timestamp_ns); - // Fill and publish occupancy grid (Pass 3) nav_msgs::msg::OccupancyGrid msg; msg.header.frame_id = GlobalInfo::instance().getFrames().map; msg.header.stamp = rclcpp::Time(timestamp_ns); msg.info.map_load_time = msg.header.stamp; - fillOccupancyGrid(gradient_map, world_T_body, tsdf_layer, msg); if (config.filter_disjoint) { filterDisjointFreeSpace(msg, world_T_body); @@ -149,143 +113,8 @@ void TsdfGradientOccupancyPublisher::call(uint64_t timestamp_ns, pub_->publish(msg); } -std::optional TsdfGradientOccupancyPublisher::extractSurfaceHeight( - const TsdfLayer& layer, - const BlockIndex& block_2d_index, - const VoxelIndex& local_2d, - float min_z, - float max_z) const { - // Uses block/local index access directly (same pattern as - // GradientTraversabilityEstimator) to avoid the signed/unsigned division bug in - // spatial_hash::blockIndexFromGlobalIndex, which corrupts getVoxelPtr lookups for - // negative world coordinates. - const float voxel_size = layer.voxel_size; - const int vps = static_cast(layer.voxels_per_side); - - // Get vertical range in voxel coordinates - const auto min_key = layer.getVoxelKey(spatial_hash::Point(0, 0, min_z)); - const auto max_key = layer.getVoxelKey(spatial_hash::Point(0, 0, max_z)); - - // Scan from top to bottom to find highest surface - 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 < 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 layer.getVoxelPosition(key).z(); - } - } - } - - return std::nullopt; -} - -void TsdfGradientOccupancyPublisher::buildHeightMap( - const TsdfLayer& layer, - float min_z, - float max_z, - Index2DMap& height_map) const { - const int vps = static_cast(layer.voxels_per_side); - - // Deduplicate to unique 2D blocks (exact pattern from - // GradientTraversabilityEstimator:293) - spatial_hash::IndexSet blocks_2d; - for (const auto& block_index : layer.allocatedBlockIndices()) { - blocks_2d.emplace(block_index.x(), block_index.y(), 0); - } - - // Process each 2D column ONCE (matches GradientTraversabilityEstimator:294-307) - for (const auto& block_idx_2d : blocks_2d) { - for (int x = 0; x < vps; ++x) { - for (int y = 0; y < vps; ++y) { - std::optional surface_height = extractSurfaceHeight( - layer, block_idx_2d, VoxelIndex(x, y, 0), min_z, max_z); - - if (surface_height) { - const Index2D key(block_idx_2d.x() * vps + x, block_idx_2d.y() * vps + y); - height_map[key] = *surface_height; - } - } - } - } -} - -void TsdfGradientOccupancyPublisher::computeGradientMap( - const Index2DMap& height_map, - float voxel_size, - Index2DMap& gradient_map) const { - // Optional smoothing pass - 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) { - const Index2D neighbor_idx(center_idx.x() + offset.x(), - center_idx.y() + offset.y()); - auto it = height_map.find(neighbor_idx); - if (it != height_map.end()) { - height_sum += it->second; - count++; - } - } - - smoothed_height_map[center_idx] = height_sum / count; - } - } - - const auto& grad_height_map = config.smoothing ? smoothed_height_map : height_map; - - // Compute gradients - gradient_map.reserve(grad_height_map.size()); - for (const auto& [center_idx, center_height] : grad_height_map) { - 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()); - - 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, voxel_size); - gradient_sum += height_diff / horiz_dist; - } - - GradientInfo info; - info.gradient = - num_neighbors_observed > 0 ? gradient_sum / num_neighbors_observed : 0.0f; - info.confidence = num_neighbors_observed / 8.0f; - gradient_map[center_idx] = info; - } -} - void TsdfGradientOccupancyPublisher::fillOccupancyGrid( - const Index2DMap& gradient_map, + const GradientMap& gradient_map, const Eigen::Isometry3d& world_T_sensor, const TsdfLayer& layer, nav_msgs::msg::OccupancyGrid& msg) const { @@ -293,7 +122,7 @@ void TsdfGradientOccupancyPublisher::fillOccupancyGrid( return; } - // Compute bounds using block origins (matches getLayerBounds pattern) + // Compute bounds using block origins Eigen::Vector2f x_min = Eigen::Vector2f::Constant(std::numeric_limits::max()); Eigen::Vector2f x_max = Eigen::Vector2f::Constant(std::numeric_limits::lowest()); @@ -314,7 +143,7 @@ void TsdfGradientOccupancyPublisher::fillOccupancyGrid( msg.info.height = std::ceil(dims.y()); msg.info.origin.position.x = x_min.x(); msg.info.origin.position.y = x_min.y(); - msg.info.origin.position.z = config.slice_height; + msg.info.origin.position.z = 0.0f; msg.info.origin.orientation.w = 1.0; msg.data.resize(msg.info.width * msg.info.height, -1); @@ -344,7 +173,6 @@ void TsdfGradientOccupancyPublisher::fillOccupancyGrid( continue; } - // Check robot footprint if (config.add_robot_footprint) { const Eigen::Vector3f pos_3d(pos.x(), pos.y(), 0.0f); if (bbox.contains((sensor_T_world * pos_3d).eval())) { @@ -353,9 +181,7 @@ void TsdfGradientOccupancyPublisher::fillOccupancyGrid( } } - // Map gradient to occupancy - msg.data[index] = - gradientToOccupancy(gradient_info.gradient, gradient_info.confidence); + msg.data[index] = gradientToOccupancy(gradient_info.gradient, gradient_info.confidence); } } @@ -368,7 +194,6 @@ void TsdfGradientOccupancyPublisher::filterDisjointFreeSpace( const int width = static_cast(msg.info.width); const int height = static_cast(msg.info.height); - // Convert robot world position to grid coordinates const auto robot_pos_world = world_T_body.translation(); const float rel_x = static_cast(robot_pos_world.x() - msg.info.origin.position.x); @@ -392,7 +217,6 @@ void TsdfGradientOccupancyPublisher::filterDisjointFreeSpace( // BFS flood fill from robot position std::vector visited(msg.data.size(), false); std::queue to_visit; - to_visit.push(robot_index); visited[robot_index] = true; @@ -404,18 +228,16 @@ void TsdfGradientOccupancyPublisher::filterDisjointFreeSpace( const int c = current_index % width; // Check 4-connected neighbors (up, down, left, right) + // 4-way more conservative than 8-way const std::array, 4> neighbors = { {{r - 1, c}, {r + 1, c}, {r, c - 1}, {r, c + 1}}}; for (const auto& [nr, nc] : neighbors) { - // Check bounds if (nr < 0 || nr >= height || nc < 0 || nc >= width) { continue; } const size_t neighbor_index = nr * width + nc; - - // Skip if already visited if (visited[neighbor_index]) { continue; } @@ -436,49 +258,25 @@ void TsdfGradientOccupancyPublisher::filterDisjointFreeSpace( } } -float TsdfGradientOccupancyPublisher::computeHorizontalDistance( - const Index2D& offset, float voxel_size) const { - // 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 TsdfGradientOccupancyPublisher::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); -} - int8_t TsdfGradientOccupancyPublisher::gradientToOccupancy(float gradient, float confidence) const { - // Check confidence threshold if (confidence < config.min_confidence) { - return -1; // Unknown + return -1; } if (config.probabilistic) { - // Continuous mode: map traversability to occupancy - const float traversability = computeTraversabilityFromGradient(gradient); + const float traversability = + hydra::places::computeTraversabilityFromGradient(gradient, config.gradient_threshold); const float occupancy_float = (1.0f - traversability) * 100.0f; return static_cast(std::clamp(occupancy_float, 0.0f, 100.0f)); } else { - // Binary mode: threshold-based return gradient >= config.gradient_threshold ? 100 : 0; } } -void TsdfGradientOccupancyPublisher::publishHeightMapViz( - const Index2DMap& height_map, - const TsdfLayer& layer, - uint64_t timestamp_ns) const { +void TsdfGradientOccupancyPublisher::publishHeightMapViz(const HeightMap& height_map, + const TsdfLayer& layer, + uint64_t timestamp_ns) const { if (!height_map_pub_->get_subscription_count()) { return; } @@ -487,7 +285,6 @@ void TsdfGradientOccupancyPublisher::publishHeightMapViz( return; } - // Find min/max heights for normalization float min_height = std::numeric_limits::max(); float max_height = std::numeric_limits::lowest(); for (const auto& [_, height] : height_map) { @@ -495,7 +292,6 @@ void TsdfGradientOccupancyPublisher::publishHeightMapViz( max_height = std::max(max_height, height); } - // Compute bounds using block origins (matches getLayerBounds pattern) Eigen::Vector2f x_min = Eigen::Vector2f::Constant(std::numeric_limits::max()); Eigen::Vector2f x_max = Eigen::Vector2f::Constant(std::numeric_limits::lowest()); @@ -510,7 +306,6 @@ void TsdfGradientOccupancyPublisher::publishHeightMapViz( const Eigen::Vector2f dims = (x_max - x_min) / voxel_size; - // Initialize grid nav_msgs::msg::OccupancyGrid msg; msg.header.frame_id = GlobalInfo::instance().getFrames().map; msg.header.stamp = rclcpp::Time(timestamp_ns); @@ -524,10 +319,8 @@ void TsdfGradientOccupancyPublisher::publishHeightMapViz( msg.info.origin.orientation.w = 1.0; msg.data.resize(msg.info.width * msg.info.height, -1); - // Fill grid with normalized heights const float height_range = max_height - min_height; for (const auto& [global_idx, height] : height_map) { - // Convert global 2D index to actual voxel position const spatial_hash::GlobalIndex global_3d(global_idx.x(), global_idx.y(), 0); const VoxelKey key = spatial_hash::keyFromGlobalIndex(global_3d, layer.voxels_per_side); @@ -543,12 +336,11 @@ void TsdfGradientOccupancyPublisher::publishHeightMapViz( continue; } - // Normalize height to 0-100 range if (height_range > 1e-6f) { const float normalized = (height - min_height) / height_range * 100.0f; msg.data[index] = static_cast(std::clamp(normalized, 0.0f, 100.0f)); } else { - msg.data[index] = 50; // All same height -> mid-gray + msg.data[index] = 50; } } @@ -556,7 +348,7 @@ void TsdfGradientOccupancyPublisher::publishHeightMapViz( } void TsdfGradientOccupancyPublisher::publishGradientMapViz( - const Index2DMap& gradient_map, + const GradientMap& gradient_map, const TsdfLayer& layer, uint64_t timestamp_ns) const { if (!gradient_map_pub_->get_subscription_count()) { @@ -567,7 +359,6 @@ void TsdfGradientOccupancyPublisher::publishGradientMapViz( return; } - // Compute bounds using block origins (matches getLayerBounds pattern) Eigen::Vector2f x_min = Eigen::Vector2f::Constant(std::numeric_limits::max()); Eigen::Vector2f x_max = Eigen::Vector2f::Constant(std::numeric_limits::lowest()); @@ -582,7 +373,6 @@ void TsdfGradientOccupancyPublisher::publishGradientMapViz( const Eigen::Vector2f dims = (x_max - x_min) / voxel_size; - // Initialize grid nav_msgs::msg::OccupancyGrid msg; msg.header.frame_id = GlobalInfo::instance().getFrames().map; msg.header.stamp = rclcpp::Time(timestamp_ns); @@ -596,9 +386,7 @@ void TsdfGradientOccupancyPublisher::publishGradientMapViz( msg.info.origin.orientation.w = 1.0; msg.data.resize(msg.info.width * msg.info.height, -1); - // Fill grid with gradient values for (const auto& [global_idx, gradient_info] : gradient_map) { - // Convert global 2D index to actual voxel position const spatial_hash::GlobalIndex global_3d(global_idx.x(), global_idx.y(), 0); const VoxelKey key = spatial_hash::keyFromGlobalIndex(global_3d, layer.voxels_per_side); @@ -614,8 +402,6 @@ void TsdfGradientOccupancyPublisher::publishGradientMapViz( continue; } - // Map gradient to 0-100 (using gradient_threshold as max) - // Low gradient (flat) -> 0 (free), high gradient (steep) -> 100 (occupied) const float normalized = std::min(gradient_info.gradient / config.gradient_threshold, 1.0f) * 100.0f; msg.data[index] = static_cast(normalized); @@ -627,7 +413,7 @@ void TsdfGradientOccupancyPublisher::publishGradientMapViz( namespace { static const auto registration_ = - config::RegistrationWithConfig( "TsdfGradientOccupancyPublisher"); From 54bf2603221eb18f9067bdcd8cc8f77abcd14ca1 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Thu, 25 Jun 2026 18:27:21 -0400 Subject: [PATCH 11/12] pre-commit --- .../active_window/tsdf_gradient_occupancy_publisher.h | 2 +- .../active_window/tsdf_gradient_occupancy_publisher.cpp | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h b/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h index b8cdf45..89b02c0 100644 --- a/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h +++ b/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h @@ -51,7 +51,7 @@ class TsdfGradientOccupancyPublisher std::string ns = "~/tsdf_gradient"; bool collate = false; bool use_relative_height = true; - bool add_robot_footprint = false; // force voxels around robot to be free + bool add_robot_footprint = false; // force voxels around robot to be free Eigen::Vector3f footprint_min = Eigen::Vector3f::Zero(); Eigen::Vector3f footprint_max = Eigen::Vector3f::Zero(); diff --git a/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp index 5571351..903fe55 100644 --- a/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp +++ b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp @@ -181,7 +181,8 @@ void TsdfGradientOccupancyPublisher::fillOccupancyGrid( } } - msg.data[index] = gradientToOccupancy(gradient_info.gradient, gradient_info.confidence); + msg.data[index] = + gradientToOccupancy(gradient_info.gradient, gradient_info.confidence); } } @@ -265,8 +266,8 @@ int8_t TsdfGradientOccupancyPublisher::gradientToOccupancy(float gradient, } if (config.probabilistic) { - const float traversability = - hydra::places::computeTraversabilityFromGradient(gradient, config.gradient_threshold); + const float traversability = hydra::places::computeTraversabilityFromGradient( + gradient, config.gradient_threshold); const float occupancy_float = (1.0f - traversability) * 100.0f; return static_cast(std::clamp(occupancy_float, 0.0f, 100.0f)); } else { From 2eb8cca2f9e7d1d9a5b8d90a2d4705535de4845a Mon Sep 17 00:00:00 2001 From: Multyxu Date: Thu, 25 Jun 2026 18:59:50 -0400 Subject: [PATCH 12/12] making sure the map is the same size as active winodow. Publish at robot height. --- .../tsdf_gradient_occupancy_publisher.h | 9 +++++--- .../tsdf_gradient_occupancy_publisher.cpp | 23 +++++++++++-------- 2 files changed, 20 insertions(+), 12 deletions(-) diff --git a/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h b/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h index 89b02c0..6bf810a 100644 --- a/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h +++ b/hydra_ros/include/hydra_ros/active_window/tsdf_gradient_occupancy_publisher.h @@ -69,7 +69,8 @@ class TsdfGradientOccupancyPublisher void call(const hydra::places::HeightMap& height_map, const hydra::places::GradientMap& gradient_map, - const ActiveWindowOutput& output) const override; + const ActiveWindowOutput& output, + const TsdfLayer& tsdf_layer) const override; private: rclcpp::Publisher::SharedPtr pub_; @@ -86,11 +87,13 @@ class TsdfGradientOccupancyPublisher void publishHeightMapViz(const hydra::places::HeightMap& height_map, const TsdfLayer& layer, - uint64_t timestamp_ns) const; + uint64_t timestamp_ns, + float robot_z) const; void publishGradientMapViz(const hydra::places::GradientMap& gradient_map, const TsdfLayer& layer, - uint64_t timestamp_ns) const; + uint64_t timestamp_ns, + float robot_z) const; int8_t gradientToOccupancy(float gradient, float confidence) const; }; diff --git a/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp index 903fe55..e503a3c 100644 --- a/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp +++ b/hydra_ros/src/active_window/tsdf_gradient_occupancy_publisher.cpp @@ -89,18 +89,19 @@ std::string TsdfGradientOccupancyPublisher::printInfo() const { void TsdfGradientOccupancyPublisher::call(const HeightMap& height_map, const GradientMap& gradient_map, - const ActiveWindowOutput& output) const { + const ActiveWindowOutput& output, + const TsdfLayer& tsdf_layer) const { if (!pub_->get_subscription_count() && !height_map_pub_->get_subscription_count() && !gradient_map_pub_->get_subscription_count()) { return; } const uint64_t timestamp_ns = output.timestamp_ns; - const auto& tsdf_layer = output.map().getTsdfLayer(); const auto& world_T_body = output.world_T_body(); + const float robot_z = static_cast(world_T_body.translation().z()); - publishHeightMapViz(height_map, tsdf_layer, timestamp_ns); - publishGradientMapViz(gradient_map, tsdf_layer, timestamp_ns); + publishHeightMapViz(height_map, tsdf_layer, timestamp_ns, robot_z); + publishGradientMapViz(gradient_map, tsdf_layer, timestamp_ns, robot_z); nav_msgs::msg::OccupancyGrid msg; msg.header.frame_id = GlobalInfo::instance().getFrames().map; @@ -143,7 +144,9 @@ void TsdfGradientOccupancyPublisher::fillOccupancyGrid( msg.info.height = std::ceil(dims.y()); msg.info.origin.position.x = x_min.x(); msg.info.origin.position.y = x_min.y(); - msg.info.origin.position.z = 0.0f; + msg.info.origin.position.z = + config.use_relative_height ? static_cast(world_T_sensor.translation().z()) + : 0.0f; msg.info.origin.orientation.w = 1.0; msg.data.resize(msg.info.width * msg.info.height, -1); @@ -277,7 +280,8 @@ int8_t TsdfGradientOccupancyPublisher::gradientToOccupancy(float gradient, void TsdfGradientOccupancyPublisher::publishHeightMapViz(const HeightMap& height_map, const TsdfLayer& layer, - uint64_t timestamp_ns) const { + uint64_t timestamp_ns, + float robot_z) const { if (!height_map_pub_->get_subscription_count()) { return; } @@ -316,7 +320,7 @@ void TsdfGradientOccupancyPublisher::publishHeightMapViz(const HeightMap& height msg.info.height = std::ceil(dims.y()); msg.info.origin.position.x = x_min.x(); msg.info.origin.position.y = x_min.y(); - msg.info.origin.position.z = 0.0; + msg.info.origin.position.z = config.use_relative_height ? robot_z : 0.0f; msg.info.origin.orientation.w = 1.0; msg.data.resize(msg.info.width * msg.info.height, -1); @@ -351,7 +355,8 @@ void TsdfGradientOccupancyPublisher::publishHeightMapViz(const HeightMap& height void TsdfGradientOccupancyPublisher::publishGradientMapViz( const GradientMap& gradient_map, const TsdfLayer& layer, - uint64_t timestamp_ns) const { + uint64_t timestamp_ns, + float robot_z) const { if (!gradient_map_pub_->get_subscription_count()) { return; } @@ -383,7 +388,7 @@ void TsdfGradientOccupancyPublisher::publishGradientMapViz( msg.info.height = std::ceil(dims.y()); msg.info.origin.position.x = x_min.x(); msg.info.origin.position.y = x_min.y(); - msg.info.origin.position.z = 0.0; + msg.info.origin.position.z = config.use_relative_height ? robot_z : 0.0f; msg.info.origin.orientation.w = 1.0; msg.data.resize(msg.info.width * msg.info.height, -1);