From 1fb1a6e9a6dab2e5d7fe77c115b520097dc0eee8 Mon Sep 17 00:00:00 2001 From: Shunsuke Miura Date: Fri, 15 Sep 2023 18:54:18 +0900 Subject: [PATCH 1/3] Revert "Merge pull request #815 from Shin-kyoto/fix/reduce_false_negative" This reverts commit c606feca18ad774bbec2fa9d9345ce03cf7ed2d9, reversing changes made to bdccfb9fcca89e21f46cce912db4276b09e54f29. # Conflicts: # launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml # launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml --- ...ra_lidar_fusion_based_detection.launch.xml | 1 + ...ar_radar_fusion_based_detection.launch.xml | 1 + .../lidar_based_detection.launch.xml | 2 + .../launch/perception.launch.xml | 2 + ...xel_grid_based_euclidean_cluster.launch.py | 96 ++++++++-- ...el_grid_based_euclidean_cluster.launch.xml | 2 + .../pointcloud_preprocessor/CMakeLists.txt | 23 +++ .../crop_box_filter_nodelet.hpp | 1 + .../faster_voxel_grid_downsample_filter.hpp | 93 +++++++++ .../voxel_grid_downsample_filter_nodelet.hpp | 15 +- .../pointcloud_preprocessor/filter.hpp | 14 +- .../ring_outlier_filter_nodelet.hpp | 1 + .../transform_info.hpp | 43 +++++ .../faster_voxel_grid_downsample_filter.cpp | 178 ++++++++++++++++++ .../voxel_grid_downsample_filter_nodelet.cpp | 25 ++- .../pointcloud_preprocessor/src/filter.cpp | 3 +- 16 files changed, 466 insertions(+), 34 deletions(-) create mode 100644 sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp create mode 100644 sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp create mode 100644 sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 34c2fd4cdc27c..150924330b360 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -81,6 +81,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 7d1353433da85..463340efdecfe 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -12,6 +12,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 9ab48a678240d..6d67df5fa7a1f 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -5,6 +5,7 @@ + @@ -33,6 +34,7 @@ + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 58ee60b9409e0..204fb561b10ea 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -58,6 +58,7 @@ + + diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index ba6e9e72107c8..82ce5605b67cd 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -16,9 +16,10 @@ from launch.actions import DeclareLaunchArgument from launch.actions import OpaqueFunction from launch.conditions import IfCondition -from launch.conditions import UnlessCondition +from launch.substitutions import AndSubstitution from launch.substitutions import AnonName from launch.substitutions import LaunchConfiguration +from launch.substitutions import NotSubstitution from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode @@ -164,7 +165,7 @@ def load_composable_node_param(param_path): ) # set euclidean cluster as a component - euclidean_cluster_component = ComposableNode( + use_downsample_euclidean_cluster_component = ComposableNode( package=pkg, namespace=ns, plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", @@ -176,40 +177,108 @@ def load_composable_node_param(param_path): parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) + use_map_disuse_downsample_euclidean_component = ComposableNode( + package=pkg, + namespace=ns, + plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", + name="euclidean_cluster", + remappings=[ + ("input", "map_filter/pointcloud"), + ("output", LaunchConfiguration("output_clusters")), + ], + parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], + ) + disuse_map_disuse_downsample_euclidean_component = ComposableNode( + package=pkg, + namespace=ns, + plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", + name="euclidean_cluster", + remappings=[ + ("input", LaunchConfiguration("input_pointcloud")), + ("output", LaunchConfiguration("output_clusters")), + ], + parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], + ) + container = ComposableNodeContainer( name="euclidean_cluster_container", package="rclcpp_components", namespace=ns, executable="component_container", - composable_node_descriptions=[ - voxel_grid_filter_component, - outlier_filter_component, - downsample_concat_component, - euclidean_cluster_component, - ], + composable_node_descriptions=[], output="screen", ) - use_map_loader = LoadComposableNodes( + use_map_use_downsample_loader = LoadComposableNodes( composable_node_descriptions=[ compare_map_filter_component, use_map_short_range_crop_box_filter_component, use_map_long_range_crop_box_filter_component, + voxel_grid_filter_component, + outlier_filter_component, + downsample_concat_component, + use_downsample_euclidean_cluster_component, ], target_container=container, - condition=IfCondition(LaunchConfiguration("use_pointcloud_map")), + condition=IfCondition( + AndSubstitution( + LaunchConfiguration("use_pointcloud_map"), + LaunchConfiguration("use_downsample_pointcloud"), + ) + ), ) - disuse_map_loader = LoadComposableNodes( + disuse_map_use_downsample_loader = LoadComposableNodes( composable_node_descriptions=[ disuse_map_short_range_crop_box_filter_component, disuse_map_long_range_crop_box_filter_component, + voxel_grid_filter_component, + outlier_filter_component, + downsample_concat_component, + use_downsample_euclidean_cluster_component, ], target_container=container, - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")), + condition=IfCondition( + AndSubstitution( + NotSubstitution(LaunchConfiguration("use_pointcloud_map")), + LaunchConfiguration("use_downsample_pointcloud"), + ) + ), ) - return [container, use_map_loader, disuse_map_loader] + use_map_disuse_downsample_loader = LoadComposableNodes( + composable_node_descriptions=[ + compare_map_filter_component, + use_map_disuse_downsample_euclidean_component, + ], + target_container=container, + condition=IfCondition( + AndSubstitution( + (LaunchConfiguration("use_pointcloud_map")), + NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")), + ) + ), + ) + + disuse_map_disuse_downsample_loader = LoadComposableNodes( + composable_node_descriptions=[ + disuse_map_disuse_downsample_euclidean_component, + ], + target_container=container, + condition=IfCondition( + AndSubstitution( + NotSubstitution(LaunchConfiguration("use_pointcloud_map")), + NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")), + ) + ), + ) + return [ + container, + use_map_use_downsample_loader, + disuse_map_use_downsample_loader, + use_map_disuse_downsample_loader, + disuse_map_disuse_downsample_loader, + ] def generate_launch_description(): @@ -222,6 +291,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_map", "/map/pointcloud_map"), add_launch_arg("output_clusters", "clusters"), add_launch_arg("use_pointcloud_map", "false"), + add_launch_arg("use_downsample_pointcloud", "false"), add_launch_arg( "voxel_grid_param_path", [ diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml index 6ae70b2612836..2833fed81c28e 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml @@ -4,6 +4,7 @@ + @@ -14,6 +15,7 @@ + diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index b14d3a181e587..c5f15ec4e49d5 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -41,6 +41,21 @@ ament_target_dependencies(pointcloud_preprocessor_filter_base pcl_ros ) +add_library(faster_voxel_grid_downsample_filter SHARED + src/downsample_filter/faster_voxel_grid_downsample_filter.cpp +) + +target_include_directories(faster_voxel_grid_downsample_filter PUBLIC + "$" + "$" +) + +ament_target_dependencies(faster_voxel_grid_downsample_filter + pcl_conversions + rclcpp + sensor_msgs +) + ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/utility/utilities.cpp src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -67,6 +82,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED target_link_libraries(pointcloud_preprocessor_filter pointcloud_preprocessor_filter_base + faster_voxel_grid_downsample_filter ${Boost_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} @@ -179,6 +195,13 @@ install( RUNTIME DESTINATION bin ) +install( + TARGETS 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/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp index 3b392401de49b..1406679ce0dba 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp @@ -54,6 +54,7 @@ #define POINTCLOUD_PREPROCESSOR__CROP_BOX_FILTER__CROP_BOX_FILTER_NODELET_HPP_ #include "pointcloud_preprocessor/filter.hpp" +#include "pointcloud_preprocessor/transform_info.hpp" #include diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp new file mode 100644 index 0000000000000..d92b511d40c69 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp @@ -0,0 +1,93 @@ +// Copyright 2023 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 "pointcloud_preprocessor/transform_info.hpp" + +#include +#include +#include + +#include +#include + +namespace pointcloud_preprocessor +{ + +class FasterVoxelGridDownsampleFilter +{ + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr; + +public: + FasterVoxelGridDownsampleFilter(); + void set_voxel_size(float voxel_size_x, float voxel_size_y, float voxel_size_z); + void set_field_offsets(const PointCloud2ConstPtr & input); + void filter( + const PointCloud2ConstPtr & input, PointCloud2 & output, const TransformInfo & transform_info, + const rclcpp::Logger & logger); + +private: + struct Centroid + { + float x; + float y; + float z; + uint32_t point_count_; + + Centroid() : x(0), y(0), z(0) {} + Centroid(float _x, float _y, float _z) : x(_x), y(_y), z(_z) { this->point_count_ = 1; } + + void add_point(float _x, float _y, float _z) + { + this->x += _x; + this->y += _y; + this->z += _z; + this->point_count_++; + } + + Eigen::Vector4f calc_centroid() const + { + Eigen::Vector4f centroid( + (this->x / this->point_count_), (this->y / this->point_count_), + (this->z / this->point_count_), 1.0f); + return centroid; + } + }; + + Eigen::Vector3f inverse_voxel_size_; + std::vector xyz_fields_; + int x_offset_; + int y_offset_; + int z_offset_; + int intensity_offset_; + bool offset_initialized_; + + Eigen::Vector3f 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/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp index 7ebf80c2da958..ae285b4fa9239 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2023 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. @@ -52,6 +52,7 @@ #define POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ #include "pointcloud_preprocessor/filter.hpp" +#include "pointcloud_preprocessor/transform_info.hpp" #include #include @@ -66,10 +67,16 @@ class VoxelGridDownsampleFilterComponent : public pointcloud_preprocessor::Filte void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; + // TODO(atsushi421): Temporary Implementation: Remove this interface when all the filter nodes + // conform to new API + virtual void faster_filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, + const TransformInfo & transform_info); + private: - double voxel_size_x_; - double voxel_size_y_; - double voxel_size_z_; + float voxel_size_x_; + float voxel_size_y_; + float voxel_size_z_; /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp index f995be741272c..47c0f2b403faa 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp @@ -52,6 +52,8 @@ #ifndef POINTCLOUD_PREPROCESSOR__FILTER_HPP_ #define POINTCLOUD_PREPROCESSOR__FILTER_HPP_ +#include "pointcloud_preprocessor/transform_info.hpp" + #include #include #include @@ -134,18 +136,6 @@ class Filter : public rclcpp::Node const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); protected: - struct TransformInfo - { - TransformInfo() - { - eigen_transform = Eigen::Matrix4f::Identity(4, 4); - need_transform = false; - } - - Eigen::Matrix4f eigen_transform; - bool need_transform; - }; - /** \brief The input PointCloud2 subscriber. */ rclcpp::Subscription::SharedPtr sub_input_; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index a906eb913d863..ab7d4e0012304 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -17,6 +17,7 @@ #include "autoware_point_types/types.hpp" #include "pointcloud_preprocessor/filter.hpp" +#include "pointcloud_preprocessor/transform_info.hpp" #include diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp new file mode 100644 index 0000000000000..a4806555695b5 --- /dev/null +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp @@ -0,0 +1,43 @@ +// Copyright 2023 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 + +namespace pointcloud_preprocessor +{ + +/** + * This holds the coordinate transformation information of the point cloud. + * Usage example: + * \code + * if (transform_info.need_transform) { + * point = transform_info.eigen_transform * point; + * } + * \endcode + */ +struct TransformInfo +{ + TransformInfo() + { + eigen_transform = Eigen::Matrix4f::Identity(4, 4); + need_transform = false; + } + + Eigen::Matrix4f eigen_transform; + bool need_transform; +}; + +} // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp new file mode 100644 index 0000000000000..7c302eec20431 --- /dev/null +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp @@ -0,0 +1,178 @@ +// Copyright 2023 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 "pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp" + +namespace pointcloud_preprocessor +{ + +FasterVoxelGridDownsampleFilter::FasterVoxelGridDownsampleFilter() +{ + pcl::for_each_type::type>( + pcl::detail::FieldAdder(xyz_fields_)); + offset_initialized_ = false; +} + +void FasterVoxelGridDownsampleFilter::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 FasterVoxelGridDownsampleFilter::set_field_offsets(const PointCloud2ConstPtr & input) +{ + 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_offset_ = input->fields[pcl::getFieldIndex(*input, "intensity")].offset; + offset_initialized_ = true; +} + +void FasterVoxelGridDownsampleFilter::filter( + const PointCloud2ConstPtr & input, PointCloud2 & output, const TransformInfo & transform_info, + const rclcpp::Logger & logger) +{ + // Check if the field offset has been set + if (!offset_initialized_) { + set_field_offsets(input); + } + + // Compute the minimum and maximum voxel coordinates + Eigen::Vector3i min_voxel, max_voxel; + if (!get_min_max_voxel(input, min_voxel, max_voxel)) { + RCLCPP_ERROR( + logger, + "Voxel size is too small for the input dataset. " + "Integer indices would overflow."); + output = *input; + return; + } + + // Storage for mapping voxel coordinates to centroids + auto voxel_centroid_map = calc_centroids_each_voxel(input, max_voxel, min_voxel); + + // Initialize the output + output.row_step = voxel_centroid_map.size() * input->point_step; + output.data.resize(output.row_step); + output.width = voxel_centroid_map.size(); + pcl_conversions::fromPCL(xyz_fields_, output.fields); + output.is_dense = true; // we filter out invalid points + output.height = input->height; + output.is_bigendian = input->is_bigendian; + output.point_step = input->point_step; + output.header = input->header; + + // Copy the centroids to the output + copy_centroids_to_output(voxel_centroid_map, output, transform_info); +} + +Eigen::Vector3f FasterVoxelGridDownsampleFilter::get_point_from_global_offset( + const PointCloud2ConstPtr & input, size_t global_offset) +{ + Eigen::Vector3f point( + *reinterpret_cast(&input->data[global_offset + x_offset_]), + *reinterpret_cast(&input->data[global_offset + y_offset_]), + *reinterpret_cast(&input->data[global_offset + z_offset_])); + return point; +} + +bool FasterVoxelGridDownsampleFilter::get_min_max_voxel( + const PointCloud2ConstPtr & input, Eigen::Vector3i & min_voxel, Eigen::Vector3i & max_voxel) +{ + // Compute the minimum and maximum point coordinates + Eigen::Vector3f min_point, max_point; + min_point.setConstant(FLT_MAX); + max_point.setConstant(-FLT_MAX); + for (size_t global_offset = 0; global_offset + input->point_step <= input->data.size(); + global_offset += input->point_step) { + Eigen::Vector3f point = get_point_from_global_offset(input, global_offset); + if (std::isfinite(point[0]) && std::isfinite(point[1]), std::isfinite(point[2])) { + min_point = min_point.cwiseMin(point); + max_point = max_point.cwiseMax(point); + } + } + + // Check that the voxel size is not too small, given the size of the data + 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; + } + + // Compute the minimum and maximum voxel coordinates + 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 +FasterVoxelGridDownsampleFilter::calc_centroids_each_voxel( + const PointCloud2ConstPtr & input, const Eigen::Vector3i & max_voxel, + const Eigen::Vector3i & min_voxel) +{ + std::unordered_map voxel_centroid_map; + // Compute the number of divisions needed along all axis + Eigen::Vector3i div_b = max_voxel - min_voxel + Eigen::Vector3i::Ones(); + // Set up the division multiplier + Eigen::Vector3i div_b_mul(1, div_b[0], div_b[0] * div_b[1]); + + for (size_t global_offset = 0; global_offset + input->point_step <= input->data.size(); + global_offset += input->point_step) { + Eigen::Vector3f point = get_point_from_global_offset(input, global_offset); + if (std::isfinite(point[0]) && std::isfinite(point[1]), std::isfinite(point[2])) { + // Calculate the voxel index to which the point belongs + 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]; + + // Add the point to the corresponding centroid + if (voxel_centroid_map.find(voxel_id) == voxel_centroid_map.end()) { + voxel_centroid_map[voxel_id] = Centroid(point[0], point[1], point[2]); + } else { + voxel_centroid_map[voxel_id].add_point(point[0], point[1], point[2]); + } + } + } + + return voxel_centroid_map; +} + +void FasterVoxelGridDownsampleFilter::copy_centroids_to_output( + std::unordered_map & voxel_centroid_map, PointCloud2 & output, + const TransformInfo & transform_info) +{ + size_t output_data_size = 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_data_size + x_offset_]) = centroid[0]; + *reinterpret_cast(&output.data[output_data_size + y_offset_]) = centroid[1]; + *reinterpret_cast(&output.data[output_data_size + z_offset_]) = centroid[2]; + *reinterpret_cast(&output.data[output_data_size + intensity_offset_]) = centroid[3]; + output_data_size += output.point_step; + } +} + +} // namespace pointcloud_preprocessor diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp index 5b22986a1b6b8..f11f37397a142 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2023 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. @@ -50,6 +50,8 @@ #include "pointcloud_preprocessor/downsample_filter/voxel_grid_downsample_filter_nodelet.hpp" +#include "pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp" + #include #include #include @@ -64,9 +66,9 @@ VoxelGridDownsampleFilterComponent::VoxelGridDownsampleFilterComponent( { // set initial 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)); + 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)); } using std::placeholders::_1; @@ -74,6 +76,8 @@ VoxelGridDownsampleFilterComponent::VoxelGridDownsampleFilterComponent( std::bind(&VoxelGridDownsampleFilterComponent::paramCallback, this, _1)); } +// TODO(atsushi421): Temporary Implementation: Delete this function definition when all the filter +// nodes conform to new API. void VoxelGridDownsampleFilterComponent::filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) { @@ -95,6 +99,19 @@ void VoxelGridDownsampleFilterComponent::filter( output.header = input->header; } +// TODO(atsushi421): Temporary Implementation: Rename this function to `filter()` when all the +// filter nodes conform to new API. Then delete the old `filter()` defined above. +void VoxelGridDownsampleFilterComponent::faster_filter( + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output, const TransformInfo & transform_info) +{ + std::scoped_lock lock(mutex_); + FasterVoxelGridDownsampleFilter faster_voxel_filter; + faster_voxel_filter.set_voxel_size(voxel_size_x_, voxel_size_y_, voxel_size_z_); + faster_voxel_filter.set_field_offsets(input); + faster_voxel_filter.filter(input, output, transform_info, this->get_logger()); +} + rcl_interfaces::msg::SetParametersResult VoxelGridDownsampleFilterComponent::paramCallback( const std::vector & p) { diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp index f0b716c73b5f1..d5843be395adf 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -124,7 +124,8 @@ void pointcloud_preprocessor::Filter::subscribe(const std::string & filter_name) // TODO(sykwer): Change the corresponding node to subscribe to `faster_input_indices_callback` // 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"}; + std::set supported_nodes = { + "CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter"}; auto callback = supported_nodes.find(filter_name) != supported_nodes.end() ? &Filter::faster_input_indices_callback : &Filter::input_indices_callback; From b2af8a5819577c93c7c9eab1a295da5e178a3cec Mon Sep 17 00:00:00 2001 From: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Date: Thu, 14 Sep 2023 10:14:32 +0900 Subject: [PATCH 2/3] Merge pull request #833 from tier4/fix/camera_lidar_fusion_launch fix(tier4_perception_launch): camera lidar fusion launch --- .../camera_lidar_fusion_based_detection.launch.xml | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 150924330b360..24edd17253634 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -332,22 +332,11 @@ - - - - - - - - - - - - + From f7322de99ffb947fc869f5b1d7502271fc68db49 Mon Sep 17 00:00:00 2001 From: atsushi yano <55824710+atsushi421@users.noreply.github.com> Date: Tue, 12 Sep 2023 09:59:14 +0900 Subject: [PATCH 3/3] fix(voxel_grid_downsample_filter): support no intensity case (#4922) fix: support for no intensity field Signed-off-by: atsushi421 --- .../faster_voxel_grid_downsample_filter.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp index 7c302eec20431..5829277335433 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp @@ -36,7 +36,12 @@ void FasterVoxelGridDownsampleFilter::set_field_offsets(const PointCloud2ConstPt 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_offset_ = input->fields[pcl::getFieldIndex(*input, "intensity")].offset; + int intensity_index = pcl::getFieldIndex(*input, "intensity"); + if (intensity_index != -1) { + intensity_offset_ = input->fields[intensity_index].offset; + } else { + intensity_offset_ = z_offset_ + sizeof(float); + } offset_initialized_ = true; }