Replace more instances of KdTreeFLANN and pcl::search::KdTree

Author: mvieth

examples/features/example_difference_of_normals.cpp

@@ -67,19 +67,6 @@ int main (int argc, char *argv[])
         pcl::fromPCLPointCloud2 (blob, *cloud);
         std::cout << "done." << std::endl;
 
-  SearchPtr tree;
-
-  if (cloud->isOrganized ())
-  {
-    tree.reset (new pcl::search::OrganizedNeighbor<PointT> ());
-  }
-  else
-  {
-      tree.reset (new pcl::search::KdTree<PointT> (false));
-  }
-
-  tree->setInputCloud (cloud);
-
   PointCloud<PointT>::Ptr small_cloud_downsampled;
   PointCloud<PointT>::Ptr large_cloud_downsampled;
 
@@ -110,7 +97,6 @@ int main (int argc, char *argv[])
   // Compute normals using both small and large scales at each point
   pcl::NormalEstimationOMP<PointT, PointNT> ne;
   ne.setInputCloud (cloud);
-        ne.setSearchMethod (tree);
 
   /**
    * NOTE: setting viewpoint is very important, so that we can ensure

filters/include/pcl/filters/impl/convolution_3d.hpp

@@ -197,7 +197,7 @@ pcl::filters::Convolution3D<PointInT, PointOutT, KernelT>::initCompute ()
   // Initialize the spatial locator
   if (!tree_)
   {
-    tree_.reset (pcl::search::autoSelectMethod<PointT>(surface_, false, pcl::search::Purpose::radius_search));
+    tree_.reset (pcl::search::autoSelectMethod<PointInT>(surface_, false, pcl::search::Purpose::radius_search));
   }
   else
   {

gpu/kinfu_large_scale/tools/standalone_texture_mapping.cpp

@@ -44,6 +44,7 @@
 #include <pcl/common/transforms.h>
 #include <pcl/features/normal_3d.h>
 #include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/search/kdtree.h>
 #include <pcl/surface/texture_mapping.h>
 #include <pcl/io/vtk_lib_io.h>
 #include <pcl/io/ply_io.h>

registration/include/pcl/registration/gicp.h

@@ -439,7 +439,7 @@ class GeneralizedIterativeClosestPoint
   template <typename PointT>
   void
   computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud,
-                     const typename pcl::search::KdTree<PointT>::Ptr tree,
+                     const typename pcl::search::Search<PointT>::Ptr tree,
                      MatricesVector& cloud_covariances);
 
   /** \return trace of mat1 . mat2

registration/include/pcl/registration/impl/gicp.hpp

@@ -72,7 +72,7 @@ template <typename PointT>
 void
 GeneralizedIterativeClosestPoint<PointSource, PointTarget, Scalar>::computeCovariances(
     typename pcl::PointCloud<PointT>::ConstPtr cloud,
-    const typename pcl::search::KdTree<PointT>::Ptr kdtree,
+    const typename pcl::search::Search<PointT>::Ptr kdtree,
     MatricesVector& cloud_covariances)
 {
   if (k_correspondences_ > static_cast<int>(cloud->size())) {

registration/include/pcl/registration/impl/ia_fpcs.hpp

@@ -45,6 +45,7 @@
 #include <pcl/registration/ia_fpcs.h>
 #include <pcl/registration/transformation_estimation_3point.h>
 #include <pcl/sample_consensus/sac_model_plane.h>
+#include <pcl/search/auto.h>
 
 #include <limits>
 
@@ -58,8 +59,8 @@ pcl::getMeanPointDensity(const typename pcl::PointCloud<PointT>::ConstPtr& cloud
   const float max_dist_sqr = max_dist * max_dist;
   const std::size_t s = cloud->size();
 
-  pcl::search::KdTree<PointT> tree;
-  tree.setInputCloud(cloud);
+  typename pcl::search::Search<PointT>::Ptr tree(pcl::search::autoSelectMethod<PointT>(
+      cloud, true, pcl::search::Purpose::many_knn_search));
 
   float mean_dist = 0.f;
   int num = 0;
@@ -71,7 +72,7 @@ pcl::getMeanPointDensity(const typename pcl::PointCloud<PointT>::ConstPtr& cloud
     firstprivate(ids, dists_sqr) reduction(+ : mean_dist, num)                         \
     firstprivate(s, max_dist_sqr) num_threads(nr_threads)
   for (int i = 0; i < 1000; i++) {
-    tree.nearestKSearch((*cloud)[rand() % s], 2, ids, dists_sqr);
+    tree->nearestKSearch((*cloud)[rand() % s], 2, ids, dists_sqr);
     if (dists_sqr[1] < max_dist_sqr) {
       mean_dist += std::sqrt(dists_sqr[1]);
       num++;
@@ -92,8 +93,8 @@ pcl::getMeanPointDensity(const typename pcl::PointCloud<PointT>::ConstPtr& cloud
   const float max_dist_sqr = max_dist * max_dist;
   const std::size_t s = indices.size();
 
-  pcl::search::KdTree<PointT> tree;
-  tree.setInputCloud(cloud);
+  typename pcl::search::Search<PointT>::Ptr tree(pcl::search::autoSelectMethod<PointT>(
+      cloud, true, pcl::search::Purpose::many_knn_search));
 
   float mean_dist = 0.f;
   int num = 0;
@@ -109,7 +110,7 @@ pcl::getMeanPointDensity(const typename pcl::PointCloud<PointT>::ConstPtr& cloud
     firstprivate(ids, dists_sqr) reduction(+ : mean_dist, num) num_threads(nr_threads)
 #endif
   for (int i = 0; i < 1000; i++) {
-    tree.nearestKSearch((*cloud)[indices[rand() % s]], 2, ids, dists_sqr);
+    tree->nearestKSearch((*cloud)[indices[rand() % s]], 2, ids, dists_sqr);
     if (dists_sqr[1] < max_dist_sqr) {
       mean_dist += std::sqrt(dists_sqr[1]);
       num++;

search/include/pcl/search/auto.h

@@ -28,7 +28,20 @@ namespace pcl {
       * \param[in] purpose Optional, can be used to give more information about what this search method will be used for, to achieve optimal performance
       */
     template<typename PointT>
-    pcl::search::Search<PointT> * autoSelectMethod(const typename pcl::PointCloud<PointT>::ConstPtr& cloud, bool sorted_results, Purpose purpose = Purpose::undefined);
+    pcl::search::Search<PointT> * autoSelectMethod(const typename pcl::PointCloud<PointT>::ConstPtr& cloud, bool sorted_results, Purpose purpose = Purpose::undefined)
+    {
+      return autoSelectMethod<PointT>(cloud, pcl::IndicesConstPtr(), sorted_results, purpose);
+    }
+
+    /**
+      * Automatically select the fastest search method for the given point cloud. Make sure to delete the returned object after use!
+      * \param[in] cloud Point cloud, this function will pass it to the search method via setInputCloud
+      * \param[in] indices Will be passed to the search method via setInputCloud, together with the point cloud
+      * \param[in] sorted_results Whether the search method should always return results sorted by distance (may be slower than unsorted)
+      * \param[in] purpose Optional, can be used to give more information about what this search method will be used for, to achieve optimal performance
+      */
+    template<typename PointT>
+    pcl::search::Search<PointT> * autoSelectMethod(const typename pcl::PointCloud<PointT>::ConstPtr& cloud, const pcl::IndicesConstPtr& indices, bool sorted_results, Purpose purpose = Purpose::undefined);
   } // namespace search
 } // namespace pcl
 

search/include/pcl/search/impl/auto.hpp

@@ -18,34 +18,34 @@
 #include <pcl/search/organized.h>
 
 template<typename PointT>
-pcl::search::Search<PointT> * pcl::search::autoSelectMethod(const typename pcl::PointCloud<PointT>::ConstPtr& cloud, bool sorted_results, pcl::search::Purpose purpose) {
+pcl::search::Search<PointT> * pcl::search::autoSelectMethod(const typename pcl::PointCloud<PointT>::ConstPtr& cloud, const pcl::IndicesConstPtr& indices, bool sorted_results, pcl::search::Purpose purpose) {
   pcl::search::Search<PointT> * searcher = nullptr;
   if (cloud->isOrganized ()) {
     searcher = new pcl::search::OrganizedNeighbor<PointT> (sorted_results);
-    if(searcher->setInputCloud (cloud)) { // may return false if OrganizedNeighbor cannot work with the cloud, then use another search method instead
+    if(searcher->setInputCloud (cloud, indices)) { // may return false if OrganizedNeighbor cannot work with the cloud, then use another search method instead
       return searcher;
     }
   }
 #if PCL_HAS_NANOFLANN
   searcher = new pcl::search::KdTreeNanoflann<PointT> (sorted_results, (purpose == pcl::search::Purpose::one_knn_search ? 10 : 20));
-  if(searcher->setInputCloud (cloud)) {
+  if(searcher->setInputCloud (cloud, indices)) {
     return searcher;
   }
 #else
   pcl::utils::ignore(purpose);
 #endif
 #if PCL_HAS_FLANN
   searcher = new pcl::search::KdTree<PointT> (sorted_results);
-  if(searcher->setInputCloud (cloud)) {
+  if(searcher->setInputCloud (cloud, indices)) {
     return searcher;
   }
 #endif
   // If nothing else works, use brute force method
   searcher = new pcl::search::BruteForce<PointT> (sorted_results);
-  searcher->setInputCloud (cloud);
+  searcher->setInputCloud (cloud, indices);
   return searcher;
 }
 
-#define PCL_INSTANTIATE_AutoSelectMethod(T) template PCL_EXPORTS pcl::search::Search<T> * pcl::search::autoSelectMethod<T>(const typename pcl::PointCloud<T>::ConstPtr& cloud, bool sorted_results, pcl::search::Purpose purpose);
+#define PCL_INSTANTIATE_AutoSelectMethod(T) template PCL_EXPORTS pcl::search::Search<T> * pcl::search::autoSelectMethod<T>(const typename pcl::PointCloud<T>::ConstPtr& cloud, const pcl::IndicesConstPtr& indices, bool sorted_results, pcl::search::Purpose purpose);
 
 #endif  //#ifndef PCL_SEARCH_AUTO_IMPL_HPP_

segmentation/include/pcl/segmentation/impl/conditional_euclidean_clustering.hpp

@@ -38,8 +38,7 @@
 #define PCL_SEGMENTATION_IMPL_CONDITIONAL_EUCLIDEAN_CLUSTERING_HPP_
 
 #include <pcl/segmentation/conditional_euclidean_clustering.h>
-#include <pcl/search/organized.h> // for OrganizedNeighbor
-#include <pcl/search/kdtree.h> // for KdTree
+#include <pcl/search/auto.h>
 
 template<typename PointT> void
 pcl::ConditionalEuclideanClustering<PointT>::segment (pcl::IndicesClusters &clusters)
@@ -59,12 +58,12 @@ pcl::ConditionalEuclideanClustering<PointT>::segment (pcl::IndicesClusters &clus
   // Initialize the search class
   if (!searcher_)
   {
-    if (input_->isOrganized ())
-      searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> (false)); // not requiring sorted results is much faster
-    else
-      searcher_.reset (new pcl::search::KdTree<PointT> (false)); // not requiring sorted results is much faster
+    searcher_.reset (pcl::search::autoSelectMethod<PointT>(input_, indices_, false, pcl::search::Purpose::radius_search)); // not requiring sorted results is much faster
+  }
+  else
+  {
+    searcher_->setInputCloud (input_, indices_);
   }
-  searcher_->setInputCloud (input_, indices_);
   // If searcher_ gives sorted results, we can skip the first one because it is the query point itself
   const int nn_start_idx = searcher_->getSortedResults () ? 1 : 0;
 

segmentation/include/pcl/segmentation/impl/extract_clusters.hpp

@@ -39,7 +39,7 @@
 #define PCL_SEGMENTATION_IMPL_EXTRACT_CLUSTERS_H_
 
 #include <pcl/segmentation/extract_clusters.h>
-#include <pcl/search/organized.h> // for OrganizedNeighbor
+#include <pcl/search/auto.h>
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
@@ -234,14 +234,13 @@ pcl::EuclideanClusterExtraction<PointT>::extract (std::vector<PointIndices> &clu
   // Initialize the spatial locator
   if (!tree_)
   {
-    if (input_->isOrganized ())
-      tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
-    else
-      tree_.reset (new pcl::search::KdTree<PointT> (false));
+    tree_.reset (pcl::search::autoSelectMethod<PointT>(input_, indices_, false, pcl::search::Purpose::radius_search));
+  }
+  else
+  {
+    // Send the input dataset to the spatial locator
+    tree_->setInputCloud (input_, indices_);
   }
-
-  // Send the input dataset to the spatial locator
-  tree_->setInputCloud (input_, indices_);
   extractEuclideanClusters (*input_, *indices_, tree_, static_cast<float> (cluster_tolerance_), clusters, min_pts_per_cluster_, max_pts_per_cluster_);
 
   //tree_->setInputCloud (input_);