[search] Add support for nanoflann (faster alternative to pcl::search::KdTree)

[mvieth]

Design considerations

The new KdTreeNanoflann should be usable in all situations where currently the FLANN-based kd-tree is used and all parameters etc of the new class should be configurable. At the same time, the existing classes should be changed as little as possible.
The feature classes accept a pcl::search::Search object, while the registration classes use a pcl::search::KdTree<PointTarget> object, which is actually pcl::search::KdTree<PointTarget, pcl::KdTreeFLANN<PointTarget>> (default for second template parameter).
I decided to make KdTreeNanoflann inherit from pcl::search::KdTree<PointT, pcl::KdTreeFLANN<PointT>>, that way it can be used almost everywhere, and existing classes do not have to be changed.

Limitation

There is one minor limitation which is, I believe, not solvable without breaking the ABI. pcl::search::KdTree has a few methods, most notably setPointRepresentation, which should be overridden by KdTreeNanoflann, but these methods are not marked virtual. So the dynamic (runtime) dispatch does not work. Adding the virtual keyword would break ABI. For example, the following would not work:

pcl::IterativeClosestPoint<PointNormalT, PointNormalT> reg;
// pass KdTreeNanoflann as search method. Since the registration classes internally store the
// search method as a pcl::search::KdTree (parent class of KdTreeNanoflann), it will be cast
reg.setSearchMethodTarget(pcl::make_shared<pcl::search::KdTreeNanoflann<pcl::PointXYZ>>());
// reg will call setPointRepresentation on the search method, but since setPointRepresentation
// in KdTree is not marked virtual, dynamic dispatch does not work and setPointRepresentation
// of KdTree is called, instead of setPointRepresentation of KdTreeNanoflann (which would be correct).
reg.setPointRepresentation (pcl::make_shared<const MyPointRepresentation> (point_representation));

There is however an easy workaround:

pcl::IterativeClosestPoint<PointNormalT, PointNormalT> reg;
auto kdtree_nanoflann = pcl::make_shared<pcl::search::KdTreeNanoflann<pcl::PointXYZ>>();
kdtree_nanoflann->setPointRepresentation(pcl::make_shared<const MyPointRepresentation> (point_representation));
reg.setSearchMethodTarget(kdtree_nanoflann);

Since it would be great to merge this PR as soon as possible and have the new KdTreeNanoflann class included in the PCL 1.15.1 release (no ABI break possible), I would suggest to accept this limitation for now, and after the PCL 1.15.1 release when breaking ABI is acceptable, we can remove this limitation by adding virtual to the functions in pcl::search::KdTree and override to the corresponding functions in pcl::search::KdTreeNanoflann.

Notes

• Since the new class is header-only, meaning it is compiled in the user project instead of when building PCL, it is important to enable optimizations when compiling the user code, e.g. -DCMAKE_BUILD_TYPE=RelWithDebInfo or -DCMAKE_BUILD_TYPE=Release, or choosing the "Release" config in Visual Studio.
• nanoflann versions currently tested in CI pipeline: 1.4.2 (Ubuntu 22.04), 1.5.4 (Ubuntu 24.04), 1.5.5 (Winx86), 1.7.1 (macOS and Winx64). Additionally, I am testing 1.6.0 locally.

How to use

Example 1: using KdTreeNanoflann in NormalEstimation:

pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
auto tree = pcl::make_shared<pcl::search::KdTreeNanoflann<pcl::PointXYZ>>();
ne.setSearchMethod (tree);
// rest as usual

Example 2: using KdTreeNanoflann in ICP:

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setSearchMethodSource(pcl::make_shared<pcl::search::KdTreeNanoflann<pcl::PointXYZ>>());
icp.setSearchMethodTarget(pcl::make_shared<pcl::search::KdTreeNanoflann<pcl::PointXYZ>>());
// rest as usual

Example 3: using a L1 distance norm:

pcl::search::KdTreeNanoflann<PointXYZ, 3, pcl::search::L1_Adaptor> kdtree_nanoflann;
kdtree_nanoflann.setInputCloud (my_cloud);
// now ready to use kdtree_nanoflann.nearestKSearch(...) and
// kdtree_nanoflann.radiusSearch(...);

Example 4: using KdTreeNanoflann with features instead of 3D points:

pcl::search::KdTreeNanoflann<pcl::FPFHSignature33, 33> kdtree_nanoflann_feat;
kdtree_nanoflann_feat.setInputCloud(my_features_cloud);
// now ready for searching

Benchmarks

I did some benchmarks to get an idea how the new KdTreeNanoflann compares to the FLANN based KdTree. Of course, these depend a lot on the processor, the point clouds used, and other factors. Switching to KdTreeNanoflann reduces the time spent to:
NormalEstimation (small radius): 75-78%
NormalEstimation (large radius): 76-78%
EuclideanClusterExtraction: 75-80%
RadiusOutlierRemoval: 25-40%
ICP (does nearestKSearch with k=1): 30-50%
NormalEstimation (small k): 85-90%
NormalEstimation (large k): 91-97%
1nn search (tree building not included): 29-32%
5nn search (tree building not included): 48-49%
50nn search (tree building not included): 83-85%
radius (small radius, sorted, tree building not included): 49-53%
radius (small radius, unsorted, tree building not included): 39-42%
radius (large radius, sorted, tree building not included): 77-81%
radius (large radius, unsorted, tree building not included): 63-65%
Where 100% refers to the time taken with pcl::search::KdTree, so e.g. 50% would be twice as fast as before.

To discuss for the future

• some classes, e.g. SampleConsensusPrerejective and SampleConsensusInitialAlignment, use pcl::search::KdTree or pcl::KdtreeFLANN for searching and do not offer a way to change this. We could consider adding the option to pass in another search method (e.g. KdTreeNanoflann)
• we could consider making KdTreeNanoflann the default for unorganized point clouds when the user did not specify a search method (currently it is pcl::search::KdTree for unorganized clouds)
• Maybe add explicit instantiations in pcl_search? If yes, for which template parameters?

[larshg]

Nice work ! 😄