diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 98d66bf63addf..cd048bd6c4959 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -63,6 +63,21 @@ ament_target_dependencies(faster_voxel_grid_downsample_filter sensor_msgs ) +add_library(roi_excluded_faster_voxel_grid_downsample_filter SHARED + src/downsample_filter/roi_excluded_faster_voxel_grid_downsample_filter.cpp +) + +target_include_directories(roi_excluded_faster_voxel_grid_downsample_filter PUBLIC + "$" + "$" +) + +ament_target_dependencies(roi_excluded_faster_voxel_grid_downsample_filter + pcl_conversions + rclcpp + sensor_msgs +) + ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/concatenate_data/concatenate_and_time_sync_node.cpp src/concatenate_data/combine_cloud_handler.cpp @@ -75,6 +90,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/downsample_filter/random_downsample_filter_node.cpp src/downsample_filter/approximate_downsample_filter_node.cpp src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp + src/downsample_filter/roi_excluded_voxel_grid_downsample_filter_node.cpp src/outlier_filter/ring_outlier_filter_node.cpp src/outlier_filter/radius_search_2d_outlier_filter_node.cpp src/outlier_filter/voxel_grid_outlier_filter_node.cpp @@ -95,6 +111,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED target_link_libraries(pointcloud_preprocessor_filter pointcloud_preprocessor_filter_base faster_voxel_grid_downsample_filter + roi_excluded_faster_voxel_grid_downsample_filter ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${Sophus_LIBRARIES} @@ -141,6 +158,11 @@ rclcpp_components_register_node(pointcloud_preprocessor_filter PLUGIN "autoware::pointcloud_preprocessor::ApproximateDownsampleFilterComponent" EXECUTABLE approximate_downsample_filter_node) +# -- ROI Excluded Voxel Grid Downsample Filter -- +rclcpp_components_register_node(pointcloud_preprocessor_filter + PLUGIN "autoware::pointcloud_preprocessor::RoiExcludedVoxelGridDownsampleFilterComponent" + EXECUTABLE roi_excluded_voxel_grid_downsample_filter_node) + # ========== Outlier Filter ========== # -- Ring Outlier Filter -- rclcpp_components_register_node(pointcloud_preprocessor_filter @@ -220,6 +242,13 @@ install( RUNTIME DESTINATION bin ) +install( + TARGETS roi_excluded_faster_voxel_grid_downsample_filter EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + install( DIRECTORY include/ DESTINATION include/${PROJECT_NAME} diff --git a/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md index f237a1871b5e5..66ff1b0fab895 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md @@ -22,6 +22,15 @@ The `downsample_filter` is a node that reduces the number of points. This algorithm samples a single actual point existing within the voxel, not the centroid. The computation cost is low compared to Centroid Based Voxel Grid Filter. +### ROI Excluded Voxel Grid Downsample Filter + +This filter provides selective downsampling that preserves detailed point data in critical regions while reducing overall point cloud density: + +1. Points are classified based on their position relative to a defined Region of Interest (ROI) +2. Points inside the ROI are preserved at their original density (no downsampling) +3. Points outside the ROI undergo standard voxel grid downsampling +4. The final output combines both sets of points + ## Inputs / Outputs These implementations inherit `autoware::pointcloud_preprocessor::Filter` class, please refer [README](../README.md). @@ -50,6 +59,10 @@ These implementations inherit `autoware::pointcloud_preprocessor::Filter` class, {{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/pickup_based_voxel_grid_downsample_filter_node.schema.json") }} +### ROI Excluded Voxel Grid Downsample Filter + +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/roi_excluded_voxel_grid_downsample_filter_node.schema.json") }} + ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/roi_excluded_faster_voxel_grid_downsample_filter.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/roi_excluded_faster_voxel_grid_downsample_filter.hpp new file mode 100644 index 0000000000000..a6a9c8bd2b1bc --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/roi_excluded_faster_voxel_grid_downsample_filter.hpp @@ -0,0 +1,91 @@ +// 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. + +#pragma once + +#include "autoware/pointcloud_preprocessor/transform_info.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::pointcloud_preprocessor +{ + +class RoiExcludedFasterVoxelGridDownsampleFilter // renamed class +{ + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr; + +public: + RoiExcludedFasterVoxelGridDownsampleFilter(); // renamed constructor + void set_voxel_size(float voxel_size_x, float voxel_size_y, float voxel_size_z); + void set_field_offsets(const PointCloud2ConstPtr & input, const rclcpp::Logger & logger); + void set_roi(float x_min, float x_max, float y_min, float y_max); + void filter( + const PointCloud2ConstPtr & input, PointCloud2 & output, + const TransformInfo & transform_info, const rclcpp::Logger & logger); + +private: + struct Centroid + { + float x; + float y; + float z; + float intensity; + uint32_t point_count_; + Centroid() : x(0), y(0), z(0), intensity(0), point_count_(0) {} + Centroid(float _x, float _y, float _z, float _intensity) + : x(_x), y(_y), z(_z), intensity(_intensity), point_count_(1) {} + void add_point(float _x, float _y, float _z, float _intensity) + { + x += _x; y += _y; z += _z; intensity += _intensity; + point_count_++; + } + Eigen::Vector4f calc_centroid() const + { + return Eigen::Vector4f(x/point_count_, y/point_count_, z/point_count_, intensity/point_count_); + } + }; + + Eigen::Vector3f inverse_voxel_size_; + int x_offset_; + int y_offset_; + int z_offset_; + int intensity_index_; + int intensity_offset_; + bool offset_initialized_; + float roi_x_min_; + float roi_x_max_; + float roi_y_min_; + float roi_y_max_; + + Eigen::Vector4f get_point_from_global_offset( + const PointCloud2ConstPtr & input, size_t global_offset); + bool get_min_max_voxel( + const PointCloud2ConstPtr & input, Eigen::Vector3i & min_voxel, Eigen::Vector3i & max_voxel); + std::unordered_map calc_centroids_each_voxel( + const PointCloud2ConstPtr & input, const Eigen::Vector3i & max_voxel, + const Eigen::Vector3i & min_voxel); + void copy_centroids_to_output( + std::unordered_map & voxel_centroid_map, PointCloud2 & output, + const TransformInfo & transform_info); +}; + +} // namespace pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/roi_excluded_voxel_grid_downsample_filter_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/roi_excluded_voxel_grid_downsample_filter_node.hpp new file mode 100644 index 0000000000000..ae493d87daca2 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/roi_excluded_voxel_grid_downsample_filter_node.hpp @@ -0,0 +1,57 @@ +// 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__DOWNSAMPLE_FILTER__ROI_EXCLUDED_VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__ROI_EXCLUDED_VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ + +#include "autoware/pointcloud_preprocessor/filter.hpp" +#include "autoware/pointcloud_preprocessor/transform_info.hpp" + +#include +#include + +#include +#include + +namespace autoware::pointcloud_preprocessor +{ +class RoiExcludedVoxelGridDownsampleFilterComponent : public Filter +{ +public: + explicit RoiExcludedVoxelGridDownsampleFilterComponent(const rclcpp::NodeOptions & options); + +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: + float voxel_size_x_; + float voxel_size_y_; + float voxel_size_z_; + float roi_x_min_; + float roi_x_max_; + float roi_y_min_; + float roi_y_max_; + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + rcl_interfaces::msg::SetParametersResult paramCallback( + const std::vector & parameters); +}; + +} // namespace pointcloud_preprocessor + +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__ROI_EXCLUDED_VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ \ No newline at end of file diff --git a/sensing/autoware_pointcloud_preprocessor/launch/roi_excluded_voxel_grid_downsample_filter.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/roi_excluded_voxel_grid_downsample_filter.launch.xml new file mode 100644 index 0000000000000..525e19059bd43 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/roi_excluded_voxel_grid_downsample_filter.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/schema/roi_excluded_voxel_grid_downsample_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/roi_excluded_voxel_grid_downsample_filter_node.schema.json new file mode 100644 index 0000000000000..0df610ab12c86 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/roi_excluded_voxel_grid_downsample_filter_node.schema.json @@ -0,0 +1,74 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for ROI Excluded Voxel Grid Downsample Filter", + "type": "object", + "definitions": { + "roi_excluded_voxel_grid_downsample_filter": { + "type": "object", + "properties": { + "voxel_size_x": { + "type": "number", + "description": "The size of a voxel (in meters) in X dimension", + "default": 0.3, + "minimum": 0.01 + }, + "voxel_size_y": { + "type": "number", + "description": "The size of a voxel (in meters) in Y dimension", + "default": 0.3, + "minimum": 0.01 + }, + "voxel_size_z": { + "type": "number", + "description": "The size of a voxel (in meters) in Z dimension", + "default": 0.1, + "minimum": 0.01 + }, + "roi_x_min": { + "type": "number", + "description": "Minimum X value of the Region of Interest (in meters)", + "default": -10.0 + }, + "roi_x_max": { + "type": "number", + "description": "Maximum X value of the Region of Interest (in meters)", + "default": 10.0 + }, + "roi_y_min": { + "type": "number", + "description": "Minimum Y value of the Region of Interest (in meters)", + "default": -10.0 + }, + "roi_y_max": { + "type": "number", + "description": "Maximum Y value of the Region of Interest (in meters)", + "default": 10.0 + } + }, + "required": [ + "voxel_size_x", + "voxel_size_y", + "voxel_size_z", + "roi_x_min", + "roi_x_max", + "roi_y_min", + "roi_y_max" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/roi_excluded_voxel_grid_downsample_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/roi_excluded_faster_voxel_grid_downsample_filter.cpp b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/roi_excluded_faster_voxel_grid_downsample_filter.cpp new file mode 100644 index 0000000000000..cf294d3607ff9 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/roi_excluded_faster_voxel_grid_downsample_filter.cpp @@ -0,0 +1,238 @@ +// 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/downsample_filter/roi_excluded_faster_voxel_grid_downsample_filter.hpp" + +#include +#include + +#include + +namespace autoware::pointcloud_preprocessor +{ + +RoiExcludedFasterVoxelGridDownsampleFilter::RoiExcludedFasterVoxelGridDownsampleFilter() +{ + offset_initialized_ = false; +} + +void RoiExcludedFasterVoxelGridDownsampleFilter::set_voxel_size( + float voxel_size_x, float voxel_size_y, float voxel_size_z) +{ + inverse_voxel_size_ = + Eigen::Array3f::Ones() / Eigen::Array3f(voxel_size_x, voxel_size_y, voxel_size_z); +} + +void RoiExcludedFasterVoxelGridDownsampleFilter::set_field_offsets( + const PointCloud2ConstPtr & input, const rclcpp::Logger & logger) +{ + x_offset_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; + y_offset_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; + z_offset_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; + intensity_index_ = pcl::getFieldIndex(*input, "intensity"); + + if ( + intensity_index_ < 0 || + input->fields[intensity_index_].datatype != sensor_msgs::msg::PointField::UINT8) { + RCLCPP_ERROR(logger, "No intensity field in the input or intensity type is not UINT8."); + } + + intensity_offset_ = (intensity_index_ != -1) ? input->fields[intensity_index_].offset : -1; + offset_initialized_ = true; +} + +void RoiExcludedFasterVoxelGridDownsampleFilter::set_roi( + float x_min, float x_max, float y_min, float y_max) +{ + roi_x_min_ = x_min; + roi_x_max_ = x_max; + roi_y_min_ = y_min; + roi_y_max_ = y_max; +} + +Eigen::Vector4f RoiExcludedFasterVoxelGridDownsampleFilter::get_point_from_global_offset( + const PointCloud2ConstPtr & input, size_t global_offset) +{ + float intensity = 0.0f; + if (intensity_index_ >= 0) { + intensity = static_cast( + *(reinterpret_cast(&input->data[global_offset + intensity_offset_]))); + } + return Eigen::Vector4f( + *(reinterpret_cast(&input->data[global_offset + x_offset_])), + *(reinterpret_cast(&input->data[global_offset + y_offset_])), + *(reinterpret_cast(&input->data[global_offset + z_offset_])), intensity); +} + +bool RoiExcludedFasterVoxelGridDownsampleFilter::get_min_max_voxel( + const PointCloud2ConstPtr & input, Eigen::Vector3i & min_voxel, Eigen::Vector3i & max_voxel) +{ + Eigen::Vector3f min_point, max_point; + min_point.setConstant(FLT_MAX); + max_point.setConstant(-FLT_MAX); + for (size_t offset = 0; offset + input->point_step <= input->data.size(); + offset += input->point_step) { + Eigen::Vector4f point = get_point_from_global_offset(input, offset); + if (std::isfinite(point[0]) && std::isfinite(point[1]) && std::isfinite(point[2])) { + min_point = min_point.cwiseMin(point.head<3>()); + max_point = max_point.cwiseMax(point.head<3>()); + } + } + + if ( + ((static_cast((max_point[0] - min_point[0]) * inverse_voxel_size_[0]) + 1) * + (static_cast((max_point[1] - min_point[1]) * inverse_voxel_size_[1]) + 1) * + (static_cast((max_point[2] - min_point[2]) * inverse_voxel_size_[2]) + 1)) > + static_cast(std::numeric_limits::max())) { + return false; + } + min_voxel[0] = static_cast(std::floor(min_point[0] * inverse_voxel_size_[0])); + min_voxel[1] = static_cast(std::floor(min_point[1] * inverse_voxel_size_[1])); + min_voxel[2] = static_cast(std::floor(min_point[2] * inverse_voxel_size_[2])); + max_voxel[0] = static_cast(std::floor(max_point[0] * inverse_voxel_size_[0])); + max_voxel[1] = static_cast(std::floor(max_point[1] * inverse_voxel_size_[1])); + max_voxel[2] = static_cast(std::floor(max_point[2] * inverse_voxel_size_[2])); + return true; +} + +std::unordered_map +RoiExcludedFasterVoxelGridDownsampleFilter::calc_centroids_each_voxel( + const PointCloud2ConstPtr & input, const Eigen::Vector3i & max_voxel, + const Eigen::Vector3i & min_voxel) +{ + std::unordered_map voxel_centroid_map; + Eigen::Vector3i div_b = max_voxel - min_voxel + Eigen::Vector3i::Ones(); + Eigen::Vector3i div_b_mul(1, div_b[0], div_b[0] * div_b[1]); + + for (size_t offset = 0; offset + input->point_step <= input->data.size(); + offset += input->point_step) { + Eigen::Vector4f point = get_point_from_global_offset(input, offset); + if (std::isfinite(point[0]) && std::isfinite(point[1]) && std::isfinite(point[2])) { + int ijk0 = static_cast(std::floor(point[0] * inverse_voxel_size_[0]) - min_voxel[0]); + int ijk1 = static_cast(std::floor(point[1] * inverse_voxel_size_[1]) - min_voxel[1]); + int ijk2 = static_cast(std::floor(point[2] * inverse_voxel_size_[2]) - min_voxel[2]); + uint32_t voxel_id = ijk0 * div_b_mul[0] + ijk1 * div_b_mul[1] + ijk2 * div_b_mul[2]; + if (voxel_centroid_map.find(voxel_id) == voxel_centroid_map.end()) { + voxel_centroid_map[voxel_id] = Centroid(point[0], point[1], point[2], point[3]); + } else { + voxel_centroid_map[voxel_id].add_point(point[0], point[1], point[2], point[3]); + } + } + } + return voxel_centroid_map; +} + +void RoiExcludedFasterVoxelGridDownsampleFilter::copy_centroids_to_output( + std::unordered_map & voxel_centroid_map, PointCloud2 & output, + const TransformInfo & transform_info) +{ + size_t output_offset = 0; + for (const auto & pair : voxel_centroid_map) { + Eigen::Vector4f centroid = pair.second.calc_centroid(); + if (transform_info.need_transform) { + centroid = transform_info.eigen_transform * centroid; + } + *reinterpret_cast(&output.data[output_offset + x_offset_]) = centroid[0]; + *reinterpret_cast(&output.data[output_offset + y_offset_]) = centroid[1]; + *reinterpret_cast(&output.data[output_offset + z_offset_]) = centroid[2]; + *reinterpret_cast(&output.data[output_offset + intensity_offset_]) = + static_cast(centroid[3]); + output_offset += output.point_step; + } +} + +void RoiExcludedFasterVoxelGridDownsampleFilter::filter( + const PointCloud2ConstPtr & input, PointCloud2 & output, const TransformInfo & transform_info, + const rclcpp::Logger & logger) +{ + if (!offset_initialized_) { + set_field_offsets(input, logger); + } + // Partition the input into points for voxel filtering and points to pass directly. + std::vector filtered_buffer; + std::vector direct_buffer; + const size_t point_step = input->point_step; + for (size_t offset = 0; offset + point_step <= input->data.size(); offset += point_step) { + Eigen::Vector4f point = get_point_from_global_offset(input, offset); + // If point is inside ROI, bypass filtering. + if ( + (point[0] >= roi_x_min_ && point[0] <= roi_x_max_) && + (point[1] >= roi_y_min_ && point[1] <= roi_y_max_)) { + direct_buffer.insert( + direct_buffer.end(), input->data.begin() + offset, + input->data.begin() + offset + point_step); + } else { + filtered_buffer.insert( + filtered_buffer.end(), input->data.begin() + offset, + input->data.begin() + offset + point_step); + } + } + + // Build a temporary cloud for filtered points. + PointCloud2 filtered_cloud; + filtered_cloud.header = input->header; + filtered_cloud.fields = input->fields; + filtered_cloud.is_bigendian = input->is_bigendian; + filtered_cloud.point_step = point_step; + filtered_cloud.data = filtered_buffer; + filtered_cloud.width = filtered_buffer.size() / point_step; + filtered_cloud.height = 1; // assume organized as unorganized + + // Process voxel filtering on the filtered_cloud if not empty. + PointCloud2 downsampled_filtered; + if (!filtered_cloud.data.empty()) { + Eigen::Vector3i min_voxel, max_voxel; + if (!get_min_max_voxel(std::make_shared(filtered_cloud), min_voxel, max_voxel)) { + RCLCPP_ERROR(logger, "Voxel size too small or data error in filtered cloud."); + downsampled_filtered = filtered_cloud; // fallback to unfiltered + } else { + auto voxel_map = calc_centroids_each_voxel( + std::make_shared(filtered_cloud), max_voxel, min_voxel); + size_t num_points = voxel_map.size(); + downsampled_filtered.header = filtered_cloud.header; + downsampled_filtered.fields = filtered_cloud.fields; + downsampled_filtered.is_bigendian = filtered_cloud.is_bigendian; + downsampled_filtered.point_step = point_step; + downsampled_filtered.width = num_points; + downsampled_filtered.height = 1; + downsampled_filtered.row_step = num_points * point_step; + downsampled_filtered.data.resize(downsampled_filtered.row_step); + copy_centroids_to_output(voxel_map, downsampled_filtered, transform_info); + } + } + + // Create final output by concatenating downsampled filtered points and direct points. + size_t num_downsampled = downsampled_filtered.data.size() / point_step; + size_t num_direct = direct_buffer.size() / point_step; + size_t total_points = num_downsampled + num_direct; + + output.header = input->header; + output.fields = input->fields; + output.is_bigendian = input->is_bigendian; + output.point_step = point_step; + output.width = total_points; + output.height = 1; + output.row_step = total_points * point_step; + output.data.resize(output.row_step); + + // Copy voxel filtered points first. + std::copy( + downsampled_filtered.data.begin(), downsampled_filtered.data.end(), output.data.begin()); + // Then append direct points. + std::copy( + direct_buffer.begin(), direct_buffer.end(), + output.data.begin() + downsampled_filtered.data.size()); +} + +} // namespace pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/roi_excluded_voxel_grid_downsample_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/roi_excluded_voxel_grid_downsample_filter_node.cpp new file mode 100644 index 0000000000000..aa2dcc6b251cd --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/roi_excluded_voxel_grid_downsample_filter_node.cpp @@ -0,0 +1,100 @@ +// 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/downsample_filter/roi_excluded_voxel_grid_downsample_filter_node.hpp" +#include "autoware/pointcloud_preprocessor/downsample_filter/roi_excluded_faster_voxel_grid_downsample_filter.hpp" +#include +#include +#include + +namespace autoware::pointcloud_preprocessor +{ + +RoiExcludedVoxelGridDownsampleFilterComponent::RoiExcludedVoxelGridDownsampleFilterComponent( + const rclcpp::NodeOptions & options) +: Filter("RoiExcludedVoxelGridDownsampleFilter", options) +{ + // Declare voxel size parameters + voxel_size_x_ = static_cast(declare_parameter("voxel_size_x", 0.3)); + voxel_size_y_ = static_cast(declare_parameter("voxel_size_y", 0.3)); + voxel_size_z_ = static_cast(declare_parameter("voxel_size_z", 0.1)); + // Declare ROI parameters + roi_x_min_ = static_cast(declare_parameter("roi_x_min", -10.0)); + roi_x_max_ = static_cast(declare_parameter("roi_x_max", 10.0)); + roi_y_min_ = static_cast(declare_parameter("roi_y_min", -10.0)); + roi_y_max_ = static_cast(declare_parameter("roi_y_max", 10.0)); + + using std::placeholders::_1; + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&RoiExcludedVoxelGridDownsampleFilterComponent::paramCallback, this, _1)); +} + +void RoiExcludedVoxelGridDownsampleFilterComponent::filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) +{ + (void)input; + (void)indices; + (void)output; +} + +void RoiExcludedVoxelGridDownsampleFilterComponent::faster_filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, + PointCloud2 & output, const TransformInfo & transform_info) +{ + std::scoped_lock lock(mutex_); + if (indices) { + RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + } + RoiExcludedFasterVoxelGridDownsampleFilter roi_excluded_filter; + roi_excluded_filter.set_voxel_size(voxel_size_x_, voxel_size_y_, voxel_size_z_); + roi_excluded_filter.set_roi(roi_x_min_, roi_x_max_, roi_y_min_, roi_y_max_); + roi_excluded_filter.set_field_offsets(input, this->get_logger()); + roi_excluded_filter.filter(input, output, transform_info, this->get_logger()); +} + +rcl_interfaces::msg::SetParametersResult RoiExcludedVoxelGridDownsampleFilterComponent::paramCallback( + const std::vector & parameters) +{ + std::scoped_lock lock(mutex_); + if (get_param(parameters, "voxel_size_x", voxel_size_x_)) { + RCLCPP_DEBUG(get_logger(), "New voxel_size_x: %f", voxel_size_x_); + } + if (get_param(parameters, "voxel_size_y", voxel_size_y_)) { + RCLCPP_DEBUG(get_logger(), "New voxel_size_y: %f", voxel_size_y_); + } + if (get_param(parameters, "voxel_size_z", voxel_size_z_)) { + RCLCPP_DEBUG(get_logger(), "New voxel_size_z: %f", voxel_size_z_); + } + if (get_param(parameters, "roi_x_min", roi_x_min_)) { + RCLCPP_DEBUG(get_logger(), "New roi_x_min: %f", roi_x_min_); + } + if (get_param(parameters, "roi_x_max", roi_x_max_)) { + RCLCPP_DEBUG(get_logger(), "New roi_x_max: %f", roi_x_max_); + } + if (get_param(parameters, "roi_y_min", roi_y_min_)) { + RCLCPP_DEBUG(get_logger(), "New roi_y_min: %f", roi_y_min_); + } + if (get_param(parameters, "roi_y_max", roi_y_max_)) { + RCLCPP_DEBUG(get_logger(), "New roi_y_max: %f", roi_y_max_); + } + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} + +} // namespace pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pointcloud_preprocessor::RoiExcludedVoxelGridDownsampleFilterComponent) \ No newline at end of file diff --git a/sensing/autoware_pointcloud_preprocessor/src/filter.cpp b/sensing/autoware_pointcloud_preprocessor/src/filter.cpp index 03ce774df6c8c..4f77c2624862c 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", "RoiExcludedVoxelGridDownsampleFilter"}; auto callback = supported_nodes.find(filter_name) != supported_nodes.end() ? &Filter::faster_input_indices_callback : &Filter::input_indices_callback;