perf(voxel_grid_downsample_filter): performance tuning

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
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
+)
+
+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_data_nodelet.cpp
@@ -65,6 +80,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}
@@ -167,6 +183,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}

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 <geometry_msgs/msg/polygon_stamped.hpp>
 

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 <pcl/filters/voxel_grid.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <sensor_msgs/msg/point_cloud2.h>
+
+#include <unordered_map>
+#include <vector>
+
+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<pcl::PCLPointField> 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<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

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 <pcl/filters/voxel_grid.h>
 #include <pcl/search/pcl_search.h>
@@ -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_;

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 <memory>
 #include <string>
 #include <vector>
@@ -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<PointCloud2>::SharedPtr sub_input_;
 

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 <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
 

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 <Eigen/Eigen>
+
+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