perf(ring_outlier_filter): an attempt of cache friendly impl

Signed-off-by: Vincent Richard <richard-v@macnica.co.jp> style(pre-commit): autofix fix(ring_outlier_filter): cleanup code with ranges Signed-off-by: Vincent Richard <richard-v@macnica.co.jp> [breaking] fix autoware point type padding for faster memory access can memcpy between input data buffer and PointXYZI* make assumption on memory layout for faster data fetch misc optimization (reordering, constexpr, etc) Signed-off-by: Vincent Richard <richard-v@macnica.co.jp> style(pre-commit): autofix comment limitations Signed-off-by: Vincent Richard <richard-v@macnica.co.jp> feat(ring_outlier_filter): add accurate isCluster impl Signed-off-by: Vincent Richard <richard-v@macnica.co.jp> style(pre-commit): autofix fix #3218 Signed-off-by: Vincent Richard <richard-v@macnica.co.jp> cleaning Signed-off-by: Vincent Richard <richard-v@macnica.co.jp> style(pre-commit): autofix
autowarefoundation · May 22, 2023 · 9a24f2c · 9a24f2c
1 parent c185a7d
commit 9a24f2c
Show file tree

Hide file tree

Showing 5 changed files with 310 additions and 119 deletions.
diff --git a/common/autoware_point_types/include/autoware_point_types/types.hpp b/common/autoware_point_types/include/autoware_point_types/types.hpp
@@ -35,6 +35,7 @@ struct PointXYZI
   float x{0.0F};
   float y{0.0F};
   float z{0.0F};
+  float _padding{0};  // compatibility with PCL::PointXYZ*
   float intensity{0.0F};
   friend bool operator==(const PointXYZI & p1, const PointXYZI & p2) noexcept
   {
@@ -59,6 +60,7 @@ struct PointXYZIRADRT
   float x{0.0F};
   float y{0.0F};
   float z{0.0F};
+  float _padding{0};  // compatibility with PCL::PointXYZ*
   float intensity{0.0F};
   uint16_t ring{0U};
   float azimuth{0.0F};

diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md
@@ -22,13 +22,12 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref
 
 ### Core Parameters
 
-| Name                      | Type    | Default Value | Description                                                                         |
-| ------------------------- | ------- | ------------- | ----------------------------------------------------------------------------------- |
-| `distance_ratio`          | double  | 1.03          |                                                                                     |
-| `object_length_threshold` | double  | 0.1           |                                                                                     |
-| `num_points_threshold`    | int     | 4             |                                                                                     |
-| `max_rings_num`           | uint_16 | 128           |                                                                                     |
-| `max_points_num_per_ring` | size_t  | 4000          | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` |
+| Name                      | Type    | Default Value | Description |
+| ------------------------- | ------- | ------------- | ----------- |
+| `distance_ratio`          | double  | 1.03          |             |
+| `object_length_threshold` | double  | 0.1           |             |
+| `num_points_threshold`    | int     | 4             |             |
+| `max_rings_num`           | uint_16 | 128           |             |
 
 ## Assumptions / Known limits
 

diff --git a/...eprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/...eprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp
@@ -45,29 +45,13 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
   double object_length_threshold_;
   int num_points_threshold_;
   uint16_t max_rings_num_;
-  size_t max_points_num_per_ring_;
 
   /** \brief Parameter service callback result : needed to be hold */
   OnSetParametersCallbackHandle::SharedPtr set_param_res_;
 
   /** \brief Parameter service callback */
   rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector<rclcpp::Parameter> & p);
 
-  bool isCluster(
-    const PointCloud2ConstPtr & input, std::pair<int, int> data_idx_both_ends, int walk_size)
-  {
-    if (walk_size > num_points_threshold_) return true;
-
-    auto first_point = reinterpret_cast<const PointXYZI *>(&input->data[data_idx_both_ends.first]);
-    auto last_point = reinterpret_cast<const PointXYZI *>(&input->data[data_idx_both_ends.second]);
-
-    const auto x = first_point->x - last_point->x;
-    const auto y = first_point->y - last_point->y;
-    const auto z = first_point->z - last_point->z;
-
-    return x * x + y * y + z * z >= object_length_threshold_ * object_length_threshold_;
-  }
-
 public:
   PCL_MAKE_ALIGNED_OPERATOR_NEW
   explicit RingOutlierFilterComponent(const rclcpp::NodeOptions & options);

diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml
@@ -32,6 +32,7 @@
   <depend>pcl_msgs</depend>
   <depend>pcl_ros</depend>
   <depend>point_cloud_msg_wrapper</depend>
+  <depend>range-v3</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>