From 581ff53902399094ca4cd0f75902e47e7e973c4c Mon Sep 17 00:00:00 2001 From: Multyxu Date: Tue, 14 Apr 2026 22:47:30 -0400 Subject: [PATCH 1/7] 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 903eb899ef28b395c728474fc4dce8926aa6661c Mon Sep 17 00:00:00 2001 From: Multyxu Date: Tue, 14 Apr 2026 23:23:04 -0400 Subject: [PATCH 2/7] 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 2dbe3da29f91e41abc7a1f55b43eb32bc231bf55 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Tue, 14 Apr 2026 23:56:10 -0400 Subject: [PATCH 3/7] 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 1b613ed18f04bfaf87c63f6adff3660bc252af12 Mon Sep 17 00:00:00 2001 From: Multyxu Date: Wed, 15 Apr 2026 00:24:08 -0400 Subject: [PATCH 4/7] 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 6360b768f025de6708a658e59a47d2446ce7eafb Mon Sep 17 00:00:00 2001 From: Multyxu Date: Wed, 15 Apr 2026 07:48:44 -0400 Subject: [PATCH 5/7] 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 3b0e9a4ca7ee795614334a47b429300be74d44cd Mon Sep 17 00:00:00 2001 From: Multyxu Date: Wed, 22 Apr 2026 16:21:34 -0400 Subject: [PATCH 6/7] 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 e237833099e8772f0836928a39b3f775825d053b Mon Sep 17 00:00:00 2001 From: Multyxu Date: Wed, 22 Apr 2026 16:22:18 -0400 Subject: [PATCH 7/7] 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