39#ifndef PCL_KDTREE_KDTREE_IMPL_FLANN_H_
40#define PCL_KDTREE_KDTREE_IMPL_FLANN_H_
42#include <flann/flann.hpp>
44#include <pcl/kdtree/kdtree_flann.h>
45#include <pcl/console/print.h>
48template <
typename Po
intT,
typename Dist>
52 , identity_mapping_ (
false)
53 , dim_ (0), total_nr_points_ (0)
57 if (!std::is_same<std::size_t, pcl::index_t>::value) {
58 const auto message =
"FLANN is not optimized for current index type. Will incur "
59 "extra allocations and copy\n";
60 if (std::is_same<int, pcl::index_t>::value) {
70template <
typename Po
intT,
typename Dist>
74 , identity_mapping_ (
false)
75 , dim_ (0), total_nr_points_ (0)
83template <
typename Po
intT,
typename Dist>
void
87 param_k_ = ::flann::SearchParams (-1 , epsilon_);
88 param_radius_ = ::flann::SearchParams (-1 , epsilon_, sorted_);
92template <
typename Po
intT,
typename Dist>
void
96 param_k_ = ::flann::SearchParams (-1, epsilon_);
97 param_radius_ = ::flann::SearchParams (-1, epsilon_, sorted_);
101template <
typename Po
intT,
typename Dist>
void
107 dim_ = point_representation_->getNumberOfDimensions ();
115 PCL_ERROR (
"[pcl::KdTreeFLANN::setInputCloud] Invalid input!\n");
118 if (indices !=
nullptr)
120 convertCloudToArray (*input_, *indices_);
124 convertCloudToArray (*input_);
126 total_nr_points_ =
static_cast<uindex_t> (index_mapping_.size ());
127 if (total_nr_points_ == 0)
129 PCL_ERROR (
"[pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!\n");
134 index_mapping_.size (),
136 ::flann::KDTreeSingleIndexParams (15)));
137 flann_index_->buildIndex ();
169 std::vector<std::size_t> indices(k);
174 std::copy(indices.cbegin(), indices.cend(),
k_indices.begin());
178template <
class IndexT,
class A,
class B,
class F, CompatWithFlann<IndexT> = true>
183 std::vector<std::vector<float>>& dists,
190template <
class IndexT,
class A,
class B,
class F, NotCompatWithFlann<IndexT> = true>
195 std::vector<std::vector<float>>& dists,
199 std::vector<std::vector<std::size_t>> indices;
201 auto ret = index.knnSearch(
query, indices, dists, k, params);
205 auto it = indices.cbegin();
207 for (;
it != indices.cend(); ++
it, ++
jt) {
209 std::copy(
it->cbegin(),
it->cend(),
jt->begin());
215template <
class FlannIndex,
228 return detail::knn_search<pcl::index_t>(index,
query, indices, dists, k, params);
232template <
typename Po
intT,
typename Dist>
int
237 assert (point_representation_->isValid (point) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
239 if (k > total_nr_points_)
240 k = total_nr_points_;
248 std::vector<float>
query (dim_);
249 point_representation_->vectorize (
static_cast<PointT> (point),
query);
262 if (!identity_mapping_)
278template <
class IndexT,
284 CompatWithFlann<IndexT> =
true>
288 std::vector<pcl::Indices> indices(1);
304 std::vector<std::vector<std::size_t>> indices(1);
307 std::copy(indices[0].cbegin(), indices[0].cend(),
k_indices.begin());
311template <
class IndexT,
class A,
class B,
class F, CompatWithFlann<IndexT> = true>
316 std::vector<std::vector<float>>& dists,
320 return index.radiusSearch(
query,
k_indices, dists, radius, params);
323template <
class IndexT,
class A,
class B,
class F, NotCompatWithFlann<IndexT> = true>
328 std::vector<std::vector<float>>& dists,
332 std::vector<std::vector<std::size_t>> indices;
334 auto ret = index.radiusSearch(
query, indices, dists, radius, params);
338 auto it = indices.cbegin();
340 for (;
it != indices.cend(); ++
it, ++
jt) {
342 std::copy(
it->cbegin(),
it->cend(),
jt->begin());
348template <
class FlannIndex,
361 return detail::radius_search<pcl::index_t>(
362 index,
query, indices, dists, radius, params);
366template <
typename Po
intT,
typename Dist>
int
370 assert (point_representation_->isValid (point) &&
"Invalid (NaN, Inf) point coordinates given to radiusSearch!");
372 std::vector<float>
query (dim_);
373 point_representation_->vectorize (
static_cast<PointT> (point),
query);
377 max_nn = total_nr_points_;
379 std::vector<std::vector<float> > dists(1);
381 ::flann::SearchParams params (param_radius_);
382 if (
max_nn == total_nr_points_)
383 params.max_neighbors = -1;
385 params.max_neighbors =
max_nn;
392 static_cast<float>(radius * radius),
398 if (!identity_mapping_)
411template <
typename Po
intT,
typename Dist>
void
415 index_mapping_.clear ();
422template <
typename Po
intT,
typename Dist>
void
432 const auto original_no_of_points = cloud.
size ();
434 cloud_.reset (
new float[original_no_of_points * dim_], std::default_delete<
float[]> ());
435 float* cloud_ptr = cloud_.get ();
436 index_mapping_.reserve (original_no_of_points);
437 identity_mapping_ =
true;
439 for (std::size_t cloud_index = 0; cloud_index < original_no_of_points; ++cloud_index)
442 if (!point_representation_->isValid (cloud[cloud_index]))
444 identity_mapping_ =
false;
448 index_mapping_.push_back (cloud_index);
450 point_representation_->vectorize (cloud[cloud_index], cloud_ptr);
456template <
typename Po
intT,
typename Dist>
void
466 int original_no_of_points =
static_cast<int> (indices.size ());
468 cloud_.reset (
new float[original_no_of_points * dim_], std::default_delete<
float[]> ());
469 float* cloud_ptr = cloud_.get ();
470 index_mapping_.reserve (original_no_of_points);
478 identity_mapping_ =
false;
480 for (
const auto &index : indices)
483 if (!point_representation_->isValid (cloud[index]))
487 index_mapping_.push_back (index);
489 point_representation_->vectorize (cloud[index], cloud_ptr);
494#define PCL_INSTANTIATE_KdTreeFLANN(T) template class PCL_EXPORTS pcl::KdTreeFLANN<T>;
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
void setEpsilon(float eps) override
Set the search epsilon precision (error bound) for nearest neighbors searches.
shared_ptr< const Indices > IndicesConstPtr
int nearestKSearch(const PointT &point, unsigned int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const override
Search for k-nearest neighbors for the given query point.
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.
typename KdTree< PointT >::PointCloudConstPtr PointCloudConstPtr
void setSortedResults(bool sorted)
KdTreeFLANN(bool sorted=true)
Default Constructor for KdTreeFLANN.
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
KdTree represents the base spatial locator class for kd-tree implementations.
PointCloud represents the base class in PCL for storing collections of 3D points.
int knn_search(A &index, B &query, C &k_indices, D &dists, unsigned int k, F ¶ms)
int radius_search(A &index, B &query, C &k_indices, D &dists, float radius, F ¶ms)
IndicesAllocator<> Indices
Type used for indices in PCL.
int radius_search(const FlannIndex &index, const Query &query, Indices &indices, Distances &dists, float radius, const SearchParams ¶ms)
Comaptibility template function to allow use of various types of indices with FLANN.
int knn_search(const FlannIndex &index, const Query &query, Indices &indices, Distances &dists, unsigned int k, const SearchParams ¶ms)
Comaptibility template function to allow use of various types of indices with FLANN.
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
A point structure representing Euclidean xyz coordinates, and the RGB color.