perf(ring_outlier_filter): a cache friendly impl

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
 

sensing/pointcloud_preprocessor/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);

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp

@@ -26,6 +26,7 @@
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
 
+#include <optional>
 #include <vector>
 
 using K = CGAL::Exact_predicates_inexact_constructions_kernel;
@@ -86,6 +87,33 @@ bool is_data_layout_compatible_with_PointXYZI(const sensor_msgs::msg::PointCloud
  * is to say whether you can memcpy from the PointCloud2 data buffer to a PointXYZIRADRT */
 bool is_data_layout_compatible_with_PointXYZIRADRT(const sensor_msgs::msg::PointCloud2 & input);
 
+/** \brief Return offset of X field (if any). The field should match PointXYZIRADRT one */
+std::optional<std::size_t> getXOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg);
+
+/** \brief Return offset of Y field (if any). The field should match PointXYZIRADRT one */
+std::optional<std::size_t> getYOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg);
+
+/** \brief Return offset of Z field (if any). The field should match PointXYZIRADRT one */
+std::optional<std::size_t> getZOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg);
+
+/** \brief Return offset of intensity field (if any). The field should match PointXYZIRADRT one */
+std::optional<std::size_t> getIntensityOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg);
+
+/** \brief Return offset of ring field (if any). The field should match PointXYZIRADRT one */
+std::optional<std::size_t> getRingOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg);
+
+/** \brief Return offset of azimuth field (if any). The field should match PointXYZIRADRT one */
+std::optional<std::size_t> getAzimuthOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg);
+
+/** \brief Return offset of distance field (if any). The field should match PointXYZIRADRT one */
+std::optional<std::size_t> getDistanceOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg);
+
+/** \brief Return offset of return type field (if any). The field should match PointXYZIRADRT one */
+std::optional<std::size_t> getReturnTypeOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg);
+
+/** \brief Return offset of time stamp field (if any). The field should match PointXYZIRADRT one */
+std::optional<std::size_t> getTimeStampOffset(const sensor_msgs::msg::PointCloud2 & cloud_msg);
+
 }  // namespace pointcloud_preprocessor::utils
 
 #endif  // POINTCLOUD_PREPROCESSOR__UTILITY__UTILITIES_HPP_

sensing/pointcloud_preprocessor/package.xml

@@ -31,6 +31,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>