Point Cloud Library (PCL)  1.15.1-dev
kdtree_nanoflann.h
1 /*
2  * SPDX-License-Identifier: BSD-3-Clause
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2025-, Open Perception Inc.
6  *
7  * All rights reserved
8  */
9 
10 #pragma once
11 
12 #include <pcl/pcl_config.h>
13 #if PCL_HAS_NANOFLANN || defined(DOXYGEN_ONLY)
14 
15 #include <pcl/search/kdtree.h>
16 #include <pcl/point_representation.h>
17 
18 #include <nanoflann.hpp>
19 
20 namespace pcl {
21 namespace search {
22 namespace internal {
23 /** Helper for KdTreeNanoflann, serves as a dataset adaptor.
24  */
25 template <typename T = float>
27  PointCloudAdaptor(const T* data,
28  bool delete_data,
29  std::size_t dim,
30  std::size_t point_count)
31  : data_(data), delete_data_(delete_data), dim_(dim), point_count_(point_count)
32  {}
33 
35  {
36  if (delete_data_)
37  delete[] data_;
38  }
39 
40  inline void
41  reset_adaptor(const T* data,
42  bool delete_data,
43  std::size_t dim,
44  std::size_t point_count)
45  {
46  if (delete_data_)
47  delete[] data_;
48  data_ = data;
49  delete_data_ = delete_data;
50  dim_ = dim;
51  point_count_ = point_count;
52  }
53 
54  inline std::size_t
56  {
57  return point_count_;
58  }
59 
60  inline T
61  kdtree_get_pt(const std::size_t idx, const std::size_t dim) const
62  {
63  return data_[dim_ * idx + dim];
64  }
65 
66  template <class BBOX>
67  bool
68  kdtree_get_bbox(BBOX& /* bb */) const
69  {
70  return false;
71  }
72 
73 private:
74  const T* data_;
75  bool delete_data_;
76  std::size_t dim_;
77  std::size_t point_count_;
78 };
79 
80 /** Helper function for radiusSearch: must have a template specialization if the
81  * distance norm is L2
82  */
83 template <typename Dist>
84 float
85 square_if_l2(float radius)
86 {
87  return radius;
88 };
89 template <>
90 float
91 square_if_l2<nanoflann::L2_Adaptor<float,
93  float,
94  pcl::index_t>>(float radius)
95 {
96  return radius * radius;
97 };
98 template <>
99 float
101  nanoflann::L2_Simple_Adaptor<float,
103  float,
104  pcl::index_t>>(float radius)
105 {
106  return radius * radius;
107 };
108 } // namespace internal
109 
110 // for convenience/brevity
111 using L1_Adaptor =
112  nanoflann::L1_Adaptor<float,
114  float,
115  pcl::index_t>;
116 using L2_Adaptor =
117  nanoflann::L2_Adaptor<float,
119  float,
120  pcl::index_t>;
122  nanoflann::L2_Simple_Adaptor<float,
124  float,
125  pcl::index_t>;
126 using SO2_Adaptor =
127  nanoflann::SO2_Adaptor<float,
129  float,
130  pcl::index_t>;
131 using SO3_Adaptor =
132  nanoflann::SO3_Adaptor<float,
134  float,
135  pcl::index_t>;
136 // TODO maybe additional adaptor with simd?
137 
138 /** @brief @b pcl::search::KdTreeNanoflann is a faster and flexible alternative to
139  * pcl::search::KdTree. It is based on the nanoflann library by Jose Luis Blanco-Claraco
140  * ( https://github.com/jlblancoc/nanoflann ). Since nanoflann is treated as an optional
141  * dependency to PCL, you must test the preprocessor symbol `PCL_HAS_NANOFLANN` after
142  * including `pcl/search/kdtree_nanoflann.h`.
143  * @code
144  * // Example 1: using KdTreeNanoflann in NormalEstimation:
145  * pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
146  * auto tree = pcl::make_shared<pcl::search::KdTreeNanoflann<pcl::PointXYZ>>();
147  * ne.setSearchMethod (tree);
148  * // rest as usual
149  * // Example 2: using KdTreeNanoflannn in ICP:
150  * pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
151  * icp.setSearchMethodSource(pcl::make_shared<pcl::search::KdTreeNanoflann<pcl::PointXYZ>>());
152  * icp.setSearchMethodTarget(pcl::make_shared<pcl::search::KdTreeNanoflann<pcl::PointXYZ>>());
153  * // rest as usual
154  * // Example 3: using a L1 distance norm:
155  * pcl::search::KdTreeNanoflann<PointXYZ, 3, pcl::search::L1_Adaptor> kdtree_nanoflann;
156  * kdtree_nanoflann.setInputCloud (my_cloud);
157  * // now ready to use kdtree_nanoflann.nearestKSearch(...) and
158  * // kdtree_nanoflann.radiusSearch(...);
159  * // Example 4: using KdTreeNanoflann with features instead of 3D points:
160  * pcl::search::KdTreeNanoflann<pcl::FPFHSignature33, 33> kdtree_nanoflann_feat;
161  * kdtree_nanoflann_feat.setInputCloud(my_features_cloud);
162  * // now ready for searching
163  * @endcode
164  *
165  * @tparam PointT Can be a point like pcl::PointXYZ or a feature like pcl::SHOT352
166  * @tparam Dim Dimension of the KdTree to build. If set to -1, it will infer the
167  * dimension at runtime from the given point representation, but with a performance
168  * penalty. Default is 3, appropriate for all xyz point types.
169  * @tparam Distance The distance to use. Possible values include pcl::L1_Adaptor
170  * (taxicab/manhattan distance), pcl::L2_Simple_Adaptor (euclidean distance, optimized
171  * for few dimensions), pcl::L2_Adaptor (euclidean distance, optimized for many
172  * dimensions), pcl::SO2_Adaptor, pcl::SO3_Adaptor. Default is pcl::L2_Simple_Adaptor
173  *
174  * @author Markus Vieth
175  * @ingroup search
176  */
177 template <typename PointT,
178  std::int32_t Dim = 3,
179  typename Distance =
180  std::conditional_t<Dim >= 4, L2_Adaptor, L2_Simple_Adaptor>,
181  typename Tree = nanoflann::KDTreeSingleIndexAdaptor<
182  Distance,
184  Dim,
185  pcl::index_t>>
186 class KdTreeNanoflann : public pcl::search::KdTree<PointT> {
187 private:
188  /** The special thing here is that indices and distances are stored in _two_ vectors,
189  * not as a pair in _one_ vector.
190  */
191  template <typename _DistanceType = float, typename _IndexType = pcl::index_t>
192  class PCLRadiusResultSet {
193  public:
194  using DistanceType = _DistanceType;
195  using IndexType = _IndexType;
196 
197  public:
198  const DistanceType radius;
199 
200  std::vector<IndexType>& m_indices;
201  std::vector<DistanceType>& m_dists;
202  std::size_t max_results_;
203 
204  explicit PCLRadiusResultSet(
205  DistanceType radius_,
206  std::vector<IndexType>& indices,
207  std::vector<DistanceType>& dists,
208  std::size_t max_results = std::numeric_limits<std::size_t>::max())
209  : radius(radius_), m_indices(indices), m_dists(dists), max_results_(max_results)
210  {
211  init();
212  }
213 
214  void
215  init()
216  {
217  clear();
218  if (max_results_ != std::numeric_limits<std::size_t>::max()) {
219  m_indices.reserve(max_results_);
220  m_dists.reserve(max_results_);
221  }
222  }
223  void
224  clear()
225  {
226  m_indices.clear();
227  m_dists.clear();
228  }
229 
230  size_t
231  size() const
232  {
233  return m_indices.size();
234  }
235  size_t
236  empty() const
237  {
238  return m_indices.empty();
239  }
240 
241  bool
242  full() const
243  {
244  return true;
245  }
246 
247  /**
248  * Called during search to add an element matching the criteria.
249  * @return true if the search should be continued, false if the results are
250  * sufficient
251  */
252  bool
253  addPoint(DistanceType dist, IndexType index)
254  {
255  if (dist < radius) {
256  m_indices.emplace_back(index);
257  m_dists.emplace_back(dist);
258  }
259  return (m_indices.size() < max_results_);
260  }
261 
262  DistanceType
263  worstDist() const
264  {
265  return radius;
266  }
267 
268  void
269  sort()
270  {
271  // this sort function does not do anything (yet). For now, PCLRadiusResultSet
272  // should only be used if the results do not have to be sorted. In the future, we
273  // might add sorting here if we find a really efficient way to sort both vectors
274  // simultaneously.
275  }
276  };
277 
278 public:
279  using PointCloud = typename Search<PointT>::PointCloud;
280  using PointCloudConstPtr = typename pcl::PointCloud<PointT>::ConstPtr;
281 
290 
291  using Ptr = shared_ptr<KdTreeNanoflann<PointT, Dim, Distance, Tree>>;
292  using ConstPtr = shared_ptr<const KdTreeNanoflann<PointT, Dim, Distance, Tree>>;
293 
294  using KdTreePtr = shared_ptr<Tree>;
295  using KdTreeConstPtr = shared_ptr<const Tree>;
296  using PointRepresentationConstPtr = shared_ptr<const PointRepresentation<PointT>>;
297 
298  /** @brief Constructor for KdTreeNanoflann.
299  *
300  * @param[in] sorted Set to true if the nearest neighbor search results
301  * need to be sorted in ascending order based on their distance to the
302  * query point (default is false, which is faster)
303  * @param[in] leaf_max_size Max number of points/samples in a leaf, can be tuned to
304  * achieve best performance. When using this for 1nn search (e.g. correspondence
305  * estimation in icp/registration), then leaf_max_size=10 is usually fastest.
306  * Otherwise, leaf_max_size=20 is a good choice.
307  * @param[in] n_thread_build Number of threads for concurrent tree build (default is
308  * 1, meaning no concurrent build)
309  */
310  KdTreeNanoflann(bool sorted,
311  std::size_t leaf_max_size,
312  unsigned int n_thread_build = 1)
313  : pcl::search::KdTree<PointT>("KdTreeNanoflann", sorted)
314  , leaf_max_size_(leaf_max_size)
315  , n_thread_build_(n_thread_build)
316  {}
317 
318  /** @brief Constructor for KdTreeNanoflann.
319  *
320  * @param[in] sorted set to true if the nearest neighbor search results
321  * need to be sorted in ascending order based on their distance to the
322  * query point
323  *
324  */
325  KdTreeNanoflann(bool sorted = false) : KdTreeNanoflann(sorted, 15, 1) {}
326 
327  /** @brief Destructor for KdTreeNanoflann. */
328  ~KdTreeNanoflann() override = default;
329 
330  /** @brief Provide a pointer to the point representation to use to convert points into
331  * k-D vectors. If you want to use this function, it is recommended to do so _before_
332  * calling setInputCloud, to avoid setting up the kd-tree twice.
333  * @param[in] point_representation the const shared pointer to a PointRepresentation
334  */
335  void
336  setPointRepresentation(const PointRepresentationConstPtr& point_representation)
337  {
338  PCL_DEBUG("[KdTreeNanoflann::setPointRepresentation] "
339  "KdTreeNanoflann::setPointRepresentation called, "
340  "point_representation->getNumberOfDimensions()=%i, "
341  "point_representation->isTrivial()=%i\n",
342  point_representation->getNumberOfDimensions(),
343  point_representation->isTrivial());
344  if (Dim != -1 && Dim != point_representation->getNumberOfDimensions()) {
345  PCL_ERROR("[KdTreeNanoflann::setPointRepresentation] The given point "
346  "representation uses %i dimensions, but the nr of dimensions given as "
347  "template parameter to KdTreeNanoflann is %i.\n",
348  point_representation->getNumberOfDimensions(),
349  Dim);
350  return;
351  }
352  point_representation_ = point_representation;
353  setUpTree();
354  }
355 
356  /** @brief Get a pointer to the point representation used when converting points into
357  * k-D vectors. */
358  inline PointRepresentationConstPtr
359  getPointRepresentation() const
360  {
361  return point_representation_;
362  }
363 
364  /** @brief Sets whether the results have to be sorted or not.
365  * @param[in] sorted_results set to true if the radius search results should be sorted
366  */
367  void
368  setSortedResults(bool sorted_results) override
369  {
370  sorted_results_ = sorted_results;
371  }
372 
373  /** @brief Set the search epsilon precision (error bound) for nearest neighbors
374  * searches.
375  * @param[in] eps precision (error bound) for nearest neighbors searches
376  */
377  void
378  setEpsilon(float eps)
379  {
380  eps_ = eps;
381  }
382 
383  /** @brief Get the search epsilon precision (error bound) for nearest neighbors
384  * searches. */
385  inline float
386  getEpsilon() const
387  {
388  return eps_;
389  }
390 
391  /** @brief Influences the results of radiusSearch when max_nn is not set to zero.
392  * If the parameter max_nn of radiusSearch is set to a value greater than zero, it
393  * will return _at most_ that many points. If you set `use_rknn=true`, it will always
394  * return the points _closest_ to the query point. If you set `use_rknn=false`, it may
395  * return _any_ points within the radius, which can be faster.
396  */
397  void
398  setUseRKNN(bool use_rknn)
399  {
400  use_rknn_ = use_rknn;
401  }
402 
403  /** @brief Provide a pointer to the input dataset, this will build the kd-tree.
404  * @param[in] cloud the const shared pointer to a PointCloud
405  * @param[in] indices the point indices subset that is to be used from \a cloud
406  */
407  bool
408  setInputCloud(const PointCloudConstPtr& cloud,
409  const IndicesConstPtr& indices = IndicesConstPtr()) override
410  {
411  input_ = cloud;
412  indices_ = indices;
413 
414  return setUpTree();
415  }
416 
417  /** @brief Get pointer to internal nanoflann tree. Use with caution.
418  */
419  KdTreePtr
420  getNanoflannTree()
421  {
422  return nanoflann_tree_;
423  }
424 
425  /** @brief Search for the k-nearest neighbors for the given query point.
426  * @param[in] point the given query point
427  * @param[in] k the number of neighbors to search for
428  * @param[out] k_indices the resultant indices of the neighboring points (must be
429  * resized to \a k a priori!)
430  * @param[out] k_sqr_distances the resultant squared distances to the neighboring
431  * points (must be resized to \a k a priori!)
432  * @return number of neighbors found
433  */
434  int
435  nearestKSearch(const PointT& point,
436  int k,
437  Indices& k_indices,
438  std::vector<float>& k_sqr_distances) const override
439  {
440  assert(point_representation_->isValid(point) &&
441  "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
442  if (static_cast<std::size_t>(k) > adaptor_->kdtree_get_point_count())
443  k = adaptor_->kdtree_get_point_count();
444  k_indices.resize(k);
445  k_sqr_distances.resize(k);
446  float* query_point = nullptr;
447  if (!point_representation_->isTrivial()) {
448  query_point = new float[point_representation_->getNumberOfDimensions()];
449  point_representation_->vectorize(point, query_point);
450  }
451  // like nanoflann_tree_->knnSearch
452  nanoflann::KNNResultSet<float, pcl::index_t> resultSet(k);
453  resultSet.init(k_indices.data(), k_sqr_distances.data());
454  nanoflann_tree_->findNeighbors(
455  resultSet,
456  (query_point ? query_point : reinterpret_cast<const float*>(&point))
457 #if NANOFLANN_VERSION < 0x150
458  ,
459  nanoflann::SearchParams()
460 #endif // NANOFLANN_VERSION < 0x150
461  );
462  const auto search_result = resultSet.size();
463  delete[] query_point;
464  assert(search_result == k);
465 
466  if (!identity_mapping_) {
467  for (auto& index : k_indices) {
468  index = index_mapping_[index];
469  }
470  }
471  return search_result;
472  }
473 
474  /** @brief Search for all the nearest neighbors of the query point in a given radius.
475  * @param[in] point the given query point
476  * @param[in] radius the radius of the sphere bounding all of p_q's neighbors
477  * @param[out] k_indices the resultant indices of the neighboring points
478  * @param[out] k_sqr_distances the resultant squared distances to the neighboring
479  * points
480  * @param[in] max_nn if given, bounds the maximum returned neighbors to this value. If
481  * \a max_nn is set to 0 or to a number higher than the number of points in the input
482  * cloud, all neighbors in \a radius will be returned.
483  * @return number of neighbors found in radius
484  */
485  int
486  radiusSearch(const PointT& point,
487  double radius,
488  Indices& k_indices,
489  std::vector<float>& k_sqr_distances,
490  unsigned int max_nn = 0) const override
491  {
492  assert(point_representation_->isValid(point) &&
493  "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
494  float* query_point = nullptr;
495  if (!point_representation_->isTrivial()) {
496  query_point = new float[point_representation_->getNumberOfDimensions()];
497  point_representation_->vectorize(point, query_point);
498  }
499  if (max_nn == 0) {
500  // return _all_ points within radius
501  // Calling this->sortResults is currently slower than sorting the vector of
502  // nanoflann::ResultItem However, if sorted results are not requested, using
503  // PCLRadiusResultSet with radiusSearchCustomCallback avoids copies and is faster
504  if (sorted_results_) {
505 #if NANOFLANN_VERSION < 0x150
506  std::vector<std::pair<pcl::index_t, float>>
507  IndicesDists; // nanoflann::ResultItem was introduced in version 1.5.0
508 #else
509  std::vector<nanoflann::ResultItem<pcl::index_t, float>> IndicesDists;
510 #endif // NANOFLANN_VERSION < 0x150
511  // like nanoflann_tree_->radiusSearch
512  nanoflann::RadiusResultSet<float, pcl::index_t> resultSet(
513  pcl::search::internal::square_if_l2<Distance>(radius), IndicesDists);
514  nanoflann_tree_->findNeighbors(
515  resultSet,
516  (query_point ? query_point : reinterpret_cast<const float*>(&point)),
517 #if NANOFLANN_VERSION < 0x150
518  {32, eps_, sorted_results_} // first parameter is ignored in older versions,
519  // and removed in newer versions
520 #else
521  {eps_, sorted_results_}
522 #endif // NANOFLANN_VERSION < 0x150
523  );
524 #if NANOFLANN_VERSION < 0x160
525  // before, nanoflann did not sort in findNeighbours
526  if (sorted_results_)
527  std::sort(
528  IndicesDists.begin(), IndicesDists.end(), nanoflann::IndexDist_Sorter());
529 #endif // NANOFLANN_VERSION < 0x160
530  const auto search_result = resultSet.size();
531  k_indices.resize(IndicesDists.size());
532  k_sqr_distances.resize(IndicesDists.size());
533  for (std::size_t i = 0; i < IndicesDists.size(); ++i) {
534  k_indices[i] = identity_mapping_ ? IndicesDists[i].first
535  : index_mapping_[IndicesDists[i].first];
536  k_sqr_distances[i] = IndicesDists[i].second;
537  }
538  delete[] query_point;
539  return search_result;
540  }
541  else {
542  PCLRadiusResultSet<float, pcl::index_t> resultSet(
543  pcl::search::internal::square_if_l2<Distance>(radius),
544  k_indices,
545  k_sqr_distances);
546  // like nanoflann_tree_->radiusSearchCustomCallback
547  nanoflann_tree_->findNeighbors(
548  resultSet,
549  (query_point ? query_point : reinterpret_cast<const float*>(&point)),
550 #if NANOFLANN_VERSION < 0x150
551  {32, eps_, sorted_results_} // first parameter is ignored in older versions,
552  // and removed in newer versions
553 #else
554  {eps_, sorted_results_}
555 #endif // NANOFLANN_VERSION < 0x150
556  );
557  const auto search_result = resultSet.size();
558  if (!identity_mapping_) {
559  for (auto& index : k_indices) {
560  index = index_mapping_[index];
561  }
562  }
563  if (sorted_results_)
564  this->sortResults(k_indices, k_sqr_distances);
565  delete[] query_point;
566  return search_result;
567  }
568  }
569  else {
570  // return no more than max_nn points
571  if (use_rknn_) {
572  // return points closest to query point
573  if (static_cast<std::size_t>(max_nn) > adaptor_->kdtree_get_point_count())
574  max_nn = adaptor_->kdtree_get_point_count();
575  k_indices.resize(max_nn);
576  k_sqr_distances.resize(max_nn);
577 #if NANOFLANN_VERSION < 0x151
578  // rknnSearch and RKNNResultSet were added in nanoflann 1.5.1, so do knn search
579  // and discard those neighbors that are outside of search radius
580  nanoflann::KNNResultSet<float, pcl::index_t> resultSet(max_nn);
581  resultSet.init(k_indices.data(), k_sqr_distances.data());
582  nanoflann_tree_->findNeighbors(
583  resultSet,
584  (query_point ? query_point : reinterpret_cast<const float*>(&point))
585 #if NANOFLANN_VERSION < 0x150
586  ,
587  nanoflann::SearchParams()
588 #endif // NANOFLANN_VERSION < 0x150
589  );
590  auto search_result = resultSet.size();
591  for (auto iter = k_sqr_distances.rbegin();
592  iter != k_sqr_distances.rend() &&
593  *iter > pcl::search::internal::square_if_l2<Distance>(radius);
594  ++iter) {
595  --search_result;
596  }
597  k_indices.resize(search_result);
598  k_sqr_distances.resize(search_result);
599 #else
600  // like nanoflann_tree_->rknnSearch
601  nanoflann::RKNNResultSet<float, pcl::index_t> resultSet(
602  max_nn, pcl::search::internal::square_if_l2<Distance>(radius));
603  resultSet.init(k_indices.data(), k_sqr_distances.data());
604  nanoflann_tree_->findNeighbors(
605  resultSet,
606  (query_point ? query_point : reinterpret_cast<const float*>(&point)));
607  const auto search_result = resultSet.size();
608  k_indices.resize(search_result);
609  k_sqr_distances.resize(search_result);
610 #endif // NANOFLANN_VERSION < 0x151
611  if (!identity_mapping_) {
612  for (auto& index : k_indices) {
613  index = index_mapping_[index];
614  }
615  }
616  delete[] query_point;
617  return search_result;
618  }
619  else {
620  // may return any points within the radius, can be faster
621  PCLRadiusResultSet<float, pcl::index_t> resultSet(
622  pcl::search::internal::square_if_l2<Distance>(radius),
623  k_indices,
624  k_sqr_distances,
625  max_nn);
626  // like nanoflann_tree_->radiusSearchCustomCallback
627  nanoflann_tree_->findNeighbors(
628  resultSet,
629  (query_point ? query_point : reinterpret_cast<const float*>(&point)),
630 #if NANOFLANN_VERSION < 0x150
631  {32, eps_, sorted_results_} // first parameter is ignored in older versions,
632  // and removed in newer versions
633 #else
634  {eps_, sorted_results_}
635 #endif // NANOFLANN_VERSION < 0x150
636  );
637  const auto search_result = resultSet.size();
638  if (!identity_mapping_) {
639  for (auto& index : k_indices) {
640  index = index_mapping_[index];
641  }
642  }
643  if (sorted_results_)
644  this->sortResults(k_indices, k_sqr_distances);
645  delete[] query_point;
646  return search_result;
647  }
648  }
649  }
650 
651 private:
652  /** Set up index_mapping_, adaptor_, and nanoflann_tree_, based on
653  * point_representation_, input_, and indices_.
654  */
655  bool
656  setUpTree()
657  {
658  if (!point_representation_ || !input_ || input_->empty()) {
659  PCL_ERROR("[KdTreeNanoflann::setUpTree] point representation is null or input is "
660  "null or input is empty\n");
661  return false;
662  }
663 
664  const std::size_t dim = point_representation_->getNumberOfDimensions();
665  if (Dim != -1 && Dim != static_cast<int>(dim)) {
666  PCL_ERROR("[KdTreeNanoflann::setUpTree] The given point "
667  "representation uses %i dimensions, but the nr of dimensions given as "
668  "template parameter to KdTreeNanoflann is %i.\n",
669  dim,
670  Dim);
671  return false;
672  }
673  index_mapping_.clear();
674  if (indices_ && !indices_->empty()) {
675  index_mapping_.reserve(indices_->size());
676  for (const auto& index : *indices_) {
677  if (point_representation_->isValid((*input_)[index])) {
678  index_mapping_.emplace_back(index);
679  }
680  }
681  }
682  else {
683  index_mapping_.reserve(input_->size());
684  for (std::size_t index = 0; index < input_->size(); ++index) {
685  if (point_representation_->isValid((*input_)[index])) {
686  index_mapping_.emplace_back(index);
687  }
688  }
689  }
690  identity_mapping_ = true;
691  for (pcl::index_t index = 0;
692  identity_mapping_ && static_cast<std::size_t>(index) < index_mapping_.size();
693  ++index) {
694  identity_mapping_ = identity_mapping_ && index_mapping_[index] == index;
695  }
696 
697  PCL_DEBUG("[KdTreeNanoflann::setUpTree] identity_mapping_=%i, "
698  "point_representation_->getNumberOfDimensions()=%lu, "
699  "point_representation_->isTrivial ()=%i\n",
700  identity_mapping_ ? 1 : 0,
701  dim,
702  point_representation_->isTrivial() ? 1 : 0);
703  if (identity_mapping_ && point_representation_->isTrivial() &&
704  sizeof(PointT) % sizeof(float) == 0) {
705  // no need to allocate memory and copy data
706  PCL_DEBUG("[KdTreeNanoflann::setUpTree] Mapping cloud directly, without "
707  "allocating memory.\n");
708  adaptor_.reset(new internal::PointCloudAdaptor<float>(
709  reinterpret_cast<const float*>(&(*input_)[0]),
710  false,
711  sizeof(PointT) / sizeof(float),
712  index_mapping_.size()));
713  }
714  else {
715  float* data = new float[dim * index_mapping_.size()];
716  float* data_itr = data;
717  for (auto& index : index_mapping_) {
718  point_representation_->vectorize((*input_)[index], data_itr);
719  data_itr += dim;
720  }
721  adaptor_.reset(new internal::PointCloudAdaptor<float>(
722  data, true, dim, index_mapping_.size()));
723  }
724  nanoflann_tree_.reset(
725  new Tree(dim,
726  *adaptor_,
727 #if NANOFLANN_VERSION >= 0x150
728  // concurrent tree building is possible since nanoflann 1.5.0
729  {leaf_max_size_,
730  nanoflann::KDTreeSingleIndexAdaptorFlags::None,
731  n_thread_build_}));
732 #else
733  {leaf_max_size_}));
734  if (n_thread_build_ != 1)
735  PCL_WARN("[KdTreeNanoflann::setUpTree] concurrent tree building is only possible "
736  "with nanoflann version 1.5.0 and newer\n");
737 #endif // NANOFLANN_VERSION >= 0x150
738  return true;
739  }
740 
741  KdTreePtr nanoflann_tree_;
742 
743  shared_ptr<internal::PointCloudAdaptor<float>> adaptor_;
744 
745  PointRepresentationConstPtr point_representation_{
747 
748  Indices index_mapping_;
749 
750  bool identity_mapping_{false};
751 
752  std::size_t leaf_max_size_{15};
753 
754  unsigned int n_thread_build_{1};
755 
756  float eps_{0.0f};
757 
758  bool use_rknn_{true};
759 };
760 } // namespace search
761 } // namespace pcl
762 #ifdef PCL_NO_PRECOMPILE
763 #include <pcl/search/impl/kdtree.hpp>
764 #endif
765 
766 #else
767 //#warning "KdTreeNanoflann is not available"
768 #endif // PCL_HAS_NANOFLANN
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:174
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:415
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
Generic search class.
Definition: search.h:75
float square_if_l2(float radius)
Helper function for radiusSearch: must have a template specialization if the distance norm is L2.
nanoflann::L2_Simple_Adaptor< float, pcl::search::internal::PointCloudAdaptor< float >, float, pcl::index_t > L2_Simple_Adaptor
nanoflann::SO2_Adaptor< float, pcl::search::internal::PointCloudAdaptor< float >, float, pcl::index_t > SO2_Adaptor
nanoflann::SO3_Adaptor< float, pcl::search::internal::PointCloudAdaptor< float >, float, pcl::index_t > SO3_Adaptor
nanoflann::L1_Adaptor< float, pcl::search::internal::PointCloudAdaptor< float >, float, pcl::index_t > L1_Adaptor
nanoflann::L2_Adaptor< float, pcl::search::internal::PointCloudAdaptor< float >, float, pcl::index_t > L2_Adaptor
shared_ptr< const Indices > IndicesConstPtr
Definition: pcl_base.h:59
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
A point structure representing Euclidean xyz coordinates, and the RGB color.
Helper for KdTreeNanoflann, serves as a dataset adaptor.
T kdtree_get_pt(const std::size_t idx, const std::size_t dim) const
void reset_adaptor(const T *data, bool delete_data, std::size_t dim, std::size_t point_count)
PointCloudAdaptor(const T *data, bool delete_data, std::size_t dim, std::size_t point_count)