perf(ring_outlier_filter): a cache friendly impl (continuation of VRi…

…chardJP's work) (autowarefoundation#4185)

* perf(ring_outlier_filter): a 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 autowarefoundation#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

* style(pre-commit): autofix

* resize vector to data size

Signed-off-by: Vincent Richard <richard-v@macnica.co.jp>

* cleaning

Signed-off-by: Vincent Richard <richard-v@macnica.co.jp>

* cleaner utilities impl

Signed-off-by: Vincent Richard <richard-v@macnica.co.jp>

* Correct ring_outlier_filter algorithm

The implementation by VRichardJP was erroneously using indices into the
input->fields array as byte offsets for copying values from the input.
Added the missing step to access the offset of the field pointed at by
the respective index.

When combining the first and last walk, the num_points fields of the two
walks need to be summed up as well. isCluster checks for num_points
as well as distance.

Signed-off-by: Maximilian Schmeller <maximilian.schmeller@tier4.jp>

* Optimize memory usage of walks array

The walks array in the previous implementation could in the worst case grow
to the number of input points (if every point is an outlier).
This increased computation time for gradually growing the walks vector.

The current implementation only saves the first and current walk for
every ring. The isCluster check is performed right when a walk is
completed, and a bool vector akin to the previous walk_is_cluster
vector is maintained.

Signed-off-by: Maximilian Schmeller <maximilian.schmeller@tier4.jp>

* Remove debug timers and outputs

Signed-off-by: Maximilian Schmeller <maximilian.schmeller@tier4.jp>

* style(pre-commit): autofix

* Reduce amount of helper functions and data structures

Remove functions for finding field offsets of Pointcloud2 points,
replace them with the previous approach of assuming their position.
This is okay since the ring_outlier_filter specification clearly states
the expected pointcloud format.

Remove accessor lambda functions for ring/azimuth/etc. because they are
onlu used once anyways.

Remove the walks vector and save the first/current walk structs directly
in the RingWalkInfo struct. This makes the code more readable and
maintainable.

Signed-off-by: Maximilian Schmeller <maximilian.schmeller@tier4.jp>

* style(pre-commit): autofix

* Check for invalid pointcloud format

In the previous revision (and before this PR), pointcloud field indices
in ring_outlier_filter were hardcoded and fields were assumed to exist.

VRichardJP solved this by implementing individual accessor functions,
which ended up to be a bit verbose for this small piece of
functionality.

Now, there is one generic accessor function and the faster_filter
function exits if any of the fields expected does not exist.
The expected field data type is also checked.

Signed-off-by: Maximilian Schmeller <maximilian.schmeller@tier4.jp>

* style(pre-commit): autofix

---------

Signed-off-by: Vincent Richard <richard-v@macnica.co.jp>
Signed-off-by: Maximilian Schmeller <maximilian.schmeller@tier4.jp>
Co-authored-by: Vincent Richard <richard-v@macnica.co.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Maximilian Schmeller <maximilian.schmeller@tier4.jp>

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

@@ -46,29 +46,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/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>