40 #ifndef PCL_SEARCH_IMPL_FLANN_SEARCH_H_
41 #define PCL_SEARCH_IMPL_FLANN_SEARCH_H_
43 #include <flann/algorithms/kdtree_index.h>
44 #include <flann/algorithms/kdtree_single_index.h>
45 #include <flann/algorithms/kmeans_index.h>
47 #include <pcl/search/flann_search.h>
48 #include <pcl/kdtree/kdtree_flann.h>
50 #include <pcl/kdtree/impl/kdtree_flann.hpp>
53 template <
typename Po
intT,
typename FlannDistance>
57 return (
static_cast<IndexPtr> (
new flann::KDTreeSingleIndex<FlannDistance> (*data,
static_cast<flann::KDTreeSingleIndexParams
> (max_leaf_size_))));
61 template <
typename Po
intT,
typename FlannDistance>
65 return (
static_cast<IndexPtr> (
new flann::KMeansIndex<FlannDistance> (*data,flann::KMeansIndexParams ())));
69 template <
typename Po
intT,
typename FlannDistance>
73 return (
static_cast<IndexPtr> (
new flann::KDTreeIndex<FlannDistance> (*data,
static_cast<flann::KDTreeIndexParams
> (trees_))));
77 template <
typename Po
intT,
typename FlannDistance>
86 template <
typename Po
intT,
typename FlannDistance>
89 if (input_copied_for_flann_)
90 delete [] input_flann_->ptr();
94 template <
typename Po
intT,
typename FlannDistance>
void
99 convertInputToFlannMatrix ();
100 index_ = creator_->createIndex (input_flann_);
101 index_->buildIndex ();
105 template <
typename Po
intT,
typename FlannDistance>
int
108 assert (point_representation_->isValid (point) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
109 bool can_cast = point_representation_->isTrivial ();
111 float* data =
nullptr;
114 data =
new float [point_representation_->getNumberOfDimensions ()];
115 point_representation_->vectorize (point,data);
118 float* cdata = can_cast ?
const_cast<float*
> (
reinterpret_cast<const float*
> (&point)): data;
121 flann::SearchParams p;
123 p.sorted = sorted_results_;
125 if (indices.size() !=
static_cast<unsigned int> (k))
126 indices.resize (k,-1);
127 if (dists.size() !=
static_cast<unsigned int> (k))
130 int result =
knn_search(*index_, m, indices, d, k, p);
134 if (!identity_mapping_)
136 for (std::size_t i = 0; i < static_cast<unsigned int> (k); ++i)
138 auto& neighbor_index = indices[i];
139 neighbor_index = index_mapping_[neighbor_index];
146 template <
typename Po
intT,
typename FlannDistance>
void
148 const PointCloud& cloud,
const Indices& indices,
int k, std::vector<Indices>& k_indices,
149 std::vector< std::vector<float> >& k_sqr_distances)
const
151 if (indices.empty ())
153 k_indices.resize (cloud.size ());
154 k_sqr_distances.resize (cloud.size ());
156 if (! cloud.is_dense)
158 for (std::size_t i = 0; i < cloud.size(); i++)
160 assert (point_representation_->isValid (cloud[i]) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
164 bool can_cast = point_representation_->isTrivial ();
170 data =
new float[dim_*cloud.size ()];
171 for (std::size_t i = 0; i < cloud.size (); ++i)
173 float* out = data+i*dim_;
174 point_representation_->vectorize (cloud[i],out);
180 float* cdata = can_cast ?
const_cast<float*
> (
reinterpret_cast<const float*
> (&cloud[0])): data;
183 flann::SearchParams p;
184 p.sorted = sorted_results_;
187 knn_search(*index_, m, k_indices, k_sqr_distances, k, p);
193 k_indices.resize (indices.size ());
194 k_sqr_distances.resize (indices.size ());
196 if (! cloud.is_dense)
198 for (std::size_t i = 0; i < indices.size(); i++)
200 assert (point_representation_->isValid (cloud [indices[i]]) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
204 float* data=
new float [dim_*indices.size ()];
205 for (std::size_t i = 0; i < indices.size (); ++i)
207 float* out = data+i*dim_;
208 point_representation_->vectorize (cloud[indices[i]],out);
210 const flann::Matrix<float> m (data ,indices.size (), point_representation_->getNumberOfDimensions ());
212 flann::SearchParams p;
213 p.sorted = sorted_results_;
216 knn_search(*index_, m, k_indices, k_sqr_distances, k, p);
220 if (!identity_mapping_)
222 for (
auto &k_index : k_indices)
224 for (
auto &neighbor_index : k_index)
226 neighbor_index = index_mapping_[neighbor_index];
233 template <
typename Po
intT,
typename FlannDistance>
int
235 Indices &indices, std::vector<float> &distances,
236 unsigned int max_nn)
const
238 assert (point_representation_->isValid (point) &&
"Invalid (NaN, Inf) point coordinates given to radiusSearch!");
239 bool can_cast = point_representation_->isTrivial ();
241 float* data =
nullptr;
244 data =
new float [point_representation_->getNumberOfDimensions ()];
245 point_representation_->vectorize (point,data);
248 float* cdata = can_cast ?
const_cast<float*
> (
reinterpret_cast<const float*
> (&point)) : data;
251 flann::SearchParams p;
252 p.sorted = sorted_results_;
254 p.max_neighbors = max_nn > 0 ? max_nn : -1;
256 std::vector<Indices> i (1);
257 std::vector<std::vector<float> > d (1);
258 int result =
radius_search(*index_, m, i, d,
static_cast<float>(radius * radius), p);
264 if (!identity_mapping_)
266 for (
auto &neighbor_index : indices)
268 neighbor_index = index_mapping_ [neighbor_index];
275 template <
typename Po
intT,
typename FlannDistance>
void
277 const PointCloud& cloud,
const Indices& indices,
double radius, std::vector<Indices>& k_indices,
278 std::vector< std::vector<float> >& k_sqr_distances,
unsigned int max_nn)
const
280 if (indices.empty ())
282 k_indices.resize (cloud.size ());
283 k_sqr_distances.resize (cloud.size ());
285 if (! cloud.is_dense)
287 for (std::size_t i = 0; i < cloud.size(); i++)
289 assert (point_representation_->isValid (cloud[i]) &&
"Invalid (NaN, Inf) point coordinates given to radiusSearch!");
293 bool can_cast = point_representation_->isTrivial ();
295 float* data =
nullptr;
298 data =
new float[dim_*cloud.size ()];
299 for (std::size_t i = 0; i < cloud.size (); ++i)
301 float* out = data+i*dim_;
302 point_representation_->vectorize (cloud[i],out);
306 float* cdata = can_cast ?
const_cast<float*
> (
reinterpret_cast<const float*
> (&cloud[0])) : data;
309 flann::SearchParams p;
310 p.sorted = sorted_results_;
314 p.max_neighbors = max_nn != 0 ? max_nn : -1;
316 *index_, m, k_indices, k_sqr_distances,
static_cast<float>(radius * radius), p);
322 k_indices.resize (indices.size ());
323 k_sqr_distances.resize (indices.size ());
325 if (! cloud.is_dense)
327 for (std::size_t i = 0; i < indices.size(); i++)
329 assert (point_representation_->isValid (cloud [indices[i]]) &&
"Invalid (NaN, Inf) point coordinates given to radiusSearch!");
333 float* data =
new float [dim_ * indices.size ()];
334 for (std::size_t i = 0; i < indices.size (); ++i)
336 float* out = data+i*dim_;
337 point_representation_->vectorize (cloud[indices[i]], out);
339 const flann::Matrix<float> m (data, cloud.size (), point_representation_->getNumberOfDimensions ());
341 flann::SearchParams p;
342 p.sorted = sorted_results_;
346 p.max_neighbors = max_nn != 0 ? max_nn : -1;
348 *index_, m, k_indices, k_sqr_distances,
static_cast<float>(radius * radius), p);
352 if (!identity_mapping_)
354 for (
auto &k_index : k_indices)
356 for (
auto &neighbor_index : k_index)
358 neighbor_index = index_mapping_[neighbor_index];
365 template <
typename Po
intT,
typename FlannDistance>
void
368 std::size_t original_no_of_points = indices_ && !indices_->empty () ? indices_->size () : input_->size ();
370 if (input_copied_for_flann_)
371 delete input_flann_->ptr();
372 input_copied_for_flann_ =
true;
373 index_mapping_.clear();
374 identity_mapping_ =
true;
380 if (!indices_ || indices_->empty ())
383 if (input_->is_dense && point_representation_->isTrivial ())
386 input_flann_ =
static_cast<MatrixPtr> (
new flann::Matrix<float> (
const_cast<float*
>(
reinterpret_cast<const float*
>(&(*input_) [0])), original_no_of_points, point_representation_->getNumberOfDimensions (),sizeof (
PointT)));
387 input_copied_for_flann_ =
false;
391 input_flann_ =
static_cast<MatrixPtr> (
new flann::Matrix<float> (
new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ()));
392 float* cloud_ptr = input_flann_->ptr();
393 for (std::size_t i = 0; i < original_no_of_points; ++i)
395 const PointT& point = (*input_)[i];
397 if (!point_representation_->isValid (point))
399 identity_mapping_ =
false;
403 index_mapping_.push_back (
static_cast<index_t> (i));
405 point_representation_->vectorize (point, cloud_ptr);
413 input_flann_ =
static_cast<MatrixPtr> (
new flann::Matrix<float> (
new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ()));
414 float* cloud_ptr = input_flann_->ptr();
415 for (std::size_t indices_index = 0; indices_index < original_no_of_points; ++indices_index)
417 index_t cloud_index = (*indices_)[indices_index];
418 const PointT& point = (*input_)[cloud_index];
420 if (!point_representation_->isValid (point))
422 identity_mapping_ =
false;
426 index_mapping_.push_back (
static_cast<index_t> (indices_index));
428 point_representation_->vectorize (point, cloud_ptr);
432 if (input_copied_for_flann_)
433 input_flann_->rows = index_mapping_.size ();
436 #define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch<T>;
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
virtual IndexPtr createIndex(MatrixConstPtr data)
Create a FLANN Index from the input data.
IndexPtr createIndex(MatrixConstPtr data) override
Create a FLANN Index from the input data.
virtual IndexPtr createIndex(MatrixConstPtr data)
Create a FLANN Index from the input data.
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
void convertInputToFlannMatrix()
converts the input data to a format usable by FLANN
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.
shared_ptr< flann::NNIndex< FlannDistance > > IndexPtr
int checks_
Number of checks to perform for approximate NN search using the multiple randomized tree index.
shared_ptr< const flann::Matrix< float > > MatrixConstPtr
FlannSearch(bool sorted=true, FlannIndexCreatorPtr creator=FlannIndexCreatorPtr(new KdTreeIndexCreator()))
~FlannSearch() override
Destructor for FlannSearch.
PointRepresentationConstPtr point_representation_
shared_ptr< FlannIndexCreator > FlannIndexCreatorPtr
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.
typename Search< PointT >::PointCloudConstPtr PointCloudConstPtr
typename Search< PointT >::PointCloud PointCloud
FlannIndexCreatorPtr creator_
The index creator, used to (re-) create the index when the search data is passed.
IndexPtr index_
The FLANN index.
bool input_copied_for_flann_
float eps_
Epsilon for approximate NN search.
shared_ptr< flann::Matrix< float > > MatrixPtr
pcl::IndicesConstPtr 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.
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.