Revert "perf(ring_outlier_filter): a cache friendly impl (continuatio…

…n of VRichardJP's work) (autowarefoundation#4185)" This reverts commit e45c1ce.
tzhong518 · Jan 23, 2024 · 75f4009 · 75f4009
1 parent e4cae6f
commit 75f4009
Show file tree

Hide file tree

Showing 4 changed files with 123 additions and 249 deletions.
diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md
@@ -22,12 +22,13 @@ 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           |             |
+| 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` |
 
 ## 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
@@ -46,13 +46,29 @@ 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
@@ -31,7 +31,6 @@
   <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>