Move further towards KdTreeNanoflann

examples/keypoints/example_sift_keypoint_estimation.cpp

@@ -67,8 +67,6 @@ main(int, char** argv)
   // Estimate the sift interest points using Intensity values from RGB values
   pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointWithScale> sift;
   pcl::PointCloud<pcl::PointWithScale> result;
-  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB> ());
-  sift.setSearchMethod(tree);
   sift.setScales(min_scale, n_octaves, n_scales_per_octave);
   sift.setMinimumContrast(min_contrast);
   sift.setInputCloud(cloud);

examples/keypoints/example_sift_normal_keypoint_estimation.cpp

@@ -72,10 +72,8 @@ main(int, char** argv)
   // Estimate the normals of the cloud_xyz
   pcl::NormalEstimation<pcl::PointXYZ, pcl::PointNormal> ne;
   pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals (new pcl::PointCloud<pcl::PointNormal>);
-  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n(new pcl::search::KdTree<pcl::PointXYZ>());
 
   ne.setInputCloud(cloud_xyz);
-  ne.setSearchMethod(tree_n);
   ne.setRadiusSearch(0.2);
   ne.compute(*cloud_normals);
 
@@ -90,8 +88,6 @@ main(int, char** argv)
   // Estimate the sift interest points using normals values from xyz as the Intensity variants
   pcl::SIFTKeypoint<pcl::PointNormal, pcl::PointWithScale> sift;
   pcl::PointCloud<pcl::PointWithScale> result;
-  pcl::search::KdTree<pcl::PointNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointNormal> ());
-  sift.setSearchMethod(tree);
   sift.setScales(min_scale, n_octaves, n_scales_per_octave);
   sift.setMinimumContrast(min_contrast);
   sift.setInputCloud(cloud_normals);

examples/keypoints/example_sift_z_keypoint_estimation.cpp

@@ -84,8 +84,6 @@ main(int, char** argv)
   // Estimate the sift interest points using z values from xyz as the Intensity variants
   pcl::SIFTKeypoint<pcl::PointXYZ, pcl::PointWithScale> sift;
   pcl::PointCloud<pcl::PointWithScale> result;
-  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ> ());
-  sift.setSearchMethod(tree);
   sift.setScales(min_scale, n_octaves, n_scales_per_octave);
   sift.setMinimumContrast(min_contrast);
   sift.setInputCloud(cloud_xyz);

examples/segmentation/example_extract_clusters_normals.cpp

@@ -43,6 +43,8 @@
 #include <pcl/io/pcd_io.h>
 #include <pcl/features/normal_3d.h>
 #include <pcl/segmentation/extract_clusters.h>
+#define PCL_NO_PRECOMPILE // ConditionalEuclideanClustering is not always instantiated with PointNormal
+#include <pcl/segmentation/conditional_euclidean_clustering.h>
 #include <pcl/point_types.h>
 
 int 
@@ -68,8 +70,6 @@ main (int argc, char **argv)
   pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
   ne.setInputCloud (cloud_ptr);
 
-  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n (new pcl::search::KdTree<pcl::PointXYZ>());
-  ne.setSearchMethod (tree_n);
   ne.setRadiusSearch (0.03);
   ne.compute (*cloud_normals);
   std::cout << "Estimated the normals" << std::endl;
@@ -106,6 +106,40 @@ main (int argc, char **argv)
     writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); 
     ++j;
   }
+
+  // Alternative: using ConditionalEuclideanClustering instead of extractEuclideanClusters
+  auto cloud_with_normals = pcl::make_shared<pcl::PointCloud<pcl::PointNormal>>();
+  pcl::concatenateFields(*cloud_ptr, *cloud_normals, *cloud_with_normals);
+  pcl::ConditionalEuclideanClustering<pcl::PointNormal> cec;
+  const double cos_eps_angle = std::cos(eps_angle);
+  std::function<bool (const pcl::PointNormal&, const pcl::PointNormal&, float)> condition_function =
+    [cos_eps_angle](const pcl::PointNormal& a, const pcl::PointNormal& b, float /*dist*/)
+      { return std::abs(a.getNormalVector3fMap().dot(b.getNormalVector3fMap())) > cos_eps_angle; };
+  cec.setConditionFunction(condition_function);
+  cec.setClusterTolerance(tolerance);
+  cec.setMinClusterSize(min_cluster_size);
+  cec.setInputCloud(cloud_with_normals);
+  std::vector<pcl::PointIndices> cluster_indices2;
+  cec.segment(cluster_indices2);
+
+  std::cout << "No of clusters formed are " << cluster_indices2.size () << std::endl;
+  // Saving the clusters in separate pcd files
+  j = 0;
+  for (const auto& cluster : cluster_indices2)
+  {
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
+    for (const auto &index : cluster.indices) {
+      cloud_cluster->push_back((*cloud_ptr)[index]);
+    }
+    cloud_cluster->width = cloud_cluster->size ();
+    cloud_cluster->height = 1;
+    cloud_cluster->is_dense = true;
 
+    std::cout << "PointCloud representing the Cluster using xyzn: " << cloud_cluster->size () << " data " << std::endl;
+    std::stringstream ss;
+    ss << "./cloud_cluster2_" << j << ".pcd";
+    writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); 
+    ++j;
+  }
   return (0);
 }

features/include/pcl/features/impl/cvfh.hpp

@@ -44,7 +44,7 @@
 #include <pcl/features/cvfh.h>
 #include <pcl/features/normal_3d.h>
 #include <pcl/common/centroid.h>
-#include <pcl/search/kdtree.h> // for KdTree
+#include <pcl/search/auto.h> // for autoSelectMethod
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointInT, typename PointNT, typename PointOutT> void
@@ -234,18 +234,12 @@ pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut
   if(normals_filtered_cloud->size() >= min_points_)
   {
     //recompute normals and use them for clustering
-    KdTreePtr normals_tree_filtered (new pcl::search::KdTree<pcl::PointNormal> (false));
-    normals_tree_filtered->setInputCloud (normals_filtered_cloud);
-
-
     pcl::NormalEstimation<PointNormal, PointNormal> n3d;
     n3d.setRadiusSearch (radius_normals_);
-    n3d.setSearchMethod (normals_tree_filtered);
     n3d.setInputCloud (normals_filtered_cloud);
     n3d.compute (*normals_filtered_cloud);
 
-    KdTreePtr normals_tree (new pcl::search::KdTree<pcl::PointNormal> (false));
-    normals_tree->setInputCloud (normals_filtered_cloud);
+    KdTreePtr normals_tree (pcl::search::autoSelectMethod<pcl::PointNormal>(normals_filtered_cloud, false, pcl::search::Purpose::radius_search));
 
     extractEuclideanClustersSmooth (*normals_filtered_cloud,
                                     *normals_filtered_cloud,

features/include/pcl/features/impl/flare.hpp

@@ -41,8 +41,7 @@
 
 #include <pcl/features/flare.h>
 #include <pcl/common/geometry.h>
-#include <pcl/search/kdtree.h> // for KdTree
-#include <pcl/search/organized.h> // for OrganizedNeighbor
+#include <pcl/search/auto.h> // for autoSelectMethod
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointInT, typename PointNT, typename PointOutT, typename SignedDistanceT> bool
@@ -76,10 +75,7 @@ template<typename PointInT, typename PointNT, typename PointOutT, typename Signe
   // Check if a space search locator was given for sampled_surface_
   if (!sampled_tree_)
   {
-    if (sampled_surface_->isOrganized () && surface_->isOrganized () && input_->isOrganized ())
-      sampled_tree_.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
-    else
-      sampled_tree_.reset (new pcl::search::KdTree<PointInT> (false));
+    sampled_tree_.reset (pcl::search::autoSelectMethod<PointInT>(sampled_surface_, false, pcl::search::Purpose::radius_search));
   }
 
   if (sampled_tree_->getInputCloud () != sampled_surface_) // Make sure the tree searches the sampled surface

features/include/pcl/features/impl/our_cvfh.hpp

@@ -47,7 +47,7 @@
 #include <pcl/common/io.h> // for copyPointCloud
 #include <pcl/common/common.h> // for getMaxDistance
 #include <pcl/common/transforms.h>
-#include <pcl/search/kdtree.h> // for KdTree
+#include <pcl/search/auto.h> // for autoSelectMethod
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointInT, typename PointNT, typename PointOutT> void
@@ -603,17 +603,13 @@ pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloud
   {
     //recompute normals and use them for clustering
     {
-      KdTreePtr normals_tree_filtered (new pcl::search::KdTree<pcl::PointNormal> (false));
-      normals_tree_filtered->setInputCloud (normals_filtered_cloud);
       pcl::NormalEstimation<PointNormal, PointNormal> n3d;
       n3d.setRadiusSearch (radius_normals_);
-      n3d.setSearchMethod (normals_tree_filtered);
       n3d.setInputCloud (normals_filtered_cloud);
       n3d.compute (*normals_filtered_cloud);
     }
 
-    KdTreePtr normals_tree (new pcl::search::KdTree<pcl::PointNormal> (false));
-    normals_tree->setInputCloud (normals_filtered_cloud);
+    KdTreePtr normals_tree (pcl::search::autoSelectMethod<pcl::PointNormal>(normals_filtered_cloud, false, pcl::search::Purpose::radius_search));
 
     extractEuclideanClustersSmooth (*normals_filtered_cloud, *normals_filtered_cloud, cluster_tolerance_, normals_tree, clusters,
                                     eps_angle_threshold_, static_cast<unsigned int> (min_points_));

gpu/examples/segmentation/src/seg.cpp

@@ -4,7 +4,6 @@
 #include <pcl/filters/extract_indices.h>
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/features/normal_3d.h>
-#include <pcl/kdtree/kdtree.h>
 #include <pcl/sample_consensus/method_types.h>
 #include <pcl/sample_consensus/model_types.h>
 #include <pcl/segmentation/sac_segmentation.h>

gpu/people/tools/people_tracking.cpp

@@ -9,7 +9,6 @@
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
 #include <pcl/io/pcd_io.h>
-#include <pcl/kdtree/kdtree.h>
 #include <pcl/segmentation/extract_clusters.h>
 #include <pcl/segmentation/extract_labeled_clusters.h>
 #include <pcl/segmentation/seeded_hue_segmentation.h>

keypoints/include/pcl/keypoints/impl/sift_keypoint.hpp

@@ -41,7 +41,7 @@
 #include <pcl/keypoints/sift_keypoint.h>
 #include <pcl/common/io.h>
 #include <pcl/filters/voxel_grid.h>
-#include <pcl/search/kdtree.h>
+#include <pcl/search/auto.h> // for autoSelectMethod
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointInT, typename PointOutT> void 
@@ -90,7 +90,6 @@ pcl::SIFTKeypoint<PointInT, PointOutT>::initCompute ()
   }
 
   this->setKSearch (1);
-  tree_.reset (new pcl::search::KdTree<PointInT> (true));
   return (true);
 }
 
@@ -134,7 +133,7 @@ pcl::SIFTKeypoint<PointInT, PointOutT>::detectKeypoints (PointCloudOut &output)
       break;
 
     // Update the KdTree with the downsampled points
-    tree_->setInputCloud (cloud);
+    tree_.reset (pcl::search::autoSelectMethod<PointInT>(cloud, true));
 
     // Detect keypoints for the current scale
     detectKeypointsForOctave (*cloud, *tree_, scale, nr_scales_per_octave_, output);

recognition/include/pcl/recognition/impl/hv/hv_go.hpp

@@ -208,11 +208,7 @@ void pcl::GlobalHypothesesVerification<ModelT, SceneT>::initialize()
   NormalEstimator_ n3d;
   scene_normals_.reset (new pcl::PointCloud<pcl::Normal> ());
 
-  typename pcl::search::KdTree<SceneT>::Ptr normals_tree (new pcl::search::KdTree<SceneT>);
-  normals_tree->setInputCloud (scene_cloud_downsampled_);
-
   n3d.setRadiusSearch (radius_normals_);
-  n3d.setSearchMethod (normals_tree);
   n3d.setInputCloud (scene_cloud_downsampled_);
   n3d.compute (*scene_normals_);
 
@@ -535,12 +531,9 @@ bool pcl::GlobalHypothesesVerification<ModelT, SceneT>::addModel(typename pcl::P
   }
 
   //compute normals unless given (now do it always...)
-  typename pcl::search::KdTree<ModelT>::Ptr normals_tree (new pcl::search::KdTree<ModelT>);
   pcl::NormalEstimation<ModelT, pcl::Normal> n3d;
   recog_model->normals_.reset (new pcl::PointCloud<pcl::Normal> ());
-  normals_tree->setInputCloud (recog_model->cloud_);
   n3d.setRadiusSearch (radius_normals_);
-  n3d.setSearchMethod (normals_tree);
   n3d.setInputCloud ((recog_model->cloud_));
   n3d.compute (*(recog_model->normals_));
 

recognition/include/pcl/recognition/impl/implicit_shape_model.hpp

@@ -44,7 +44,7 @@
 #include "../implicit_shape_model.h"
 #include <pcl/filters/voxel_grid.h> // for VoxelGrid
 #include <pcl/filters/extract_indices.h> // for ExtractIndices
-#include <pcl/search/kdtree.h> // for KdTree
+#include <pcl/search/auto.h> // for autoSelectMethod
 
 #include <pcl/memory.h>  // for dynamic_pointer_cast
 
@@ -222,9 +222,7 @@ pcl::features::ISMVoteList<PointT>::validateTree ()
 {
   if (!tree_is_valid_)
   {
-    if (tree_ == nullptr)
-      tree_.reset (new pcl::KdTreeFLANN<pcl::InterestPoint>);
-    tree_->setInputCloud (votes_);
+    tree_.reset (pcl::search::autoSelectMethod<pcl::InterestPoint>(votes_, false, pcl::search::Purpose::radius_search));
     k_ind_.resize ( votes_->size (), -1 );
     k_sqr_dist_.resize ( votes_->size (), 0.0f );
     tree_is_valid_ = true;
@@ -1206,12 +1204,8 @@ pcl::ism::ImplicitShapeModelEstimation<FeatureSize, PointT, NormalT>::estimateFe
   typename pcl::PointCloud<NormalT>::Ptr normal_cloud,
   typename pcl::PointCloud<pcl::Histogram<FeatureSize> >::Ptr feature_cloud)
 {
-  typename pcl::search::Search<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
-//  tree->setInputCloud (point_cloud);
-
   feature_estimator_->setInputCloud (sampled_point_cloud->makeShared ());
 //  feature_estimator_->setSearchSurface (point_cloud->makeShared ());
-  feature_estimator_->setSearchMethod (tree);
 
 //  typename pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> >::Ptr feat_est_norm =
 //    dynamic_pointer_cast<pcl::SpinImageEstimation<pcl::PointXYZ, pcl::Normal, pcl::Histogram<FeatureSize> > > (feature_estimator_);

recognition/include/pcl/recognition/implicit_shape_model.h

@@ -43,7 +43,6 @@
 #include <pcl/point_types.h>
 #include <pcl/point_representation.h>
 #include <pcl/features/feature.h>
-#include <pcl/kdtree/kdtree_flann.h> // for KdTreeFLANN
 
 namespace pcl
 {
@@ -140,7 +139,7 @@ namespace pcl
         std::vector<int> votes_class_{};
 
         /** \brief Stores the search tree. */
-        pcl::KdTreeFLANN<pcl::InterestPoint>::Ptr tree_{nullptr};
+        pcl::search::Search<pcl::InterestPoint>::Ptr tree_{nullptr};
 
         /** \brief Stores neighbours indices. */
         pcl::Indices k_ind_{};

registration/include/pcl/registration/bfgs.h

@@ -7,6 +7,8 @@
 
 #include <unsupported/Eigen/Polynomials> // for PolynomialSolver, PolynomialSolverBase
 
+#include <cassert> // for assert
+
 namespace Eigen {
 template <typename _Scalar>
 class PolynomialSolver<_Scalar, 2> : public PolynomialSolverBase<_Scalar, 2> {

registration/include/pcl/registration/correspondence_estimation_backprojection.h

@@ -78,9 +78,6 @@ class CorrespondenceEstimationBackProjection
       point_representation_;
   using CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::target_indices_;
 
-  using KdTree = pcl::search::KdTree<PointTarget>;
-  using KdTreePtr = typename KdTree::Ptr;
-
   using PointCloudSource = pcl::PointCloud<PointSource>;
   using PointCloudSourcePtr = typename PointCloudSource::Ptr;
   using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;

registration/include/pcl/registration/gicp6d.h

@@ -38,8 +38,8 @@
 
 #pragma once
 
-#include <pcl/kdtree/impl/kdtree_flann.hpp>
 #include <pcl/registration/gicp.h>
+#include <pcl/search/kdtree.h>
 #include <pcl/memory.h>
 #include <pcl/pcl_exports.h> // for PCL_EXPORTS
 #include <pcl/point_cloud.h>
@@ -118,7 +118,7 @@ class PCL_EXPORTS GeneralizedIterativeClosestPoint6D
   pcl::PointCloud<PointXYZLAB>::Ptr target_lab_;
 
   /** \brief 6d-tree to search in model cloud. */
-  KdTreeFLANN<PointXYZLAB> target_tree_lab_;
+  pcl::search::KdTree<PointXYZLAB>::Ptr target_tree_lab_;
 
   /** \brief The color weight. */
   float lab_weight_;

registration/include/pcl/registration/transformation_validation_euclidean.h

@@ -40,7 +40,6 @@
 
 #pragma once
 
-#include <pcl/kdtree/kdtree.h>
 #include <pcl/registration/transformation_validation.h>
 #include <pcl/search/kdtree.h>
 #include <pcl/memory.h>

registration/src/gicp6d.cpp

@@ -37,6 +37,7 @@
  */
 
 #include <pcl/registration/gicp6d.h>
+#include <pcl/search/kdtree_nanoflann.h>
 #include <pcl/memory.h>                 // for pcl::make_shared
 #include <pcl/point_types_conversion.h> // for PointXYZRGBtoXYZLAB
 
@@ -80,17 +81,32 @@ GeneralizedIterativeClosestPoint6D::setInputTarget(
   }
 
   // ...and build 6d-tree
-  target_tree_lab_.setInputCloud(target_lab_);
-  target_tree_lab_.setPointRepresentation(
+#if PCL_HAS_NANOFLANN
+  // 6 search dimensions for MyPointRepresentation; max. 10 points per leaf is a good
+  // choice for one-nearest-neighbor search
+  target_tree_lab_.reset(
+      new pcl::search::KdTreeNanoflann<pcl::PointXYZLAB, 6>(false, 10));
+  target_tree_lab_->setPointRepresentation(
       pcl::make_shared<MyPointRepresentation>(point_rep_));
+  target_tree_lab_->setInputCloud(target_lab_);
+#else
+#if PCL_HAS_FLANN
+  target_tree_lab_.reset(new pcl::search::KdTree<pcl::PointXYZLAB>(false));
+  target_tree_lab_->setPointRepresentation(
+      pcl::make_shared<MyPointRepresentation>(point_rep_));
+  target_tree_lab_->setInputCloud(target_lab_);
+#else
+#error "GeneralizedIterativeClosestPoint6D needs nanoflann or FLANN!"
+#endif
+#endif
 }
 
 bool
 GeneralizedIterativeClosestPoint6D::searchForNeighbors(const PointXYZLAB& query,
                                                        pcl::Indices& index,
                                                        std::vector<float>& distance)
 {
-  int k = target_tree_lab_.nearestKSearch(query, 1, index, distance);
+  int k = target_tree_lab_->nearestKSearch(query, 1, index, distance);
 
   // check if neighbor was found
   return (k != 0);

segmentation/include/pcl/segmentation/impl/cpc_segmentation.hpp

@@ -176,12 +176,9 @@ pcl::CPCSegmentation<PointT>::applyCuttingPlane (std::uint32_t depth_levels_left
       // We also just actually cut when the edge goes through the plane. This is why we check the planedistance
       std::vector<pcl::PointIndices> cluster_indices;
       pcl::EuclideanClusterExtraction<WeightSACPointType> euclidean_clusterer;
-      pcl::search::KdTree<WeightSACPointType>::Ptr tree (new pcl::search::KdTree<WeightSACPointType>);
-      tree->setInputCloud (edge_cloud_cluster);
       euclidean_clusterer.setClusterTolerance (seed_resolution_);
       euclidean_clusterer.setMinClusterSize (1);
       euclidean_clusterer.setMaxClusterSize (25000);
-      euclidean_clusterer.setSearchMethod (tree);
       euclidean_clusterer.setInputCloud (edge_cloud_cluster);
       euclidean_clusterer.setIndices (support_indices);
       euclidean_clusterer.extract (cluster_indices);