12 #include <pcl/pcl_config.h>
13 #if PCL_HAS_NANOFLANN || defined(DOXYGEN_ONLY)
15 #include <pcl/search/kdtree.h>
16 #include <pcl/point_representation.h>
18 #include <nanoflann.hpp>
25 template <
typename T =
float>
30 std::size_t point_count)
31 : data_(data), delete_data_(delete_data), dim_(dim), point_count_(point_count)
44 std::size_t point_count)
49 delete_data_ = delete_data;
51 point_count_ = point_count;
63 return data_[dim_ * idx + dim];
77 std::size_t point_count_;
83 template <
typename Dist>
96 return radius * radius;
101 nanoflann::L2_Simple_Adaptor<float,
106 return radius * radius;
112 nanoflann::L1_Adaptor<float,
117 nanoflann::L2_Adaptor<float,
122 nanoflann::L2_Simple_Adaptor<float,
127 nanoflann::SO2_Adaptor<float,
132 nanoflann::SO3_Adaptor<float,
177 template <
typename PointT,
178 std::int32_t Dim = 3,
181 typename Tree = nanoflann::KDTreeSingleIndexAdaptor<
191 template <
typename _DistanceType =
float,
typename _IndexType = pcl::index_t>
192 class PCLRadiusResultSet {
194 using DistanceType = _DistanceType;
195 using IndexType = _IndexType;
198 const DistanceType radius;
200 std::vector<IndexType>& m_indices;
201 std::vector<DistanceType>& m_dists;
202 std::size_t max_results_;
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)
218 if (max_results_ != std::numeric_limits<std::size_t>::max()) {
219 m_indices.reserve(max_results_);
220 m_dists.reserve(max_results_);
233 return m_indices.size();
238 return m_indices.empty();
253 addPoint(DistanceType dist, IndexType index)
256 m_indices.emplace_back(index);
257 m_dists.emplace_back(dist);
259 return (m_indices.size() < max_results_);
291 using Ptr = shared_ptr<KdTreeNanoflann<PointT, Dim, Distance, Tree>>;
292 using ConstPtr = shared_ptr<const KdTreeNanoflann<PointT, Dim, Distance, Tree>>;
294 using KdTreePtr = shared_ptr<Tree>;
295 using KdTreeConstPtr = shared_ptr<const Tree>;
296 using PointRepresentationConstPtr = shared_ptr<const PointRepresentation<PointT>>;
310 KdTreeNanoflann(
bool sorted,
311 std::size_t leaf_max_size,
312 unsigned int n_thread_build = 1)
314 , leaf_max_size_(leaf_max_size)
315 , n_thread_build_(n_thread_build)
325 KdTreeNanoflann(
bool sorted =
false) : KdTreeNanoflann(sorted, 15, 1) {}
328 ~KdTreeNanoflann()
override =
default;
336 setPointRepresentation(
const PointRepresentationConstPtr& point_representation)
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(),
352 point_representation_ = point_representation;
358 inline PointRepresentationConstPtr
359 getPointRepresentation()
const
361 return point_representation_;
368 setSortedResults(
bool sorted_results)
override
370 sorted_results_ = sorted_results;
378 setEpsilon(
float eps)
398 setUseRKNN(
bool use_rknn)
400 use_rknn_ = use_rknn;
408 setInputCloud(
const PointCloudConstPtr& cloud,
422 return nanoflann_tree_;
435 nearestKSearch(
const PointT& point,
438 std::vector<float>& k_sqr_distances)
const override
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();
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);
452 nanoflann::KNNResultSet<float, pcl::index_t> resultSet(k);
453 resultSet.init(k_indices.data(), k_sqr_distances.data());
454 nanoflann_tree_->findNeighbors(
456 (query_point ? query_point :
reinterpret_cast<const float*
>(&point))
457 #
if NANOFLANN_VERSION < 0x150
459 nanoflann::SearchParams()
462 const auto search_result = resultSet.size();
463 delete[] query_point;
464 assert(search_result == k);
466 if (!identity_mapping_) {
467 for (
auto& index : k_indices) {
468 index = index_mapping_[index];
471 return search_result;
486 radiusSearch(
const PointT& point,
489 std::vector<float>& k_sqr_distances,
490 unsigned int max_nn = 0)
const override
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);
504 if (sorted_results_) {
505 #if NANOFLANN_VERSION < 0x150
506 std::vector<std::pair<pcl::index_t, float>>
509 std::vector<nanoflann::ResultItem<pcl::index_t, float>> IndicesDists;
512 nanoflann::RadiusResultSet<float, pcl::index_t> resultSet(
513 pcl::search::internal::square_if_l2<Distance>(radius), IndicesDists);
514 nanoflann_tree_->findNeighbors(
516 (query_point ? query_point :
reinterpret_cast<const float*
>(&point)),
517 #
if NANOFLANN_VERSION < 0x150
518 {32, eps_, sorted_results_}
521 {eps_, sorted_results_}
524 #if NANOFLANN_VERSION < 0x160
528 IndicesDists.begin(), IndicesDists.end(), nanoflann::IndexDist_Sorter());
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;
538 delete[] query_point;
539 return search_result;
542 PCLRadiusResultSet<float, pcl::index_t> resultSet(
543 pcl::search::internal::square_if_l2<Distance>(radius),
547 nanoflann_tree_->findNeighbors(
549 (query_point ? query_point :
reinterpret_cast<const float*
>(&point)),
550 #
if NANOFLANN_VERSION < 0x150
551 {32, eps_, sorted_results_}
554 {eps_, sorted_results_}
557 const auto search_result = resultSet.size();
558 if (!identity_mapping_) {
559 for (
auto& index : k_indices) {
560 index = index_mapping_[index];
564 this->sortResults(k_indices, k_sqr_distances);
565 delete[] query_point;
566 return search_result;
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
580 nanoflann::KNNResultSet<float, pcl::index_t> resultSet(max_nn);
581 resultSet.init(k_indices.data(), k_sqr_distances.data());
582 nanoflann_tree_->findNeighbors(
584 (query_point ? query_point :
reinterpret_cast<const float*
>(&point))
585 #
if NANOFLANN_VERSION < 0x150
587 nanoflann::SearchParams()
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);
597 k_indices.resize(search_result);
598 k_sqr_distances.resize(search_result);
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(
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);
611 if (!identity_mapping_) {
612 for (
auto& index : k_indices) {
613 index = index_mapping_[index];
616 delete[] query_point;
617 return search_result;
621 PCLRadiusResultSet<float, pcl::index_t> resultSet(
622 pcl::search::internal::square_if_l2<Distance>(radius),
627 nanoflann_tree_->findNeighbors(
629 (query_point ? query_point :
reinterpret_cast<const float*
>(&point)),
630 #
if NANOFLANN_VERSION < 0x150
631 {32, eps_, sorted_results_}
634 {eps_, sorted_results_}
637 const auto search_result = resultSet.size();
638 if (!identity_mapping_) {
639 for (
auto& index : k_indices) {
640 index = index_mapping_[index];
644 this->sortResults(k_indices, k_sqr_distances);
645 delete[] query_point;
646 return search_result;
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");
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",
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);
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);
690 identity_mapping_ =
true;
692 identity_mapping_ &&
static_cast<std::size_t
>(index) < index_mapping_.size();
694 identity_mapping_ = identity_mapping_ && index_mapping_[index] == index;
697 PCL_DEBUG(
"[KdTreeNanoflann::setUpTree] identity_mapping_=%i, "
698 "point_representation_->getNumberOfDimensions()=%lu, "
699 "point_representation_->isTrivial ()=%i\n",
700 identity_mapping_ ? 1 : 0,
702 point_representation_->isTrivial() ? 1 : 0);
703 if (identity_mapping_ && point_representation_->isTrivial() &&
704 sizeof(
PointT) %
sizeof(
float) == 0) {
706 PCL_DEBUG(
"[KdTreeNanoflann::setUpTree] Mapping cloud directly, without "
707 "allocating memory.\n");
709 reinterpret_cast<const float*
>(&(*input_)[0]),
711 sizeof(
PointT) /
sizeof(
float),
712 index_mapping_.size()));
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);
722 data,
true, dim, index_mapping_.size()));
724 nanoflann_tree_.reset(
727 #
if NANOFLANN_VERSION >= 0x150
730 nanoflann::KDTreeSingleIndexAdaptorFlags::None,
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");
741 KdTreePtr nanoflann_tree_;
743 shared_ptr<internal::PointCloudAdaptor<float>> adaptor_;
745 PointRepresentationConstPtr point_representation_{
750 bool identity_mapping_{
false};
752 std::size_t leaf_max_size_{15};
754 unsigned int n_thread_build_{1};
758 bool use_rknn_{
true};
762 #ifdef PCL_NO_PRECOMPILE
763 #include <pcl/search/impl/kdtree.hpp>
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< const PointCloud< PointT > > ConstPtr
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
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
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
IndicesAllocator<> Indices
Type used for indices in PCL.
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
std::size_t kdtree_get_point_count() const
bool kdtree_get_bbox(BBOX &) 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)