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

* Add support for nanoflann

* Fix and additional test

Author: mvieth

CMakeLists.txt

@@ -361,6 +361,14 @@ else()
   endif()
 endif()
 
+find_package(nanoflann 1.4.2 QUIET)
+if(TARGET nanoflann::nanoflann)
+  set(PCL_HAS_NANOFLANN 1)
+  message(STATUS "Found nanoflann")
+else()
+  message(STATUS "nanoflann was not found, so some PCL classes will not be available.")
+endif()
+
 # libusb
 option(WITH_LIBUSB "Build USB RGBD-Camera drivers" TRUE)
 if(WITH_LIBUSB)

pcl_config.h.in

@@ -49,6 +49,10 @@
 
 #cmakedefine HAVE_DAVIDSDK 1
 
+#ifndef PCL_HAS_NANOFLANN
+#cmakedefine PCL_HAS_NANOFLANN 1
+#endif
+
 // SSE macros
 #cmakedefine HAVE_POSIX_MEMALIGN
 #cmakedefine HAVE_MM_MALLOC

search/CMakeLists.txt

@@ -22,6 +22,7 @@ set(srcs
 set(incs
   "include/pcl/${SUBSYS_NAME}/search.h"
   "include/pcl/${SUBSYS_NAME}/kdtree.h"
+  "include/pcl/${SUBSYS_NAME}/kdtree_nanoflann.h"
   "include/pcl/${SUBSYS_NAME}/brute_force.h"
   "include/pcl/${SUBSYS_NAME}/organized.h"
   "include/pcl/${SUBSYS_NAME}/octree.h"

search/include/pcl/search/impl/kdtree.hpp

@@ -48,12 +48,21 @@ pcl::search::KdTree<PointT,Tree>::KdTree (bool sorted)
 {
 }
 
+template <typename PointT, class Tree>
+pcl::search::KdTree<PointT,Tree>::KdTree (const std::string& name, bool sorted)
+  : pcl::search::Search<PointT> (name, sorted)
+{
+}
+
 ///////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT, class Tree> void
 pcl::search::KdTree<PointT,Tree>::setPointRepresentation (
     const PointRepresentationConstPtr &point_representation)
 {
-  tree_->setPointRepresentation (point_representation);
+  if(tree_)
+    tree_->setPointRepresentation (point_representation);
+  else
+    PCL_ERROR("Calling setPointRepresentation on KdTreeNanoflann that has been cast to KdTree is not possible in this PCL version. Call setPointRepresentation directly on the KdTreeNanoflann object.\n");
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////
@@ -68,7 +77,10 @@ pcl::search::KdTree<PointT,Tree>::setSortedResults (bool sorted_results)
 template <typename PointT, class Tree> void
 pcl::search::KdTree<PointT,Tree>::setEpsilon (float eps)
 {
-  tree_->setEpsilon (eps);
+  if(tree_)
+    tree_->setEpsilon (eps);
+  else
+    PCL_ERROR("Calling setEpsilon on KdTreeNanoflann that has been cast to KdTree is not possible in this PCL version. Call setEpsilon directly on the KdTreeNanoflann object.\n");
 }
 
 ///////////////////////////////////////////////////////////////////////////////////////////

search/include/pcl/search/kdtree.h

@@ -161,6 +161,8 @@ namespace pcl
                       std::vector<float> &k_sqr_distances,
                       unsigned int max_nn = 0) const override;
       protected:
+        /** \brief This leaves the internal tree_ object uninitialized! */
+        KdTree (const std::string& name, bool sorted);
         /** \brief A pointer to the internal KdTree object. */
         KdTreePtr tree_;
     };