12#include <pcl/pcl_config.h>
13#if PCL_HAS_NANOFLANN || defined(DOXYGEN_ONLY)
15#include <pcl/search/search.h>
16#include <pcl/point_representation.h>
18#include <nanoflann.hpp>
25template <
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_;
83template <
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,
126#if NANOFLANN_VERSION < 0x020000
128 nanoflann::SO2_Adaptor<float,
133 nanoflann::SO3_Adaptor<float,
180 std::int32_t Dim = 3,
182#if defined(DOXYGEN_ONLY)
187 typename Tree = nanoflann::KDTreeSingleIndexAdaptor<
197 template <
typename _DistanceType =
float,
typename _IndexType = pcl::index_t>
198 class PCLRadiusResultSet {
200 using DistanceType = _DistanceType;
201 using IndexType = _IndexType;
204 const DistanceType radius;
206 std::vector<IndexType>& m_indices;
207 std::vector<DistanceType>& m_dists;
208 std::size_t max_results_;
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)
224 if (max_results_ != std::numeric_limits<std::size_t>::max()) {
225 m_indices.reserve(max_results_);
226 m_dists.reserve(max_results_);
239 return m_indices.size();
244 return m_indices.empty();
259 addPoint(DistanceType dist, IndexType index)
262 m_indices.emplace_back(index);
263 m_dists.emplace_back(dist);
265 return (m_indices.size() < max_results_);
297 using Ptr = shared_ptr<KdTreeNanoflann<PointT, Dim, Distance, Tree>>;
298 using ConstPtr = shared_ptr<const KdTreeNanoflann<PointT, Dim, Distance, Tree>>;
317 std::size_t leaf_max_size,
318 unsigned int n_thread_build = 1)
320 , leaf_max_size_(leaf_max_size)
321 , n_thread_build_(n_thread_build)
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(),
358 point_representation_ = point_representation;
369 return point_representation_;
408 use_rknn_ = use_rknn;
430 return nanoflann_tree_;
446 std::vector<float>& k_sqr_distances)
const override
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();
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);
460 nanoflann::KNNResultSet<float, pcl::index_t> resultSet(k);
461 resultSet.init(k_indices.data(), k_sqr_distances.data());
462 nanoflann_tree_->findNeighbors(
464 (query_point ? query_point :
reinterpret_cast<const float*
>(&point))
465#if NANOFLANN_VERSION < 0x150
467 nanoflann::SearchParams()
470 const auto search_result = resultSet.size();
471 delete[] query_point;
472 assert(search_result == k);
474 if (!identity_mapping_) {
475 for (
auto& index : k_indices) {
476 index = index_mapping_[index];
479 return search_result;
497 std::vector<float>& k_sqr_distances,
498 unsigned int max_nn = 0)
const override
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);
513#if NANOFLANN_VERSION < 0x150
514 std::vector<std::pair<pcl::index_t, float>>
517 std::vector<nanoflann::ResultItem<pcl::index_t, float>> IndicesDists;
520 nanoflann::RadiusResultSet<float, pcl::index_t> resultSet(
521 pcl::search::internal::square_if_l2<Distance>(radius), IndicesDists);
522 nanoflann_tree_->findNeighbors(
524 (query_point ? query_point :
reinterpret_cast<const float*
>(&point)),
525#if NANOFLANN_VERSION < 0x150
532#if NANOFLANN_VERSION < 0x160
536 IndicesDists.begin(), IndicesDists.end(), nanoflann::IndexDist_Sorter());
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;
546 delete[] query_point;
547 return search_result;
550 PCLRadiusResultSet<float, pcl::index_t> resultSet(
551 pcl::search::internal::square_if_l2<Distance>(radius),
555 nanoflann_tree_->findNeighbors(
557 (query_point ? query_point :
reinterpret_cast<const float*
>(&point)),
558#if NANOFLANN_VERSION < 0x150
565 const auto search_result = resultSet.size();
566 if (!identity_mapping_) {
567 for (
auto& index : k_indices) {
568 index = index_mapping_[index];
573 delete[] query_point;
574 return search_result;
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
588 nanoflann::KNNResultSet<float, pcl::index_t> resultSet(max_nn);
589 resultSet.init(k_indices.data(), k_sqr_distances.data());
590 nanoflann_tree_->findNeighbors(
592 (query_point ? query_point :
reinterpret_cast<const float*
>(&point))
593#if NANOFLANN_VERSION < 0x150
595 nanoflann::SearchParams()
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);
605 k_indices.resize(search_result);
606 k_sqr_distances.resize(search_result);
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(
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);
619 if (!identity_mapping_) {
620 for (
auto& index : k_indices) {
621 index = index_mapping_[index];
624 delete[] query_point;
625 return search_result;
629 PCLRadiusResultSet<float, pcl::index_t> resultSet(
630 pcl::search::internal::square_if_l2<Distance>(radius),
635 nanoflann_tree_->findNeighbors(
637 (query_point ? query_point :
reinterpret_cast<const float*
>(&point)),
638#if NANOFLANN_VERSION < 0x150
645 const auto search_result = resultSet.size();
646 if (!identity_mapping_) {
647 for (
auto& index : k_indices) {
648 index = index_mapping_[index];
653 delete[] query_point;
654 return search_result;
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");
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",
681 index_mapping_.clear();
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);
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);
698 identity_mapping_ =
true;
700 identity_mapping_ &&
static_cast<std::size_t
>(index) < index_mapping_.size();
702 identity_mapping_ = identity_mapping_ && index_mapping_[index] == index;
705 PCL_DEBUG(
"[KdTreeNanoflann::setUpTree] identity_mapping_=%i, "
706 "point_representation_->getNumberOfDimensions()=%lu, "
707 "point_representation_->isTrivial ()=%i\n",
708 identity_mapping_ ? 1 : 0,
710 point_representation_->isTrivial() ? 1 : 0);
711 if (identity_mapping_ && point_representation_->isTrivial() &&
712 sizeof(
PointT) %
sizeof(
float) == 0) {
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]),
719 sizeof(
PointT) /
sizeof(
float),
720 index_mapping_.size()));
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);
729 adaptor_.reset(
new internal::PointCloudAdaptor<float>(
730 data,
true, dim, index_mapping_.size()));
732 nanoflann_tree_.reset(
735#
if NANOFLANN_VERSION >= 0x150
738 nanoflann::KDTreeSingleIndexAdaptorFlags::None,
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");
751 shared_ptr<internal::PointCloudAdaptor<float>> adaptor_;
754 new DefaultPointRepresentation<PointT>};
758 bool identity_mapping_{
false};
760 std::size_t leaf_max_size_{15};
762 unsigned int n_thread_build_{1};
766 bool use_rknn_{
true};
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< Tree > KdTreePtr
shared_ptr< const KdTreeNanoflann< PointT, Dim, Distance, Tree > > ConstPtr
~KdTreeNanoflann() override=default
Destructor for KdTreeNanoflann.
virtual IndicesConstPtr getIndices() const
Get a pointer to the vector of indices used.
PointCloudConstPtr input_
void sortResults(Indices &indices, std::vector< float > &distances) const
pcl::IndicesConstPtr IndicesConstPtr
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
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.
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)