perf(occupancy_grid_map_outlier_filter): improve performance

[takanotaiga]

Signed-off-by: Taiga Takano [email protected]

Description

This PR improves performance with the following major changes:

• Elimination of unnecessary conversions between PCL and ROS Messages.
• Changing the function used for filtering from radiusSearch to nearestKSearch.
• Reducing computational load by revising point cloud processing.

Measurement

With this PR, the node (measurement is the overall latency of the callback) will be approximately more than twice as fast.

PERCENTILE	Before [ms]	After [ms]	Improvement Factor
99%	13.0	6.2	2.1 times
90%	10.5	4.4	2.4 times
50%	0.7	0.4	1.8 times

[YoshiRi]

LGTM

[veqcc]

If possible, .size() should not be used to many times.
This is because .size() 's complexity depends on its data structure (eg. vector is O(1) but linked list is O(N)) and the internal implementation is usually hidden in libraries.

[codecov]

Codecov Report

Attention: Patch coverage is 0% with 18 lines in your changes are missing coverage. Please review.

Project coverage is 15.04%. Comparing base (8bdb542) to head (e0225cc).
Report is 21 commits behind head on main.

Files	Patch %	Lines
.../src/occupancy_grid_map_outlier_filter_nodelet.cpp	0.00%	18 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #5980      +/-   ##
==========================================
+ Coverage   14.80%   15.04%   +0.24%     
==========================================
  Files        1915     1839      -76     
  Lines      132245   126319    -5926     
  Branches    39317    37969    -1348     
==========================================
- Hits        19576    19009     -567     
+ Misses      90838    86027    -4811     
+ Partials    21831    21283     -548     

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	15.05% <ø> (+0.24%)	⬆️	Carriedforward from 3d1d84e

*This pull request uses carry forward flags. Click here to find out more.

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[YoshiRi]

@veqcc Is this PR ready for merge?

[takanotaiga]

@YoshiRi
The PR has been revised to address certain points that were raised, and is now in a state where it can be merged. As for the changes made to the filter part (changes to the nearestKSearch function), these have been reverted.

[YoshiRi]

Thanks! LGTM

[takanotaiga]

I measured the latency in the subscriber function onOccupancyGridMapAndPointCloud2 and mainly improved the latency in splitPointCloudFrontBack. The changes made the other day have not improved the speed as initially anticipated.

[veqcc]

@takanotaiga
Thank you for measuring the performance after improvements!
It looks the first figure indicates that the latency becomes worse, right?

[takanotaiga]

@veqcc
When comparing with percentiles, there is a slight improvement.

PERCENTILE	Before [ms]	After [ms]
99%	16.9	16.4
90%	13.1	12.6
50%	9.2	8.8

[veqcc]

@takanotaiga
You mean the onOccupancy... function slightly improved, while the splitPoint... function got worse, right?
Your improvements are implemented only in splitPoint..., so it sounds strange for me.
Could you give me more detailed explanation?

[takanotaiga]

@veqcc
First, it should be noted that the splitPoint... function is called within the onOccupancy... function. And since the onOccupancy... function is the main callback for this node, it measures it.

I thought that simply displaying the improvement graph of the main callback might not clearly show the difference, so I also displayed the improved splitPoint... function to make the improvement more understandable.

[veqcc]

@takanotaiga
Thank you! I understand the point.
My question here is "Why splitPoint... latency got larger?".

[takanotaiga]

@veqcc

I'm sorry. I misunderstood the graph. Tomorrow, I will check if the numbers are incorrect. It seems that I might have confused "before" and "after."

[takanotaiga]

@veqcc
As a result of measuring performance to verify once again based on the feedback, performance improvements were observed in both the splitPoint... function and the onOccupancy... function.

onOccupancyGridMapAndPointCloud2

	before[ms]	after[ms]
99%	26.98	24.60
90%	21.29	18.98
50%	13.88	12.77

splitPointCloudFrontBack

	before[ms]	after[ms]
99%	2.20	1.02
90%	1.58	0.63
50%	1.07	0.38

[veqcc]

LGTM 👍