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>
autowarefoundation · Apr 6, 2023 · ac3427a · ac3427a
1 parent 0f9149b
commit ac3427a
Show file tree

Hide file tree

Showing 5 changed files with 221 additions and 80 deletions.
diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md
@@ -22,11 +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             |             |
+| 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
@@ -20,6 +20,7 @@
 
 #include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
 
+#include <utility>
 #include <vector>
 
 namespace pointcloud_preprocessor
@@ -33,32 +34,24 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter
   virtual void filter(
     const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output);
 
+  // TODO(sykwer): 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 distance_ratio_;
   double object_length_threshold_;
   int num_points_threshold_;
+  uint16_t max_rings_num_;
 
   /** \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(
-    sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr,
-    const std::vector<std::size_t> & tmp_indices)
-  {
-    PointXYZI * front_pt = reinterpret_cast<PointXYZI *>(&input_ptr->data[tmp_indices.front()]);
-    PointXYZI * back_pt = reinterpret_cast<PointXYZI *>(&input_ptr->data[tmp_indices.back()]);
-
-    const auto x_diff = front_pt->x - back_pt->x;
-    const auto y_diff = front_pt->y - back_pt->y;
-    const auto z_diff = front_pt->z - back_pt->z;
-    return static_cast<int>(tmp_indices.size()) > num_points_threshold_ ||
-           (x_diff * x_diff) + (y_diff * y_diff) + (z_diff * z_diff) >=
-             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
@@ -33,6 +33,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>

diff --git a/sensing/pointcloud_preprocessor/src/filter.cpp b/sensing/pointcloud_preprocessor/src/filter.cpp
@@ -56,6 +56,7 @@
 #include <pcl/io/io.h>
 
 #include <memory>
+#include <set>
 #include <string>
 #include <utility>
 #include <vector>
@@ -123,8 +124,10 @@ 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.
-  auto callback = filter_name == "CropBoxFilter" ? &Filter::faster_input_indices_callback
-                                                 : &Filter::input_indices_callback;
+  std::set<std::string> supported_nodes = {"CropBoxFilter", "RingOutlierFilter"};
+  auto callback = supported_nodes.find(filter_name) != supported_nodes.end()
+                    ? &Filter::faster_input_indices_callback
+                    : &Filter::input_indices_callback;
 
   if (use_indices_) {
     // Subscribe to the input using a filter