From ce5c03eb0c5b8df6e425d658423c8e096b03d2f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kaan=20=C3=87olak?= Date: Wed, 5 Mar 2025 01:08:44 +0300 Subject: [PATCH 1/5] feat(autoware_pointcloud_preprocessor): add pointcloud_densifier package MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kaan Çolak --- .../CMakeLists.txt | 15 + .../README.md | 2 + .../config/pointcloud_densifier.param.yaml | 8 + .../docs/pointcloud-densifier.md | 63 ++++ .../pointcloud_densifier/occupancy_grid.hpp | 48 +++ .../pointcloud_densifier_node.hpp | 77 ++++ .../launch/pointcloud_densifier.launch.py | 68 ++++ .../package.xml | 1 + .../src/filter.cpp | 2 +- .../pointcloud_densifier/occupancy_grid.cpp | 84 +++++ .../pointcloud_densifier_node.cpp | 337 ++++++++++++++++++ 11 files changed, 704 insertions(+), 1 deletion(-) create mode 100644 sensing/autoware_pointcloud_preprocessor/config/pointcloud_densifier.param.yaml create mode 100644 sensing/autoware_pointcloud_preprocessor/docs/pointcloud-densifier.md create mode 100644 sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/occupancy_grid.hpp create mode 100644 sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/pointcloud_densifier_node.hpp create mode 100644 sensing/autoware_pointcloud_preprocessor/launch/pointcloud_densifier.launch.py create mode 100644 sensing/autoware_pointcloud_preprocessor/src/pointcloud_densifier/occupancy_grid.cpp create mode 100644 sensing/autoware_pointcloud_preprocessor/src/pointcloud_densifier/pointcloud_densifier_node.cpp diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 98d66bf63addf..61e9f1fbf8dea 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(Sophus REQUIRED) find_package(Boost REQUIRED) find_package(PCL REQUIRED) find_package(CGAL REQUIRED COMPONENTS Core) +find_package(tf2_sensor_msgs REQUIRED) include_directories( include @@ -90,6 +91,8 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/polygon_remover/polygon_remover.cpp src/vector_map_filter/vector_map_inside_area_filter_node.cpp src/utility/geometry.cpp + src/pointcloud_densifier/pointcloud_densifier_node.cpp + src/pointcloud_densifier/occupancy_grid.cpp ) target_link_libraries(pointcloud_preprocessor_filter @@ -99,6 +102,7 @@ target_link_libraries(pointcloud_preprocessor_filter ${OpenCV_LIBRARIES} ${Sophus_LIBRARIES} ${PCL_LIBRARIES} + ${tf2_sensor_msgs_LIBRARIES} ) # ========== Time synchronizer ========== @@ -206,6 +210,17 @@ rclcpp_components_register_node(pointcloud_preprocessor_filter PLUGIN "autoware::pointcloud_preprocessor::VectorMapInsideAreaFilterComponent" EXECUTABLE vector_map_inside_area_filter_node) +# ========== Pointcloud Densifier ========== +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "autoware::pointcloud_preprocessor::PointCloudDensifierNode" + EXECUTABLE pointcloud_densifier_node) + +# Make sure launch directory is installed +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + install( TARGETS pointcloud_preprocessor_filter_base EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib diff --git a/sensing/autoware_pointcloud_preprocessor/README.md b/sensing/autoware_pointcloud_preprocessor/README.md index 4424259a52daa..1b4e61f91fb47 100644 --- a/sensing/autoware_pointcloud_preprocessor/README.md +++ b/sensing/autoware_pointcloud_preprocessor/README.md @@ -9,6 +9,7 @@ The `autoware_pointcloud_preprocessor` is a package that includes the following - concatenating pointclouds - correcting distortion - downsampling +- densifying pointclouds ## Inner-workings / Algorithms @@ -23,6 +24,7 @@ Detail description of each filter's algorithm is in the following links. | outlier_filter | remove points caused by hardware problems, rain drops and small insects as a noise | [link](docs/outlier-filter.md) | | passthrough_filter | remove points on the outside of a range in given field (e.g. x, y, z, intensity) | [link](docs/passthrough-filter.md) | | pointcloud_accumulator | accumulate pointclouds for a given amount of time | [link](docs/pointcloud-accumulator.md) | +| pointcloud_densifier | enhance sparse point clouds by using information from previous frames | [link](docs/pointcloud-densifier.md) | | vector_map_filter | remove points on the outside of lane by using vector map | [link](docs/vector-map-filter.md) | | vector_map_inside_area_filter | remove points inside of vector map area that has given type by parameter | [link](docs/vector-map-inside-area-filter.md) | diff --git a/sensing/autoware_pointcloud_preprocessor/config/pointcloud_densifier.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/pointcloud_densifier.param.yaml new file mode 100644 index 0000000000000..8809efe2161bb --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/pointcloud_densifier.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + num_previous_frames: 2 + x_min: 80.0 # meters + x_max: 220.0 # meters + y_min: -20.0 # meters + y_max: 20.0 # meters + grid_resolution: 0.3 # meters \ No newline at end of file diff --git a/sensing/autoware_pointcloud_preprocessor/docs/pointcloud-densifier.md b/sensing/autoware_pointcloud_preprocessor/docs/pointcloud-densifier.md new file mode 100644 index 0000000000000..5b14db0b18fef --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/docs/pointcloud-densifier.md @@ -0,0 +1,63 @@ +# PointCloud Densifier + +## Purpose + +The `pointcloud_densifier` enhances sparse point cloud data by leveraging information from previous LiDAR frames, +creating a denser representation especially for long-range points. This is particularly useful for improving perception +of distant objects where LiDAR data tends to be sparse. + +## Inner-workings / Algorithm + +The algorithm works as follows: + +1. **ROI Filtering**: First filters the input point cloud to only keep points in a specific region of interest (ROI), + typically focused on the distant area in front of the vehicle. + +2. **Occupancy Grid Creation**: Creates a 2D occupancy grid from the filtered points to track which areas contain valid + points in the current frame. + +3. **Previous Frame Integration**: Transforms points from previous frames into the current frame's coordinate system + using TF transformations. + +4. **Selective Point Addition**: Adds points from previous frames only if they fall into grid cells that are occupied + in the current frame. This ensures that only relevant points are added, avoiding ghost points from dynamic objects. + +5. **Combined Output**: Returns a combined point cloud that includes both the current frame's points and selected + points from previous frames. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +|----------|--------------------------------|------------------------------| +| `input` | `sensor_msgs::msg::PointCloud2` | Input point cloud | + +### Output + +| Name | Type | Description | +|----------|--------------------------------|------------------------------| +| `output` | `sensor_msgs::msg::PointCloud2` | Densified point cloud output | + +## Parameters + +| Name | Type | Default Value | Description | +|----------------------|--------|--------------|---------------------------------------------------| +| `num_previous_frames` | int | 1 | Number of previous frames to consider | +| `x_min` | double | 80.0 | Minimum x coordinate of ROI in meters | +| `x_max` | double | 200.0 | Maximum x coordinate of ROI in meters | +| `y_min` | double | -20.0 | Minimum y coordinate of ROI in meters | +| `y_max` | double | 20.0 | Maximum y coordinate of ROI in meters | +| `grid_resolution` | double | 0.3 | Resolution of occupancy grid in meters | + +## Assumptions / Known limits + +- The filter assumes that the TF tree contains valid transformations between coordinate frames from previous point clouds to the current frame. +- Performance depends on the number of previous frames used - more frames increase density but also processing time. +- The filter performs best on static elements in the scene, as dynamic objects may create artifacts if they move between frames. +- The accuracy of the densification depends on the quality of the TF transformations and ego-vehicle motion estimation. + +## Usage + +The pointcloud_densifier can be launched using: + diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/occupancy_grid.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/occupancy_grid.hpp new file mode 100644 index 0000000000000..5784d207a3a3e --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/occupancy_grid.hpp @@ -0,0 +1,48 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_DENSIFIER__OCCUPANCY_GRID_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_DENSIFIER__OCCUPANCY_GRID_HPP_ + +#include +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/point_cloud2_iterator.hpp" + +namespace autoware::pointcloud_preprocessor +{ + +class OccupancyGrid +{ +public: + + OccupancyGrid(double x_min, double x_max, double y_min, double y_max, double resolution); + void updateOccupancy(const sensor_msgs::msg::PointCloud2& cloud); + bool isOccupied(double x, double y) const; + +private: + double x_min_; + double x_max_; + double y_min_; + double y_max_; + double resolution_; + size_t cols_; + size_t rows_; + std::vector grid_; + + size_t index(size_t row, size_t col) const; +}; + +} // namespace autoware::pointcloud_preprocessor + +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_DENSIFIER__OCCUPANCY_GRID_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/pointcloud_densifier_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/pointcloud_densifier_node.hpp new file mode 100644 index 0000000000000..371dea8b56bb2 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/pointcloud_densifier_node.hpp @@ -0,0 +1,77 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POINTCLOUD_DENSIFIER__POINTCLOUD_DENSIFIER_NODE_HPP_ +#define POINTCLOUD_DENSIFIER__POINTCLOUD_DENSIFIER_NODE_HPP_ + +#include +#include +#include + +#include "autoware/pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/transform_info.hpp" +#include "autoware/pointcloud_preprocessor/pointcloud_densifier/occupancy_grid.hpp" + +namespace autoware::pointcloud_preprocessor +{ + +class PointCloudDensifierNode : public Filter +{ +protected: + void filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; + + void faster_filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, + const TransformInfo & transform_info) override; + +private: + sensor_msgs::msg::PointCloud2::SharedPtr filterPointCloudByROI( + const PointCloud2ConstPtr & input_cloud, const IndicesPtr & indices = nullptr); + + void transformAndMergePreviousClouds( + const PointCloud2ConstPtr & current_msg, + const OccupancyGrid & occupancy_grid, + PointCloud2 & combined_cloud); + + void storeCurrentCloud( + const sensor_msgs::msg::PointCloud2::SharedPtr & filtered_cloud); + + bool isValidTransform(const Eigen::Matrix4d & transform) const; + + struct DensifierParam { + int num_previous_frames{1}; + double x_min{80.0}; + double x_max{200.0}; + double y_min{-20.0}; + double y_max{20.0}; + double grid_resolution{0.3}; + } param_; + + std::deque previous_pointclouds_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); + +public: + PCL_MAKE_ALIGNED_OPERATOR_NEW + explicit PointCloudDensifierNode(const rclcpp::NodeOptions & options); +}; + +} // namespace autoware::pointcloud_preprocessor + +#endif // POINTCLOUD_DENSIFIER__POINTCLOUD_DENSIFIER_NODE_HPP_ \ No newline at end of file diff --git a/sensing/autoware_pointcloud_preprocessor/launch/pointcloud_densifier.launch.py b/sensing/autoware_pointcloud_preprocessor/launch/pointcloud_densifier.launch.py new file mode 100644 index 0000000000000..2a604a383ac4f --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/pointcloud_densifier.launch.py @@ -0,0 +1,68 @@ +# Copyright 2025 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +import os +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + """Launch the pointcloud densifier node.""" + pkg_prefix = get_package_share_directory('autoware_pointcloud_preprocessor') + config_file = os.path.join(pkg_prefix, 'config/pointcloud_densifier.param.yaml') + + input_topic = DeclareLaunchArgument( + "input_topic", + default_value="/sensing/lidar/concatenated/pointcloud", + description="Input pointcloud topic", + ) + + output_topic = DeclareLaunchArgument( + "output_topic", + default_value="/sensing/lidar/densified/pointcloud", + description="Output pointcloud topic", + ) + + # Create the composable node + component = ComposableNode( + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::PointCloudDensifierNode", + name="pointcloud_densifier", + remappings=[ + ("input", LaunchConfiguration("input_topic")), + ("output", LaunchConfiguration("output_topic")), + ], + parameters=[config_file], + ) + + # Create container + container = ComposableNodeContainer( + name="pointcloud_densifier_container", + namespace="", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[component], + output="screen", + ) + + return launch.LaunchDescription([ + # Launch arguments + input_topic, + output_topic, + # Nodes + container, + ]) \ No newline at end of file diff --git a/sensing/autoware_pointcloud_preprocessor/package.xml b/sensing/autoware_pointcloud_preprocessor/package.xml index 3efffe688e1b9..5679c3532e8ad 100644 --- a/sensing/autoware_pointcloud_preprocessor/package.xml +++ b/sensing/autoware_pointcloud_preprocessor/package.xml @@ -14,6 +14,7 @@ David Wong Melike Tanrikulu Max Schmeller + Kaan Colak Apache License 2.0 LGPLv3 diff --git a/sensing/autoware_pointcloud_preprocessor/src/filter.cpp b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp index 03ce774df6c8c..ba5c52bde6bea 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/filter.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp @@ -140,7 +140,7 @@ void autoware::pointcloud_preprocessor::Filter::subscribe(const std::string & fi // each time a child class supports the faster version. // When all the child classes support the faster version, this workaround is deleted. std::set supported_nodes = { - "CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter", "ScanGroundFilter"}; + "CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter", "ScanGroundFilter", "PointCloudDensifier"}; auto callback = supported_nodes.find(filter_name) != supported_nodes.end() ? &Filter::faster_input_indices_callback : &Filter::input_indices_callback; diff --git a/sensing/autoware_pointcloud_preprocessor/src/pointcloud_densifier/occupancy_grid.cpp b/sensing/autoware_pointcloud_preprocessor/src/pointcloud_densifier/occupancy_grid.cpp new file mode 100644 index 0000000000000..a219e311ee65c --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/src/pointcloud_densifier/occupancy_grid.cpp @@ -0,0 +1,84 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/pointcloud_preprocessor/pointcloud_densifier/occupancy_grid.hpp" +#include +#include + +namespace autoware::pointcloud_preprocessor +{ + +OccupancyGrid::OccupancyGrid(double x_min, double x_max, double y_min, double y_max, double resolution) +: x_min_(x_min), x_max_(x_max), y_min_(y_min), y_max_(y_max), resolution_(resolution) +{ + if (resolution <= 0) { + throw std::invalid_argument("Resolution must be positive"); + } + if (x_min >= x_max || y_min >= y_max) { + throw std::invalid_argument("Invalid grid dimensions"); + } + + cols_ = static_cast(std::ceil((x_max_ - x_min_) / resolution_)); + rows_ = static_cast(std::ceil((y_max_ - y_min_) / resolution_)); + + grid_.resize(rows_ * cols_, false); +} + +void OccupancyGrid::updateOccupancy(const sensor_msgs::msg::PointCloud2& cloud) +{ + std::fill(grid_.begin(), grid_.end(), false); + + sensor_msgs::PointCloud2ConstIterator iter_x(cloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y) { + float x = *iter_x; + float y = *iter_y; + + if (x < x_min_ || x >= x_max_ || y < y_min_ || y >= y_max_) { + continue; + } + + size_t col = static_cast((x - x_min_) / resolution_); + size_t row = static_cast((y - y_min_) / resolution_); + + if (row < rows_ && col < cols_) { + grid_[index(row, col)] = true; + } + } +} + +bool OccupancyGrid::isOccupied(double x, double y) const +{ + + if (x < x_min_ || x >= x_max_ || y < y_min_ || y >= y_max_) { + return false; + } + + size_t col = static_cast((x - x_min_) / resolution_); + size_t row = static_cast((y - y_min_) / resolution_); + + if (row >= rows_ || col >= cols_) { + return false; + } + + return grid_[index(row, col)]; +} + +size_t OccupancyGrid::index(size_t row, size_t col) const +{ + return row * cols_ + col; +} + +} // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/src/pointcloud_densifier/pointcloud_densifier_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/pointcloud_densifier/pointcloud_densifier_node.cpp new file mode 100644 index 0000000000000..a88139c366959 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/src/pointcloud_densifier/pointcloud_densifier_node.cpp @@ -0,0 +1,337 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/pointcloud_preprocessor/pointcloud_densifier/pointcloud_densifier_node.hpp" + +#include +#include +#include +#include +#include + +#include +#include + +namespace autoware::pointcloud_preprocessor +{ + +PointCloudDensifierNode::PointCloudDensifierNode(const rclcpp::NodeOptions & options) +: Filter("PointCloudDensifier", options), + tf_buffer_(std::make_shared(this->get_clock())), + tf_listener_(std::make_shared(*tf_buffer_)) +{ + // Initialize debug tools + { + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, this->get_name()); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + } + + // Set initial parameters + { + param_.num_previous_frames = declare_parameter("num_previous_frames", 1); + param_.x_min = declare_parameter("x_min", 80.0); + param_.x_max = declare_parameter("x_max", 200.0); + param_.y_min = declare_parameter("y_min", -20.0); + param_.y_max = declare_parameter("y_max", 20.0); + param_.grid_resolution = declare_parameter("grid_resolution", 0.3); + + if (param_.num_previous_frames < 0) { + RCLCPP_WARN(get_logger(), "num_previous_frames must be non-negative. Setting to 0."); + param_.num_previous_frames = 0; + } + if (param_.x_min >= param_.x_max || param_.y_min >= param_.y_max) { + RCLCPP_ERROR(get_logger(), "Invalid ROI bounds: x_min must be less than x_max, and y_min must be less than y_max"); + throw std::invalid_argument("Invalid ROI bounds"); + } + if (param_.grid_resolution <= 0.0) { + RCLCPP_ERROR(get_logger(), "grid_resolution must be positive"); + throw std::invalid_argument("Invalid grid resolution"); + } + } + + // Set parameter service callback + { + using std::placeholders::_1; + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&PointCloudDensifierNode::paramCallback, this, _1)); + } +} + +// Legacy filter implementation - for backward compatibility +void PointCloudDensifierNode::filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) +{ + // This is not used as we're using faster_filter directly + (void)input; + (void)indices; + (void)output; +} + +// Optimized filter implementation using TransformInfo +void PointCloudDensifierNode::faster_filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, + const TransformInfo & transform_info) +{ + std::scoped_lock lock(mutex_); + stop_watch_ptr_->toc("processing_time", true); + + // No need this transformer + (void)transform_info; + + auto far_front_pointcloud_ptr = filterPointCloudByROI(input, indices); + + // Build occupancy grid from the current filtered cloud + OccupancyGrid occupancy_grid( + param_.x_min, param_.x_max, param_.y_min, param_.y_max, param_.grid_resolution); + occupancy_grid.updateOccupancy(*far_front_pointcloud_ptr); + + + output = *input; + + transformAndMergePreviousClouds(input, occupancy_grid, output); + + storeCurrentCloud(far_front_pointcloud_ptr); + + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + + auto pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) + .count(); + + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); + } +} + +sensor_msgs::msg::PointCloud2::SharedPtr PointCloudDensifierNode::filterPointCloudByROI( + const PointCloud2ConstPtr & input_cloud, const IndicesPtr & indices) +{ + auto filtered_cloud = std::make_shared(); + + filtered_cloud->header = input_cloud->header; + filtered_cloud->height = 1; + filtered_cloud->fields = input_cloud->fields; + filtered_cloud->is_bigendian = input_cloud->is_bigendian; + filtered_cloud->point_step = input_cloud->point_step; + filtered_cloud->is_dense = input_cloud->is_dense; + + int x_offset = input_cloud->fields[pcl::getFieldIndex(*input_cloud, "x")].offset; + int y_offset = input_cloud->fields[pcl::getFieldIndex(*input_cloud, "y")].offset; + + // Pre-allocate data for the filtered cloud + filtered_cloud->data.resize(input_cloud->data.size()); + size_t output_size = 0; + + if (indices && !indices->empty()) { + for (const auto & idx : *indices) { + const size_t data_offset = idx * input_cloud->point_step; + + float x, y; + std::memcpy(&x, &input_cloud->data[data_offset + x_offset], sizeof(float)); + std::memcpy(&y, &input_cloud->data[data_offset + y_offset], sizeof(float)); + + if (x > param_.x_min && x < param_.x_max && y > param_.y_min && y < param_.y_max) { + // Copy point data if within ROI + std::memcpy( + &filtered_cloud->data[output_size], + &input_cloud->data[data_offset], + input_cloud->point_step + ); + output_size += input_cloud->point_step; + } + } + } else { + for (size_t i = 0; i < input_cloud->width * input_cloud->height; ++i) { + const size_t data_offset = i * input_cloud->point_step; + + // Get x,y coordinates + float x, y; + std::memcpy(&x, &input_cloud->data[data_offset + x_offset], sizeof(float)); + std::memcpy(&y, &input_cloud->data[data_offset + y_offset], sizeof(float)); + + if (x > param_.x_min && x < param_.x_max && y > param_.y_min && y < param_.y_max) { + // Copy point data if within ROI + std::memcpy( + &filtered_cloud->data[output_size], + &input_cloud->data[data_offset], + input_cloud->point_step + ); + output_size += input_cloud->point_step; + } + } + } + + // Set width based on actual number of points + filtered_cloud->width = output_size / filtered_cloud->point_step; + filtered_cloud->row_step = filtered_cloud->width * filtered_cloud->point_step; + filtered_cloud->data.resize(output_size); + + return filtered_cloud; +} + +void PointCloudDensifierNode::transformAndMergePreviousClouds( + const PointCloud2ConstPtr & current_msg, + const OccupancyGrid & occupancy_grid, + PointCloud2 & combined_cloud) +{ + int x_offset = current_msg->fields[pcl::getFieldIndex(*current_msg, "x")].offset; + int y_offset = current_msg->fields[pcl::getFieldIndex(*current_msg, "y")].offset; + + for (const auto& previous_cloud : previous_pointclouds_) { + if (!previous_cloud || previous_cloud->data.empty()) { + continue; + } + + geometry_msgs::msg::TransformStamped transform_stamped; + try { + tf2::TimePoint current_time_point = tf2::TimePoint( + std::chrono::nanoseconds(current_msg->header.stamp.nanosec) + + std::chrono::seconds(current_msg->header.stamp.sec)); + tf2::TimePoint prev_time_point = tf2::TimePoint( + std::chrono::nanoseconds(previous_cloud->header.stamp.nanosec) + + std::chrono::seconds(previous_cloud->header.stamp.sec)); + transform_stamped = tf_buffer_->lookupTransform( + current_msg->header.frame_id, + current_time_point, + previous_cloud->header.frame_id, + prev_time_point, + "map" + ); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(get_logger(), "Could not transform point cloud: %s", ex.what()); + continue; + } + + // Validate and obtain the transformation + Eigen::Isometry3d transform_eigen = tf2::transformToEigen(transform_stamped); + if (!isValidTransform(transform_eigen.matrix())) { + RCLCPP_WARN(get_logger(), "Invalid transform matrix, skipping point cloud"); + continue; + } + + // Transform previous point cloud to current frame + sensor_msgs::msg::PointCloud2 transformed_cloud; + tf2::doTransform(*previous_cloud, transformed_cloud, transform_stamped); + + // Get transformed cloud field offsets + x_offset = transformed_cloud.fields[pcl::getFieldIndex(transformed_cloud, "x")].offset; + y_offset = transformed_cloud.fields[pcl::getFieldIndex(transformed_cloud, "y")].offset; + + size_t original_size = combined_cloud.data.size(); + combined_cloud.data.resize(original_size + transformed_cloud.data.size()); + size_t output_size = original_size; + + // Add previous points only if they fall into occupied grid cells + for (size_t i = 0; i < transformed_cloud.width; ++i) { + size_t data_offset = i * transformed_cloud.point_step; + + float x, y; + std::memcpy(&x, &transformed_cloud.data[data_offset + x_offset], sizeof(float)); + std::memcpy(&y, &transformed_cloud.data[data_offset + y_offset], sizeof(float)); + + if (occupancy_grid.isOccupied(x, y)) { + std::memcpy( + &combined_cloud.data[output_size], + &transformed_cloud.data[data_offset], + transformed_cloud.point_step + ); + output_size += transformed_cloud.point_step; + } + } + + combined_cloud.data.resize(output_size); + } + + combined_cloud.width = combined_cloud.data.size() / combined_cloud.point_step; + combined_cloud.row_step = combined_cloud.width * combined_cloud.point_step; + combined_cloud.height = 1; +} + +void PointCloudDensifierNode::storeCurrentCloud( + const sensor_msgs::msg::PointCloud2::SharedPtr & filtered_cloud) +{ + if (previous_pointclouds_.size() >= static_cast(param_.num_previous_frames)) { + previous_pointclouds_.pop_front(); + } + previous_pointclouds_.push_back(filtered_cloud); +} + +bool PointCloudDensifierNode::isValidTransform(const Eigen::Matrix4d & transform) const +{ + return transform.allFinite() && + std::abs(transform.determinant() - 1.0) < 1e-3; // Check if it's a proper rigid transformation +} + +rcl_interfaces::msg::SetParametersResult PointCloudDensifierNode::paramCallback( + const std::vector & p) +{ + std::scoped_lock lock(mutex_); + + DensifierParam new_param = param_; + + if (get_param(p, "num_previous_frames", new_param.num_previous_frames)) { + if (new_param.num_previous_frames < 0) { + new_param.num_previous_frames = 0; + RCLCPP_WARN(get_logger(), "num_previous_frames must be non-negative. Setting to 0."); + } + } + + bool update_grid = false; + if (get_param(p, "x_min", new_param.x_min)) update_grid = true; + if (get_param(p, "x_max", new_param.x_max)) update_grid = true; + if (get_param(p, "y_min", new_param.y_min)) update_grid = true; + if (get_param(p, "y_max", new_param.y_max)) update_grid = true; + if (get_param(p, "grid_resolution", new_param.grid_resolution)) update_grid = true; + + if (update_grid) { + // Validate grid parameters + if (new_param.x_min >= new_param.x_max || new_param.y_min >= new_param.y_max) { + RCLCPP_ERROR(get_logger(), "Invalid ROI bounds: x_min must be less than x_max, and y_min must be less than y_max"); + rcl_interfaces::msg::SetParametersResult result; + result.successful = false; + result.reason = "Invalid ROI bounds"; + return result; + } + if (new_param.grid_resolution <= 0.0) { + RCLCPP_ERROR(get_logger(), "grid_resolution must be positive"); + rcl_interfaces::msg::SetParametersResult result; + result.successful = false; + result.reason = "Invalid grid resolution"; + return result; + } + } + + param_ = new_param; + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} + +} // namespace autoware::pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pointcloud_preprocessor::PointCloudDensifierNode) From 4ac59b9545b84f593a9e9ac96660fe78f86b83ba Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 4 Mar 2025 22:39:48 +0000 Subject: [PATCH 2/5] style(pre-commit): autofix --- .../CMakeLists.txt | 4 +- .../config/pointcloud_densifier.param.yaml | 2 +- .../docs/pointcloud-densifier.md | 31 +++--- .../pointcloud_densifier/occupancy_grid.hpp | 12 +-- .../pointcloud_densifier_node.hpp | 35 +++--- .../launch/pointcloud_densifier.launch.py | 28 ++--- .../src/filter.cpp | 3 +- .../pointcloud_densifier/occupancy_grid.cpp | 33 +++--- .../pointcloud_densifier_node.cpp | 102 +++++++++--------- 9 files changed, 124 insertions(+), 126 deletions(-) diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 61e9f1fbf8dea..49106fa767362 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -13,7 +13,7 @@ find_package(Sophus REQUIRED) find_package(Boost REQUIRED) find_package(PCL REQUIRED) find_package(CGAL REQUIRED COMPONENTS Core) -find_package(tf2_sensor_msgs REQUIRED) +find_package(tf2_sensor_msgs REQUIRED) include_directories( include @@ -102,7 +102,7 @@ target_link_libraries(pointcloud_preprocessor_filter ${OpenCV_LIBRARIES} ${Sophus_LIBRARIES} ${PCL_LIBRARIES} - ${tf2_sensor_msgs_LIBRARIES} + ${tf2_sensor_msgs_LIBRARIES} ) # ========== Time synchronizer ========== diff --git a/sensing/autoware_pointcloud_preprocessor/config/pointcloud_densifier.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/pointcloud_densifier.param.yaml index 8809efe2161bb..ea9c7a51af9a7 100644 --- a/sensing/autoware_pointcloud_preprocessor/config/pointcloud_densifier.param.yaml +++ b/sensing/autoware_pointcloud_preprocessor/config/pointcloud_densifier.param.yaml @@ -5,4 +5,4 @@ x_max: 220.0 # meters y_min: -20.0 # meters y_max: 20.0 # meters - grid_resolution: 0.3 # meters \ No newline at end of file + grid_resolution: 0.3 # meters diff --git a/sensing/autoware_pointcloud_preprocessor/docs/pointcloud-densifier.md b/sensing/autoware_pointcloud_preprocessor/docs/pointcloud-densifier.md index 5b14db0b18fef..4d82f4a349e05 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/pointcloud-densifier.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/pointcloud-densifier.md @@ -2,7 +2,7 @@ ## Purpose -The `pointcloud_densifier` enhances sparse point cloud data by leveraging information from previous LiDAR frames, +The `pointcloud_densifier` enhances sparse point cloud data by leveraging information from previous LiDAR frames, creating a denser representation especially for long-range points. This is particularly useful for improving perception of distant objects where LiDAR data tends to be sparse. @@ -10,10 +10,10 @@ of distant objects where LiDAR data tends to be sparse. The algorithm works as follows: -1. **ROI Filtering**: First filters the input point cloud to only keep points in a specific region of interest (ROI), +1. **ROI Filtering**: First filters the input point cloud to only keep points in a specific region of interest (ROI), typically focused on the distant area in front of the vehicle. -2. **Occupancy Grid Creation**: Creates a 2D occupancy grid from the filtered points to track which areas contain valid +2. **Occupancy Grid Creation**: Creates a 2D occupancy grid from the filtered points to track which areas contain valid points in the current frame. 3. **Previous Frame Integration**: Transforms points from previous frames into the current frame's coordinate system @@ -29,26 +29,26 @@ The algorithm works as follows: ### Input -| Name | Type | Description | -|----------|--------------------------------|------------------------------| -| `input` | `sensor_msgs::msg::PointCloud2` | Input point cloud | +| Name | Type | Description | +| ------- | ------------------------------- | ----------------- | +| `input` | `sensor_msgs::msg::PointCloud2` | Input point cloud | ### Output | Name | Type | Description | -|----------|--------------------------------|------------------------------| +| -------- | ------------------------------- | ---------------------------- | | `output` | `sensor_msgs::msg::PointCloud2` | Densified point cloud output | ## Parameters -| Name | Type | Default Value | Description | -|----------------------|--------|--------------|---------------------------------------------------| -| `num_previous_frames` | int | 1 | Number of previous frames to consider | -| `x_min` | double | 80.0 | Minimum x coordinate of ROI in meters | -| `x_max` | double | 200.0 | Maximum x coordinate of ROI in meters | -| `y_min` | double | -20.0 | Minimum y coordinate of ROI in meters | -| `y_max` | double | 20.0 | Maximum y coordinate of ROI in meters | -| `grid_resolution` | double | 0.3 | Resolution of occupancy grid in meters | +| Name | Type | Default Value | Description | +| --------------------- | ------ | ------------- | -------------------------------------- | +| `num_previous_frames` | int | 1 | Number of previous frames to consider | +| `x_min` | double | 80.0 | Minimum x coordinate of ROI in meters | +| `x_max` | double | 200.0 | Maximum x coordinate of ROI in meters | +| `y_min` | double | -20.0 | Minimum y coordinate of ROI in meters | +| `y_max` | double | 20.0 | Maximum y coordinate of ROI in meters | +| `grid_resolution` | double | 0.3 | Resolution of occupancy grid in meters | ## Assumptions / Known limits @@ -60,4 +60,3 @@ The algorithm works as follows: ## Usage The pointcloud_densifier can be launched using: - diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/occupancy_grid.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/occupancy_grid.hpp index 5784d207a3a3e..1b091b17cdcc9 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/occupancy_grid.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/occupancy_grid.hpp @@ -15,21 +15,21 @@ #ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_DENSIFIER__OCCUPANCY_GRID_HPP_ #define AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_DENSIFIER__OCCUPANCY_GRID_HPP_ -#include #include "sensor_msgs/msg/point_cloud2.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" +#include + namespace autoware::pointcloud_preprocessor { class OccupancyGrid { public: - OccupancyGrid(double x_min, double x_max, double y_min, double y_max, double resolution); - void updateOccupancy(const sensor_msgs::msg::PointCloud2& cloud); + void updateOccupancy(const sensor_msgs::msg::PointCloud2 & cloud); bool isOccupied(double x, double y) const; - + private: double x_min_; double x_max_; @@ -39,10 +39,10 @@ class OccupancyGrid size_t cols_; size_t rows_; std::vector grid_; - + size_t index(size_t row, size_t col) const; }; -} // namespace autoware::pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor #endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_DENSIFIER__OCCUPANCY_GRID_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/pointcloud_densifier_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/pointcloud_densifier_node.hpp index 371dea8b56bb2..811f2a64384d5 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/pointcloud_densifier_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/pointcloud_densifier_node.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINTCLOUD_DENSIFIER__POINTCLOUD_DENSIFIER_NODE_HPP_ -#define POINTCLOUD_DENSIFIER__POINTCLOUD_DENSIFIER_NODE_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_DENSIFIER__POINTCLOUD_DENSIFIER_NODE_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_DENSIFIER__POINTCLOUD_DENSIFIER_NODE_HPP_ + +#include "autoware/pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/pointcloud_densifier/occupancy_grid.hpp" +#include "autoware/pointcloud_preprocessor/transform_info.hpp" #include #include #include -#include "autoware/pointcloud_preprocessor/filter.hpp" -#include "autoware/pointcloud_preprocessor/transform_info.hpp" -#include "autoware/pointcloud_preprocessor/pointcloud_densifier/occupancy_grid.hpp" - namespace autoware::pointcloud_preprocessor { @@ -39,18 +39,17 @@ class PointCloudDensifierNode : public Filter private: sensor_msgs::msg::PointCloud2::SharedPtr filterPointCloudByROI( const PointCloud2ConstPtr & input_cloud, const IndicesPtr & indices = nullptr); - + void transformAndMergePreviousClouds( - const PointCloud2ConstPtr & current_msg, - const OccupancyGrid & occupancy_grid, + const PointCloud2ConstPtr & current_msg, const OccupancyGrid & occupancy_grid, PointCloud2 & combined_cloud); - - void storeCurrentCloud( - const sensor_msgs::msg::PointCloud2::SharedPtr & filtered_cloud); + + void storeCurrentCloud(const sensor_msgs::msg::PointCloud2::SharedPtr & filtered_cloud); bool isValidTransform(const Eigen::Matrix4d & transform) const; - - struct DensifierParam { + + struct DensifierParam + { int num_previous_frames{1}; double x_min{80.0}; double x_max{200.0}; @@ -58,12 +57,12 @@ class PointCloudDensifierNode : public Filter double y_max{20.0}; double grid_resolution{0.3}; } param_; - + std::deque previous_pointclouds_; - + std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; - + OnSetParametersCallbackHandle::SharedPtr set_param_res_; rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); @@ -74,4 +73,4 @@ class PointCloudDensifierNode : public Filter } // namespace autoware::pointcloud_preprocessor -#endif // POINTCLOUD_DENSIFIER__POINTCLOUD_DENSIFIER_NODE_HPP_ \ No newline at end of file +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__POINTCLOUD_DENSIFIER__POINTCLOUD_DENSIFIER_NODE_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/launch/pointcloud_densifier.launch.py b/sensing/autoware_pointcloud_preprocessor/launch/pointcloud_densifier.launch.py index 2a604a383ac4f..23e8276733cec 100644 --- a/sensing/autoware_pointcloud_preprocessor/launch/pointcloud_densifier.launch.py +++ b/sensing/autoware_pointcloud_preprocessor/launch/pointcloud_densifier.launch.py @@ -12,25 +12,27 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os + +from ament_index_python.packages import get_package_share_directory import launch from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode -import os -from ament_index_python.packages import get_package_share_directory + def generate_launch_description(): """Launch the pointcloud densifier node.""" - pkg_prefix = get_package_share_directory('autoware_pointcloud_preprocessor') - config_file = os.path.join(pkg_prefix, 'config/pointcloud_densifier.param.yaml') + pkg_prefix = get_package_share_directory("autoware_pointcloud_preprocessor") + config_file = os.path.join(pkg_prefix, "config/pointcloud_densifier.param.yaml") input_topic = DeclareLaunchArgument( "input_topic", default_value="/sensing/lidar/concatenated/pointcloud", description="Input pointcloud topic", ) - + output_topic = DeclareLaunchArgument( "output_topic", default_value="/sensing/lidar/densified/pointcloud", @@ -59,10 +61,12 @@ def generate_launch_description(): output="screen", ) - return launch.LaunchDescription([ - # Launch arguments - input_topic, - output_topic, - # Nodes - container, - ]) \ No newline at end of file + return launch.LaunchDescription( + [ + # Launch arguments + input_topic, + output_topic, + # Nodes + container, + ] + ) diff --git a/sensing/autoware_pointcloud_preprocessor/src/filter.cpp b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp index ba5c52bde6bea..4dea8bbfdcb38 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/filter.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp @@ -140,7 +140,8 @@ void autoware::pointcloud_preprocessor::Filter::subscribe(const std::string & fi // each time a child class supports the faster version. // When all the child classes support the faster version, this workaround is deleted. std::set supported_nodes = { - "CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter", "ScanGroundFilter", "PointCloudDensifier"}; + "CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter", "ScanGroundFilter", + "PointCloudDensifier"}; auto callback = supported_nodes.find(filter_name) != supported_nodes.end() ? &Filter::faster_input_indices_callback : &Filter::input_indices_callback; diff --git a/sensing/autoware_pointcloud_preprocessor/src/pointcloud_densifier/occupancy_grid.cpp b/sensing/autoware_pointcloud_preprocessor/src/pointcloud_densifier/occupancy_grid.cpp index a219e311ee65c..ec3a33d1a0710 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/pointcloud_densifier/occupancy_grid.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/pointcloud_densifier/occupancy_grid.cpp @@ -13,13 +13,15 @@ // limitations under the License. #include "autoware/pointcloud_preprocessor/pointcloud_densifier/occupancy_grid.hpp" -#include -#include + +#include +#include namespace autoware::pointcloud_preprocessor { -OccupancyGrid::OccupancyGrid(double x_min, double x_max, double y_min, double y_max, double resolution) +OccupancyGrid::OccupancyGrid( + double x_min, double x_max, double y_min, double y_max, double resolution) : x_min_(x_min), x_max_(x_max), y_min_(y_min), y_max_(y_max), resolution_(resolution) { if (resolution <= 0) { @@ -28,31 +30,31 @@ OccupancyGrid::OccupancyGrid(double x_min, double x_max, double y_min, double y_ if (x_min >= x_max || y_min >= y_max) { throw std::invalid_argument("Invalid grid dimensions"); } - + cols_ = static_cast(std::ceil((x_max_ - x_min_) / resolution_)); rows_ = static_cast(std::ceil((y_max_ - y_min_) / resolution_)); - + grid_.resize(rows_ * cols_, false); } -void OccupancyGrid::updateOccupancy(const sensor_msgs::msg::PointCloud2& cloud) +void OccupancyGrid::updateOccupancy(const sensor_msgs::msg::PointCloud2 & cloud) { std::fill(grid_.begin(), grid_.end(), false); - + sensor_msgs::PointCloud2ConstIterator iter_x(cloud, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); - + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y) { float x = *iter_x; float y = *iter_y; - + if (x < x_min_ || x >= x_max_ || y < y_min_ || y >= y_max_) { continue; } - + size_t col = static_cast((x - x_min_) / resolution_); size_t row = static_cast((y - y_min_) / resolution_); - + if (row < rows_ && col < cols_) { grid_[index(row, col)] = true; } @@ -61,18 +63,17 @@ void OccupancyGrid::updateOccupancy(const sensor_msgs::msg::PointCloud2& cloud) bool OccupancyGrid::isOccupied(double x, double y) const { - if (x < x_min_ || x >= x_max_ || y < y_min_ || y >= y_max_) { return false; } - + size_t col = static_cast((x - x_min_) / resolution_); size_t row = static_cast((y - y_min_) / resolution_); - + if (row >= rows_ || col >= cols_) { return false; } - + return grid_[index(row, col)]; } @@ -81,4 +82,4 @@ size_t OccupancyGrid::index(size_t row, size_t col) const return row * cols_ + col; } -} // namespace autoware::pointcloud_preprocessor +} // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/src/pointcloud_densifier/pointcloud_densifier_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/pointcloud_densifier/pointcloud_densifier_node.cpp index a88139c366959..2345fb8b76c3b 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/pointcloud_densifier/pointcloud_densifier_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/pointcloud_densifier/pointcloud_densifier_node.cpp @@ -14,11 +14,13 @@ #include "autoware/pointcloud_preprocessor/pointcloud_densifier/pointcloud_densifier_node.hpp" +#include + #include +#include + #include #include -#include -#include #include #include @@ -55,7 +57,9 @@ PointCloudDensifierNode::PointCloudDensifierNode(const rclcpp::NodeOptions & opt param_.num_previous_frames = 0; } if (param_.x_min >= param_.x_max || param_.y_min >= param_.y_max) { - RCLCPP_ERROR(get_logger(), "Invalid ROI bounds: x_min must be less than x_max, and y_min must be less than y_max"); + RCLCPP_ERROR( + get_logger(), + "Invalid ROI bounds: x_min must be less than x_max, and y_min must be less than y_max"); throw std::invalid_argument("Invalid ROI bounds"); } if (param_.grid_resolution <= 0.0) { @@ -90,18 +94,17 @@ void PointCloudDensifierNode::faster_filter( std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); - // No need this transformer + // No need this transformer (void)transform_info; auto far_front_pointcloud_ptr = filterPointCloudByROI(input, indices); - + // Build occupancy grid from the current filtered cloud OccupancyGrid occupancy_grid( param_.x_min, param_.x_max, param_.y_min, param_.y_max, param_.grid_resolution); occupancy_grid.updateOccupancy(*far_front_pointcloud_ptr); - - output = *input; + output = *input; transformAndMergePreviousClouds(input, occupancy_grid, output); @@ -129,81 +132,76 @@ sensor_msgs::msg::PointCloud2::SharedPtr PointCloudDensifierNode::filterPointClo const PointCloud2ConstPtr & input_cloud, const IndicesPtr & indices) { auto filtered_cloud = std::make_shared(); - + filtered_cloud->header = input_cloud->header; - filtered_cloud->height = 1; + filtered_cloud->height = 1; filtered_cloud->fields = input_cloud->fields; filtered_cloud->is_bigendian = input_cloud->is_bigendian; filtered_cloud->point_step = input_cloud->point_step; filtered_cloud->is_dense = input_cloud->is_dense; - + int x_offset = input_cloud->fields[pcl::getFieldIndex(*input_cloud, "x")].offset; int y_offset = input_cloud->fields[pcl::getFieldIndex(*input_cloud, "y")].offset; - + // Pre-allocate data for the filtered cloud filtered_cloud->data.resize(input_cloud->data.size()); size_t output_size = 0; - + if (indices && !indices->empty()) { for (const auto & idx : *indices) { const size_t data_offset = idx * input_cloud->point_step; - + float x, y; std::memcpy(&x, &input_cloud->data[data_offset + x_offset], sizeof(float)); std::memcpy(&y, &input_cloud->data[data_offset + y_offset], sizeof(float)); - + if (x > param_.x_min && x < param_.x_max && y > param_.y_min && y < param_.y_max) { // Copy point data if within ROI std::memcpy( - &filtered_cloud->data[output_size], - &input_cloud->data[data_offset], - input_cloud->point_step - ); + &filtered_cloud->data[output_size], &input_cloud->data[data_offset], + input_cloud->point_step); output_size += input_cloud->point_step; } } } else { for (size_t i = 0; i < input_cloud->width * input_cloud->height; ++i) { const size_t data_offset = i * input_cloud->point_step; - + // Get x,y coordinates float x, y; std::memcpy(&x, &input_cloud->data[data_offset + x_offset], sizeof(float)); std::memcpy(&y, &input_cloud->data[data_offset + y_offset], sizeof(float)); - + if (x > param_.x_min && x < param_.x_max && y > param_.y_min && y < param_.y_max) { // Copy point data if within ROI std::memcpy( - &filtered_cloud->data[output_size], - &input_cloud->data[data_offset], - input_cloud->point_step - ); + &filtered_cloud->data[output_size], &input_cloud->data[data_offset], + input_cloud->point_step); output_size += input_cloud->point_step; } } } - + // Set width based on actual number of points filtered_cloud->width = output_size / filtered_cloud->point_step; filtered_cloud->row_step = filtered_cloud->width * filtered_cloud->point_step; filtered_cloud->data.resize(output_size); - + return filtered_cloud; } void PointCloudDensifierNode::transformAndMergePreviousClouds( - const PointCloud2ConstPtr & current_msg, - const OccupancyGrid & occupancy_grid, + const PointCloud2ConstPtr & current_msg, const OccupancyGrid & occupancy_grid, PointCloud2 & combined_cloud) { int x_offset = current_msg->fields[pcl::getFieldIndex(*current_msg, "x")].offset; int y_offset = current_msg->fields[pcl::getFieldIndex(*current_msg, "y")].offset; - - for (const auto& previous_cloud : previous_pointclouds_) { + + for (const auto & previous_cloud : previous_pointclouds_) { if (!previous_cloud || previous_cloud->data.empty()) { continue; } - + geometry_msgs::msg::TransformStamped transform_stamped; try { tf2::TimePoint current_time_point = tf2::TimePoint( @@ -213,60 +211,54 @@ void PointCloudDensifierNode::transformAndMergePreviousClouds( std::chrono::nanoseconds(previous_cloud->header.stamp.nanosec) + std::chrono::seconds(previous_cloud->header.stamp.sec)); transform_stamped = tf_buffer_->lookupTransform( - current_msg->header.frame_id, - current_time_point, - previous_cloud->header.frame_id, - prev_time_point, - "map" - ); + current_msg->header.frame_id, current_time_point, previous_cloud->header.frame_id, + prev_time_point, "map"); } catch (tf2::TransformException & ex) { RCLCPP_WARN(get_logger(), "Could not transform point cloud: %s", ex.what()); continue; } - + // Validate and obtain the transformation Eigen::Isometry3d transform_eigen = tf2::transformToEigen(transform_stamped); if (!isValidTransform(transform_eigen.matrix())) { RCLCPP_WARN(get_logger(), "Invalid transform matrix, skipping point cloud"); continue; } - + // Transform previous point cloud to current frame sensor_msgs::msg::PointCloud2 transformed_cloud; tf2::doTransform(*previous_cloud, transformed_cloud, transform_stamped); - + // Get transformed cloud field offsets x_offset = transformed_cloud.fields[pcl::getFieldIndex(transformed_cloud, "x")].offset; y_offset = transformed_cloud.fields[pcl::getFieldIndex(transformed_cloud, "y")].offset; - + size_t original_size = combined_cloud.data.size(); combined_cloud.data.resize(original_size + transformed_cloud.data.size()); size_t output_size = original_size; - + // Add previous points only if they fall into occupied grid cells for (size_t i = 0; i < transformed_cloud.width; ++i) { size_t data_offset = i * transformed_cloud.point_step; - + float x, y; std::memcpy(&x, &transformed_cloud.data[data_offset + x_offset], sizeof(float)); std::memcpy(&y, &transformed_cloud.data[data_offset + y_offset], sizeof(float)); - + if (occupancy_grid.isOccupied(x, y)) { std::memcpy( - &combined_cloud.data[output_size], - &transformed_cloud.data[data_offset], - transformed_cloud.point_step - ); + &combined_cloud.data[output_size], &transformed_cloud.data[data_offset], + transformed_cloud.point_step); output_size += transformed_cloud.point_step; } } - + combined_cloud.data.resize(output_size); } - + combined_cloud.width = combined_cloud.data.size() / combined_cloud.point_step; combined_cloud.row_step = combined_cloud.width * combined_cloud.point_step; - combined_cloud.height = 1; + combined_cloud.height = 1; } void PointCloudDensifierNode::storeCurrentCloud( @@ -280,8 +272,8 @@ void PointCloudDensifierNode::storeCurrentCloud( bool PointCloudDensifierNode::isValidTransform(const Eigen::Matrix4d & transform) const { - return transform.allFinite() && - std::abs(transform.determinant() - 1.0) < 1e-3; // Check if it's a proper rigid transformation + return transform.allFinite() && std::abs(transform.determinant() - 1.0) < + 1e-3; // Check if it's a proper rigid transformation } rcl_interfaces::msg::SetParametersResult PointCloudDensifierNode::paramCallback( @@ -308,7 +300,9 @@ rcl_interfaces::msg::SetParametersResult PointCloudDensifierNode::paramCallback( if (update_grid) { // Validate grid parameters if (new_param.x_min >= new_param.x_max || new_param.y_min >= new_param.y_max) { - RCLCPP_ERROR(get_logger(), "Invalid ROI bounds: x_min must be less than x_max, and y_min must be less than y_max"); + RCLCPP_ERROR( + get_logger(), + "Invalid ROI bounds: x_min must be less than x_max, and y_min must be less than y_max"); rcl_interfaces::msg::SetParametersResult result; result.successful = false; result.reason = "Invalid ROI bounds"; From 5d356779f873ebb21824476ff4c8de8157424a58 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kaan=20=C3=87olak?= Date: Wed, 5 Mar 2025 11:08:04 +0300 Subject: [PATCH 3/5] fix(autoware_pointcloud_preprocessor): add header MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kaan Çolak --- .../pointcloud_densifier/pointcloud_densifier_node.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/pointcloud_densifier_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/pointcloud_densifier_node.hpp index 811f2a64384d5..7116eec63e61f 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/pointcloud_densifier_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/pointcloud_densifier_node.hpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace autoware::pointcloud_preprocessor { From 55cb56756a4fb5d79e9d0388069abe041591a8c0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kaan=20=C3=87olak?= Date: Wed, 5 Mar 2025 11:37:53 +0300 Subject: [PATCH 4/5] fix(autoware_pointcloud_preprocessor): add schema and fix debugger MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kaan Çolak --- .../config/pointcloud_densifier.param.yaml | 12 ++--- .../pointcloud_densifier_node.hpp | 3 ++ .../pointcloud_densifier_node.schema.json | 48 +++++++++++++++++++ .../pointcloud_densifier_node.cpp | 4 +- 4 files changed, 59 insertions(+), 8 deletions(-) create mode 100644 sensing/autoware_pointcloud_preprocessor/schema/pointcloud_densifier_node.schema.json diff --git a/sensing/autoware_pointcloud_preprocessor/config/pointcloud_densifier.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/pointcloud_densifier.param.yaml index ea9c7a51af9a7..38f09883c8f54 100644 --- a/sensing/autoware_pointcloud_preprocessor/config/pointcloud_densifier.param.yaml +++ b/sensing/autoware_pointcloud_preprocessor/config/pointcloud_densifier.param.yaml @@ -1,8 +1,8 @@ /**: ros__parameters: - num_previous_frames: 2 - x_min: 80.0 # meters - x_max: 220.0 # meters - y_min: -20.0 # meters - y_max: 20.0 # meters - grid_resolution: 0.3 # meters + num_previous_frames: 1 + x_min: 80.0 + x_max: 230.0 + y_min: -20.0 + y_max: 20.0 + grid_resolution: 0.3 diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/pointcloud_densifier_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/pointcloud_densifier_node.hpp index 7116eec63e61f..7d560684acf11 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/pointcloud_densifier_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/pointcloud_densifier/pointcloud_densifier_node.hpp @@ -59,6 +59,9 @@ class PointCloudDensifierNode : public Filter double grid_resolution{0.3}; } param_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; + std::deque previous_pointclouds_; std::shared_ptr tf_buffer_; diff --git a/sensing/autoware_pointcloud_preprocessor/schema/pointcloud_densifier_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/pointcloud_densifier_node.schema.json new file mode 100644 index 0000000000000..c4c1546e8a39d --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/pointcloud_densifier_node.schema.json @@ -0,0 +1,48 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for PointCloud Densifier Node", + "type": "object", + "properties": { + "num_previous_frames": { + "type": "integer", + "description": "Number of previous frames to use for densification", + "default": 1, + "minimum": 0 + }, + "x_min": { + "type": "number", + "description": "Minimum X coordinate (in meters) of the region of interest", + "default": 80.0 + }, + "x_max": { + "type": "number", + "description": "Maximum X coordinate (in meters) of the region of interest", + "default": 230.0 + }, + "y_min": { + "type": "number", + "description": "Minimum Y coordinate (in meters) of the region of interest", + "default": -20.0 + }, + "y_max": { + "type": "number", + "description": "Maximum Y coordinate (in meters) of the region of interest", + "default": 20.0 + }, + "grid_resolution": { + "type": "number", + "description": "Resolution (in meters) of the occupancy grid used for point selection", + "default": 0.3, + "exclusiveMinimum": 0 + } + }, + "required": [ + "num_previous_frames", + "x_min", + "x_max", + "y_min", + "y_max", + "grid_resolution" + ], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/src/pointcloud_densifier/pointcloud_densifier_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/pointcloud_densifier/pointcloud_densifier_node.cpp index 2345fb8b76c3b..9aa0d357c3492 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/pointcloud_densifier/pointcloud_densifier_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/pointcloud_densifier/pointcloud_densifier_node.cpp @@ -35,8 +35,8 @@ PointCloudDensifierNode::PointCloudDensifierNode(const rclcpp::NodeOptions & opt { // Initialize debug tools { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; + using autoware_utils::DebugPublisher; + using autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, this->get_name()); stop_watch_ptr_->tic("cyclic_time"); From 43bd7e4c687fcf58cfb0e148a9b871d8e08e2adf Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 5 Mar 2025 08:41:00 +0000 Subject: [PATCH 5/5] style(pre-commit): autofix --- .../schema/pointcloud_densifier_node.schema.json | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/sensing/autoware_pointcloud_preprocessor/schema/pointcloud_densifier_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/pointcloud_densifier_node.schema.json index c4c1546e8a39d..9d54091cc6041 100644 --- a/sensing/autoware_pointcloud_preprocessor/schema/pointcloud_densifier_node.schema.json +++ b/sensing/autoware_pointcloud_preprocessor/schema/pointcloud_densifier_node.schema.json @@ -36,13 +36,6 @@ "exclusiveMinimum": 0 } }, - "required": [ - "num_previous_frames", - "x_min", - "x_max", - "y_min", - "y_max", - "grid_resolution" - ], + "required": ["num_previous_frames", "x_min", "x_max", "y_min", "y_max", "grid_resolution"], "additionalProperties": false }