perf(autoware_ndt_scan_matcher): replacement of pcl::KdTreeFLANN with nanoflann

[taisa1]

Description

This PR speeds up callback_sensor_points_main() on autoware_ndt_scan_matcher without changing the logical output.

The tail latency gets about x1.65 faster with this PR merged.

In this PR, I propose to replace pcl::KdTreeFLANN with nanoflann in this node.
nanoflann is a header-only kdtree library, a fork of FLANN used in PCL.

It is provided under the BSD license. kdtree_nanoflann.hpp contains code from nanoflann, so the license is displayed.

nanoflann.hpp is a copy from nanoflann v1.6.3, and is currently placed in include directory of this node, but will need to be moved if it is used on other nodes in the future.

Measurement Condition

• Ubuntu22.04 + ROS2 Humble + Autoware Universe rosbag simulation
• not core isolated / not core flequency fixed (so multi-threaded)

Related links

• nanoflann

Private Links:

• Performance analysis of previous implementation (TIER IV Internal link)
• Description, tests, measurements of nanoflann (TIER IV Internal link)

How was this PR tested?

• I set up an environment that fixes the input to the node, and confirmed that changing to nanoflann results in an increase in speed without changing the output (details in TIER IV internal).
• I confirmed that it can be run normally with sample rosbag simulation.

Notes for reviewers

In this PR, the library is temporarily placed under the include/ of autoware_ndt_scan_matcher, but

• nanoflann.hpp is a bit too large (80+ kB) to place as source code
• There are other nodes that use kdtree

So it might be better to install it in a location like /usr/include/ instead of placing it as source code.

Do you have any comments about it?

Interface changes

None.

Effects on system behavior

None.

[github-actions]

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

• You've checked our contribution guidelines.
• Your PR follows our pull request guidelines.
• All required CI checks pass before marking the PR ready for review.

[SakodaShintaro]

Thank you for the pull request!

Since this pull request has a big impact, we are running tests on all the evaluation data.

TIER IV internal links

• https://evaluation.tier4.jp/evaluation/reports/fe5b07e9-35ff-536f-a63f-e896d063a608?project_id=autoware_dev
• https://evaluation.tier4.jp/evaluation/reports/9e30d262-d004-5510-8222-a04bddf8a636?project_id=prd_jt

Once these are complete, we will check for any errors and compare the processing times with the previous ones.

[codecov]

Codecov Report

Attention: Patch coverage is 82.50000% with 42 lines in your changes missing coverage. Please review.

Project coverage is 28.42%. Comparing base (a98a8f6) to head (5d37112).
Report is 3 commits behind head on main.

Files with missing lines	Patch %	Lines
...de/autoware/ndt_scan_matcher/ndt_omp/nanoflann.hpp	82.79%	14 Missing and 23 partials ⚠️
...ware/ndt_scan_matcher/ndt_omp/kdtree_nanoflann.hpp	77.27%	2 Missing and 3 partials ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main   #10013      +/-   ##
==========================================
+ Coverage   28.29%   28.42%   +0.12%     
==========================================
  Files        1484     1486       +2     
  Lines      111087   111334     +247     
  Branches    43149    43231      +82     
==========================================
+ Hits        31437    31647     +210     
- Misses      76631    76640       +9     
- Partials     3019     3047      +28     

Flag	Coverage Δ		*Carryforward flag
differential	28.51% <82.50%> (?)
total	28.30% <ø> (+<0.01%)	⬆️	Carriedforward from fd7db7b

*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.

[SakodaShintaro]

@taisa1
spell-check-differential detected many errors.
https://github.com/autowarefoundation/autoware.universe/actions/runs/13024889595/job/36332234200?pr=10013

Could you fix them?

ref) https://github.com/streetsidesoftware/cspell/tree/main/packages/cspell#in-document-settings

[taisa1]

Fixed spell check error.
By the way, it seems that there are some cases in which the evaluation test fails, so maybe I should reconsider the validity?

[SakodaShintaro]

Thank you for the correction.

By the way, it seems that there are some cases in which the evaluation test fails, so maybe I should reconsider the validity?

It's probably fine because some of the data is abnormal data that is expected to fail. I'll check the results tomorrow.

[YamatoAndo]

@taisa1 Thank you for the pull request!

So it might be better to install it in a location like /usr/include/ instead of placing it as source code.
Do you have any comments about it?

Is it not possible to install nanoflann via apt?
If you have made code modifications to nanoflann, I think it would be better to place it in include/autoware/ndt_scan_matcher rather than /usr/include/.

Alternatively, you could create a separate repository for nanoflann and include it in autoware.repos.

[YamatoAndo]

In any case, there is no problem with placing it in include/autoware/ndt_scan_matcher.

[SakodaShintaro]

The results are good. 👍

• Mean error [m] with respect to the reference trajectory

Almost no degradation

• Mean execution time [msec]

Faster than before

I will check the code later.

[taisa1]

Is it not possible to install nanoflann via apt?

nanoflann v1.4.2 can be installed via apt, but it's older than latest v1.6.3 and there are some fixes, API changes and optimizations between them.

I haven't modified code of nanoflann, but if there is no problem it might be better to place the latest nanoflann in autoware_ndt_scan_matcher (as this PR is).

[SakodaShintaro]

I ran ndt-ekf-deterministic-test (TIER IV INTERNAL) and the result was passed.
This means that this pull request does not change the behavior of ndt and ekf at all.

However, nanoflann.hpp has 2470 lines...
It takes a lot of effort to review.

[SakodaShintaro]

The speedup brought by this pull request is very good and we would like to adopt it, but we are thinking how to maintain nanoflann.hpp. Please wait a little longer.