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>
48 template <
typename Po
intT,
typename Dist>
52 , identity_mapping_ (false)
53 , dim_ (0), total_nr_points_ (0)
54 , param_k_ (::
flann::SearchParams (-1 , epsilon_))
55 , param_radius_ (::
flann::SearchParams (-1, epsilon_, sorted))
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) {
70 template <
typename Po
intT,
typename Dist>
74 , identity_mapping_ (false)
75 , dim_ (0), total_nr_points_ (0)
76 , param_k_ (::
flann::SearchParams (-1 , epsilon_))
77 , param_radius_ (::
flann::SearchParams (-1, epsilon_, false))
83 template <
typename Po
intT,
typename Dist>
void
87 param_k_ = ::flann::SearchParams (-1 , epsilon_);
88 param_radius_ = ::flann::SearchParams (-1 , epsilon_, sorted_);
92 template <
typename Po
intT,
typename Dist>
void
96 param_k_ = ::flann::SearchParams (-1, epsilon_);
97 param_radius_ = ::flann::SearchParams (-1, epsilon_, sorted_);
101 template <
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 ();
144 template <
class IndexT,
150 CompatWithFlann<IndexT> =
true>
152 knn_search(A& index,
B& query, C& k_indices, D& dists,
unsigned int k, F& params)
156 return index.knnSearch(query, k_indices_mat, dists, k, params);
159 template <
class IndexT,
165 NotCompatWithFlann<IndexT> =
true>
167 knn_search(A& index,
B& query, C& k_indices, D& dists,
unsigned int k, F& params)
169 std::vector<std::size_t> indices(k);
173 auto ret = index.knnSearch(query, indices_mat, dists, k, params);
175 std::transform(indices.cbegin(),
178 [](
const auto& x) { return static_cast<pcl::index_t>(x); });
182 template <
class IndexT,
class A,
class B,
class F, CompatWithFlann<IndexT> = true>
186 std::vector<Indices>& k_indices,
187 std::vector<std::vector<float>>& dists,
191 return index.knnSearch(query, k_indices, dists, k, params);
194 template <
class IndexT,
class A,
class B,
class F, NotCompatWithFlann<IndexT> = true>
198 std::vector<Indices>& k_indices,
199 std::vector<std::vector<float>>& dists,
203 std::vector<std::vector<std::size_t>> indices;
205 auto ret = index.knnSearch(query, indices, dists, k, params);
207 k_indices.resize(indices.size());
209 auto it = indices.cbegin();
210 auto jt = k_indices.begin();
211 for (; it != indices.cend(); ++it, ++jt) {
212 jt->resize(it->size());
213 std::copy(it->cbegin(), it->cend(), jt->begin());
219 template <
class FlannIndex,
230 const SearchParams& params)
232 return detail::knn_search<pcl::index_t>(index, query, indices, dists, k, params);
236 template <
typename Po
intT,
typename Dist>
int
239 std::vector<float> &k_distances)
const
241 assert (point_representation_->isValid (point) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
243 if (k > total_nr_points_)
244 k = total_nr_points_;
246 k_indices.resize (k);
247 k_distances.resize (k);
252 std::vector<float> query (dim_);
253 point_representation_->vectorize (
static_cast<PointT> (point), query);
266 if (!identity_mapping_)
268 for (std::size_t i = 0; i < static_cast<std::size_t> (k); ++i)
270 auto& neighbor_index = k_indices[i];
271 neighbor_index = index_mapping_[neighbor_index];
282 template <
class IndexT,
288 CompatWithFlann<IndexT> =
true>
290 radius_search(A& index,
B& query, C& k_indices, D& dists,
float radius, F& params)
292 std::vector<pcl::Indices> indices(1);
293 int neighbors_in_radius = index.radiusSearch(query, indices, dists, radius, params);
294 k_indices = std::move(indices[0]);
295 return neighbors_in_radius;
298 template <
class IndexT,
304 NotCompatWithFlann<IndexT> =
true>
306 radius_search(A& index,
B& query, C& k_indices, D& dists,
float radius, F& params)
308 std::vector<std::vector<std::size_t>> indices(1);
309 int neighbors_in_radius = index.radiusSearch(query, indices, dists, radius, params);
310 k_indices.resize(indices[0].size());
312 std::transform(indices[0].cbegin(),
315 [](
const auto& x) { return static_cast<pcl::index_t>(x); });
316 return neighbors_in_radius;
319 template <
class IndexT,
class A,
class B,
class F, CompatWithFlann<IndexT> = true>
323 std::vector<Indices>& k_indices,
324 std::vector<std::vector<float>>& dists,
328 return index.radiusSearch(query, k_indices, dists, radius, params);
331 template <
class IndexT,
class A,
class B,
class F, NotCompatWithFlann<IndexT> = true>
335 std::vector<Indices>& k_indices,
336 std::vector<std::vector<float>>& dists,
340 std::vector<std::vector<std::size_t>> indices;
342 auto ret = index.radiusSearch(query, indices, dists, radius, params);
344 k_indices.resize(indices.size());
346 auto it = indices.cbegin();
347 auto jt = k_indices.begin();
348 for (; it != indices.cend(); ++it, ++jt) {
349 jt->resize(it->size());
350 std::copy(it->cbegin(), it->cend(), jt->begin());
356 template <
class FlannIndex,
367 const SearchParams& params)
369 return detail::radius_search<pcl::index_t>(
370 index, query, indices, dists, radius, params);
374 template <
typename Po
intT,
typename Dist>
int
376 std::vector<float> &k_sqr_dists,
unsigned int max_nn)
const
378 assert (point_representation_->isValid (point) &&
"Invalid (NaN, Inf) point coordinates given to radiusSearch!");
380 std::vector<float> query (dim_);
381 point_representation_->vectorize (
static_cast<PointT> (point), query);
384 if (max_nn == 0 || max_nn > total_nr_points_)
385 max_nn = total_nr_points_;
387 std::vector<std::vector<float> > dists(1);
389 ::flann::SearchParams params (param_radius_);
390 if (max_nn == total_nr_points_)
391 params.max_neighbors = -1;
393 params.max_neighbors = max_nn;
400 static_cast<float>(radius * radius),
403 k_sqr_dists = dists[0];
406 if (!identity_mapping_)
408 for (
int i = 0; i < neighbors_in_radius; ++i)
410 auto& neighbor_index = k_indices[i];
411 neighbor_index = index_mapping_[neighbor_index];
415 return (neighbors_in_radius);
419 template <
typename Po
intT,
typename Dist>
void
423 index_mapping_.clear ();
430 template <
typename Po
intT,
typename Dist>
void
440 const auto original_no_of_points = cloud.
size ();
442 cloud_.reset (
new float[original_no_of_points * dim_], std::default_delete<
float[]> ());
443 float* cloud_ptr = cloud_.get ();
444 index_mapping_.reserve (original_no_of_points);
445 identity_mapping_ =
true;
447 for (std::size_t cloud_index = 0; cloud_index < original_no_of_points; ++cloud_index)
450 if (!point_representation_->isValid (cloud[cloud_index]))
452 identity_mapping_ =
false;
456 index_mapping_.push_back (cloud_index);
458 point_representation_->vectorize (cloud[cloud_index], cloud_ptr);
464 template <
typename Po
intT,
typename Dist>
void
474 int original_no_of_points =
static_cast<int> (indices.size ());
476 cloud_.reset (
new float[original_no_of_points * dim_], std::default_delete<
float[]> ());
477 float* cloud_ptr = cloud_.get ();
478 index_mapping_.reserve (original_no_of_points);
486 identity_mapping_ =
false;
488 for (
const auto &index : indices)
491 if (!point_representation_->isValid (cloud[index]))
495 index_mapping_.push_back (index);
497 point_representation_->vectorize (cloud[index], cloud_ptr);
502 #define PCL_INSTANTIATE_KdTreeFLANN(T) template class PCL_EXPORTS pcl::KdTreeFLANN<T>;
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.
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.
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.
shared_ptr< const Indices > IndicesConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
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)
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
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.
A point structure representing Euclidean xyz coordinates, and the RGB color.