-
Notifications
You must be signed in to change notification settings - Fork 691
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat(autoware_pointcloud_preprocessor): add roi excluded downsample
Signed-off-by: Kaan Çolak <kaancolak95@gmail.com>
- Loading branch information
Showing
9 changed files
with
626 additions
and
1 deletion.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
91 changes: 91 additions & 0 deletions
91
...cloud_preprocessor/downsample_filter/roi_excluded_faster_voxel_grid_downsample_filter.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <pcl/filters/voxel_grid.h> | ||
| #include <pcl_conversions/pcl_conversions.h> | ||
| #include <sensor_msgs/msg/point_cloud2.h> | ||
| #include <unordered_map> | ||
| #include <vector> | ||
| #include <limits> | ||
| #include <cfloat> | ||
| #include <cmath> | ||
| #include <Eigen/Core> | ||
|
|
||
| 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<uint32_t, Centroid> 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<uint32_t, Centroid> & voxel_centroid_map, PointCloud2 & output, | ||
| const TransformInfo & transform_info); | ||
| }; | ||
|
|
||
| } // namespace pointcloud_preprocessor |
57 changes: 57 additions & 0 deletions
57
...ntcloud_preprocessor/downsample_filter/roi_excluded_voxel_grid_downsample_filter_node.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <rclcpp/rclcpp.hpp> | ||
| #include <rcl_interfaces/msg/set_parameters_result.hpp> | ||
|
|
||
| #include <mutex> | ||
| #include <vector> | ||
|
|
||
| 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<rclcpp::Parameter> & parameters); | ||
| }; | ||
|
|
||
| } // namespace pointcloud_preprocessor | ||
|
|
||
| #endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__ROI_EXCLUDED_VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ |
23 changes: 23 additions & 0 deletions
23
...oware_pointcloud_preprocessor/launch/roi_excluded_voxel_grid_downsample_filter.launch.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,23 @@ | ||
| <launch> | ||
| <arg name="input_topic" default="/sensing/lidar/concatenated/pointcloud"/> | ||
| <arg name="output_topic" default="/sensing/lidar/pointcloud/roi_excluded_downsampled"/> | ||
| <arg name="voxel_size_x" default="0.3"/> | ||
| <arg name="voxel_size_y" default="0.3"/> | ||
| <arg name="voxel_size_z" default="0.1"/> | ||
| <arg name="roi_x_min" default="-10.0"/> | ||
| <arg name="roi_x_max" default="10.0"/> | ||
| <arg name="roi_y_min" default="-10.0"/> | ||
| <arg name="roi_y_max" default="10.0"/> | ||
|
|
||
| <node pkg="autoware_pointcloud_preprocessor" exec="roi_excluded_voxel_grid_downsample_filter_node" name="roi_excluded_voxel_grid_downsample_filter_node" output="screen"> | ||
| <remap from="input" to="$(var input_topic)"/> | ||
| <remap from="output" to="$(var output_topic)"/> | ||
| <param name="voxel_size_x" value="$(var voxel_size_x)"/> | ||
| <param name="voxel_size_y" value="$(var voxel_size_y)"/> | ||
| <param name="voxel_size_z" value="$(var voxel_size_z)"/> | ||
| <param name="roi_x_min" value="$(var roi_x_min)"/> | ||
| <param name="roi_x_max" value="$(var roi_x_max)"/> | ||
| <param name="roi_y_min" value="$(var roi_y_min)"/> | ||
| <param name="roi_y_max" value="$(var roi_y_max)"/> | ||
| </node> | ||
| </launch> |
74 changes: 74 additions & 0 deletions
74
...pointcloud_preprocessor/schema/roi_excluded_voxel_grid_downsample_filter_node.schema.json
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 | ||
| } |
Oops, something went wrong.