[registration] Add OMP based Multi-threading to NDT

[koide3]

This PR adds a multi-threaded and SSE-optimized NDT (for reference: #4180 #4135). Taking a diff with ndt.(h|hpp) would be helpful to see the added/updated codes.

[Added] NormalDistributionsTransformOMP

• The following two methods are added to the original NDT
• setNumThreads() specifies the number of threads to be used
• setNeighborSearchMethod() offers a trade-off between registration stability and speed.

Benchmark result on KITTI00 (ApproxVoxelGrid: 0.25m, voxel resolution: 2.0m, transformation epsilon 1e-3):
(Average processing time per scan and relative trajectory errors are reported)

--- NDT ---
Time per scan    : 496.053 +- 162.218 [msec]
RTE(10, 50, 100m): 0.320342 1.564389 3.363665


--- NDT_OMP (KDTREE, 1 thread) ---
Time per scan    : 367.684 +- 126.874 [msec]
RTE(10, 50, 100m): 0.320346, 1.564428, 3.363695

--- NDT_OMP (KDTREE, 16 threads) ---
Time per scan    : 76.216 +- 39.373 [msec]
RTE(10, 50, 100m): 0.320346, 1.564428, 3.363695


--- NDT_OMP (DIRECT1, 1 thread) ---
Time per scan    : 73.426 +- 21.062 [msec]
RTE(10, 50, 100m): 0.240691, 0.931008, 2.039546

--- NDT_OMP (DIRECT1, 16 threads) ---
Time per scan    : 23.819 +- 7.633 [msec]
RTE(10, 50, 100m): 0.240691, 0.931008, 2.039546


--- NDT_OMP (DIRECT7, 1 thread) ---
Time per scan    : 287.151 +- 95.607 [msec]
RTE(10, 50, 100m): 0.216932, 0.926056, 1.977688

--- NDT_OMP (DIRECT7, 16 threads) ---
Time per scan    : 60.003 +- 32.637 [msec]
RTE(10, 50, 100m): 0.216932, 0.926056, 1.977688


--- NDT_OMP (DIRECT26, 16 threads) ---
Time per scan    : 241.036 +- 133.761 [msec]
RTE(10, 50, 100m): 13.434766, 52.471817, 81.097012

--- NDT_OMP (DIRECT27, 16 threads) ---
Time per scan    : 138.623 +- 84.487 [msec]
RTE(10, 50, 100m): 0.293359, 1.378685, 3.139081

It's not a very comprehensive comparison, but we can see the trend of processing speed and accuracy.

Note:

• It's faster than the original NDT even when the number of threads is set to 1 thanks to the SSE optimization.
• KDTREE shows the same result as the original implementation (very small difference is caused by SSE single-float calculation).
• DIRECT1 is very fast, but can be unreliable when the voxel resolution is small
• DIRECT7 is faster and more accurate than KDTREE. I think this should be the default method.
• DIRECT27 is worse than DIRECT7 in terms of both processing speed and accuracy in this case. But, it may have a larger convergence basin than the other methods.
• I didn't include DIRECT26 in NDT_OMP because it looks not worthy.

[kunaltyagi]

Tagging @shrijitsingh99 for relevance with GSoC

[shrijitsingh99]

Tagging @shrijitsingh99 for relevance with GSoC

Looking into this seems as if this will be similar to how integration with filters is going to take place. The only point which would need to be taken care of is the setNeighborSearchMethod which doesn't seem to be present the original ndt implementation.

Is the implementation set to replace the original CPU implementation too?

[kunaltyagi]

Should we create a separate issue to note that the non-special NDT code needs update (till the level of NDT_OMP implementation)? If (or rather when) the executors get integrated, a large update would not be needed.

[shrijitsingh99]

Is an update to non-special needed? Is there any reason we are not just removing original NDT and renaming ndt_omp to ndt besides maybe potential bugs?

[koide3]

Is an update to non-special needed? Is there any reason we are not just removing original NDT and renaming ndt_omp to ndt besides maybe potential bugs?

Some concerns about replacing the original ndt with the new one are:

• The new code requires more memory during registration, because it allocates score, gradient, and hessian for each input point for thread safety
• The readability of updateDerivatives gets a bit worse in exchange for SSE optimization
• Does the policy of PCL allow to use OpenMP in a method whose name doesn't explicitly indicate its a multi-threaded algorithm? I thought PCL intentionally separate sinle-thread and multi-threaded algorithms like pcl::NormalEstimation and pcl::NormalEstimationOMP (I don't really know...)

[kunaltyagi]

Regarding the last point:

OMP and normal implementations started diverging and it was decided to bring both under one umbrella and to use #pragma omp if to achieve that.

We'll be seeing some larger unification of API across PCL 🤞 Hopefully, all goes well

If possible something along the lines of PRs started by @shrijitsingh99 would be better 😄

[koide3]

OK, the other concerns don't matter IMO. I think we can use less memory consuming routines when num_threads = 1. Should we append the new additions to this PR #4180 (or open another PR)?

[kunaltyagi]

Please make the modifications here since we've merged 1.11.1 and next target release is API/ABI breaking 1.12

[koide3]

OK, thanks! I'll push updates here.

[koide3]

I pushed multi-threaded NDT code. In addition to #4277 (comment), I made two modifications:

• trialValueSelectionMT() in ndt.hpp
  Since the execution order of a multi-threaded loop can be changed, computeDerivatives() may return slightly different scores for the same input due to the floating point summation error. This can cause a condition where a_l == a_t but f_t != f_l (f_* is scores, a_* are line search values) in trialValueSelectionMT() that results in yielding NaN at:

  pcl/registration/include/pcl/registration/impl/ndt.hpp

  Line 481 in ff9adcf

  const double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;

  
  To avoid this, I added some condition checks in trialValueSelectionMT(): https://github.com/koide3/pcl/blob/dcb4be171d2e504c85c7f976496a7b719cc4ae6e/registration/include/pcl/registration/impl/ndt.hpp#L587

• test_ndt.cpp
  NDT doesn't use kdtree, and the following test code doesn't make sense:

  pcl/test/registration/test_ndt.cpp

  Line 75 in ff9adcf

  for (int iter = 0; iter < 4; iter++)

  
  I removed this test and added test for the consistency of single and multi-threaded results.

I'm testing the correctness of the updated NDT on KITTI. I'll report the test result once it's finished.

[kunaltyagi]

Try not to force-push. Reviewers lose all context

[koide3]

Oh, sorry, I'll avoid force pushing

[kunaltyagi]

You mentioned an unrolled loop, but I can't find it

[kunaltyagi]

Could you please verify that the deprecated functions aren't used anywhere inside PCL (except any code explicitly testing them)

[koide3]

All the deprecated ones are all protected members, and they are not used in anywhere other than NDT.

[kunaltyagi]

LGTM. Minor comment only

[koide3]

Can I do rebase, squash, and force-push to resolve the conflict?

[kunaltyagi]

There's a conflict due to the merge of formatting in registration module

[koide3]

I resolved the conflict.

[kunaltyagi]

GCC 16.04 is failing because the it doesn't accept the fourth lambda

[koide3]

I remove the lambda in INSTANTIATE_TEST_SUITE_P

[kunaltyagi]

LGTM

[koide3]

I resolved conflicts.

[larshg]

Should this be merged 🚀 ?

[kunaltyagi]

Needs 1 more review. Currently, requested from mvieth, but please feel free to review instead Lars :)