From 16956bfe0559098f9a02fd4999721b4ee7e3993d Mon Sep 17 00:00:00 2001 From: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> Date: Mon, 11 Sep 2023 07:36:38 +0900 Subject: [PATCH 1/2] Revert "perf(voxel_grid_downsample_filter): performance tuning (#3679)" This reverts commit edcad42338a56cd2c7eaf0803f0861c56493608c. --- .../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 +- 10 files changed, 21 insertions(+), 375 deletions(-) delete mode 100644 sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp delete mode 100644 sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp delete mode 100644 sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index c5f15ec4e49d5..b14d3a181e587 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -41,21 +41,6 @@ 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 @@ -82,7 +67,6 @@ 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} @@ -195,13 +179,6 @@ 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 1406679ce0dba..3b392401de49b 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,7 +54,6 @@ #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 deleted file mode 100644 index d92b511d40c69..0000000000000 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/downsample_filter/faster_voxel_grid_downsample_filter.hpp +++ /dev/null @@ -1,93 +0,0 @@ -// 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 ae285b4fa9239..7ebf80c2da958 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 2023 TIER IV, Inc. +// Copyright 2020 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,7 +52,6 @@ #define POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_ #include "pointcloud_preprocessor/filter.hpp" -#include "pointcloud_preprocessor/transform_info.hpp" #include #include @@ -67,16 +66,10 @@ 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: - float voxel_size_x_; - float voxel_size_y_; - float voxel_size_z_; + double voxel_size_x_; + double voxel_size_y_; + double 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 47c0f2b403faa..f995be741272c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/filter.hpp @@ -52,8 +52,6 @@ #ifndef POINTCLOUD_PREPROCESSOR__FILTER_HPP_ #define POINTCLOUD_PREPROCESSOR__FILTER_HPP_ -#include "pointcloud_preprocessor/transform_info.hpp" - #include #include #include @@ -136,6 +134,18 @@ 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 ab7d4e0012304..a906eb913d863 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,7 +17,6 @@ #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 deleted file mode 100644 index a4806555695b5..0000000000000 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/transform_info.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// 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 deleted file mode 100644 index 7c302eec20431..0000000000000 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp +++ /dev/null @@ -1,178 +0,0 @@ -// 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 f11f37397a142..5b22986a1b6b8 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 2023 TIER IV, Inc. +// Copyright 2020 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,8 +50,6 @@ #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 @@ -66,9 +64,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; @@ -76,8 +74,6 @@ 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) { @@ -99,19 +95,6 @@ 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 d5843be395adf..f0b716c73b5f1 100644 --- a/sensing/pointcloud_preprocessor/src/filter.cpp +++ b/sensing/pointcloud_preprocessor/src/filter.cpp @@ -124,8 +124,7 @@ 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", "VoxelGridDownsampleFilter"}; + std::set supported_nodes = {"CropBoxFilter", "RingOutlierFilter"}; auto callback = supported_nodes.find(filter_name) != supported_nodes.end() ? &Filter::faster_input_indices_callback : &Filter::input_indices_callback; From 021d45aef6e1cdc6c6285faa973d5e1fd0d714af Mon Sep 17 00:00:00 2001 From: Shin-kyoto <58775300+Shin-kyoto@users.noreply.github.com> Date: Mon, 11 Sep 2023 07:36:58 +0900 Subject: [PATCH 2/2] Revert "fix(euclidean_cluster): add disuse downsample in clustering pipeline (#4385)" This reverts commit 559f8737a89ca1b20eda705ecaf9d3cd1851a7b9. --- ...ra_lidar_fusion_based_detection.launch.xml | 2 - ...ar_radar_fusion_based_detection.launch.xml | 2 - .../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 - 6 files changed, 13 insertions(+), 93 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 12961c1470eb9..aef7220680d67 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 @@ -30,7 +30,6 @@ - @@ -74,7 +73,6 @@ - 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 3a24ec2fdf500..23f3bd0b532a0 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 @@ -4,7 +4,6 @@ - @@ -60,7 +59,6 @@ - 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 6d67df5fa7a1f..9ab48a678240d 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,7 +5,6 @@ - @@ -34,7 +33,6 @@ - diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 204fb561b10ea..58ee60b9409e0 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -58,7 +58,6 @@ - - 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 82ce5605b67cd..ba6e9e72107c8 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,10 +16,9 @@ from launch.actions import DeclareLaunchArgument from launch.actions import OpaqueFunction from launch.conditions import IfCondition -from launch.substitutions import AndSubstitution +from launch.conditions import UnlessCondition 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 @@ -165,7 +164,7 @@ def load_composable_node_param(param_path): ) # set euclidean cluster as a component - use_downsample_euclidean_cluster_component = ComposableNode( + euclidean_cluster_component = ComposableNode( package=pkg, namespace=ns, plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", @@ -177,108 +176,40 @@ 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=[], + composable_node_descriptions=[ + voxel_grid_filter_component, + outlier_filter_component, + downsample_concat_component, + euclidean_cluster_component, + ], output="screen", ) - use_map_use_downsample_loader = LoadComposableNodes( + use_map_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( - AndSubstitution( - LaunchConfiguration("use_pointcloud_map"), - LaunchConfiguration("use_downsample_pointcloud"), - ) - ), + condition=IfCondition(LaunchConfiguration("use_pointcloud_map")), ) - disuse_map_use_downsample_loader = LoadComposableNodes( + disuse_map_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=IfCondition( - AndSubstitution( - NotSubstitution(LaunchConfiguration("use_pointcloud_map")), - LaunchConfiguration("use_downsample_pointcloud"), - ) - ), + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")), ) - 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, - ] + return [container, use_map_loader, disuse_map_loader] def generate_launch_description(): @@ -291,7 +222,6 @@ 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 2833fed81c28e..6ae70b2612836 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,7 +4,6 @@ - @@ -15,7 +14,6 @@ -