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