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> 50 template <
typename Po
intT,
typename FlannDistance>
54 return (
IndexPtr (
new flann::KDTreeSingleIndex<FlannDistance> (*data,flann::KDTreeSingleIndexParams (max_leaf_size_))));
58 template <
typename Po
intT,
typename FlannDistance>
62 return (
IndexPtr (
new flann::KMeansIndex<FlannDistance> (*data,flann::KMeansIndexParams ())));
66 template <
typename Po
intT,
typename FlannDistance>
70 return (
IndexPtr (
new flann::KDTreeIndex<FlannDistance> (*data, flann::KDTreeIndexParams (trees_))));
74 template <
typename Po
intT,
typename FlannDistance>
83 template <
typename Po
intT,
typename FlannDistance>
86 if (input_copied_for_flann_)
87 delete [] input_flann_->ptr();
91 template <
typename Po
intT,
typename FlannDistance>
void 96 convertInputToFlannMatrix ();
97 index_ = creator_->createIndex (input_flann_);
98 index_->buildIndex ();
102 template <
typename Po
intT,
typename FlannDistance>
int 105 assert (point_representation_->isValid (point) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
106 bool can_cast = point_representation_->isTrivial ();
108 float* data =
nullptr;
111 data =
new float [point_representation_->getNumberOfDimensions ()];
112 point_representation_->vectorize (point,data);
115 float* cdata = can_cast ?
const_cast<float*
> (
reinterpret_cast<const float*
> (&point)): data;
118 flann::SearchParams p;
120 p.sorted = sorted_results_;
122 if (indices.size() !=
static_cast<unsigned int> (k))
123 indices.resize (k,-1);
124 if (dists.size() !=
static_cast<unsigned int> (k))
128 int result = index_->knnSearch (m,i,d,k, p);
132 if (!identity_mapping_)
134 for (std::size_t i = 0; i < static_cast<unsigned int> (k); ++i)
136 auto& neighbor_index = indices[i];
137 neighbor_index = index_mapping_[neighbor_index];
144 template <
typename Po
intT,
typename FlannDistance>
void 146 const PointCloud& cloud,
const Indices& indices,
int k, std::vector<Indices>& k_indices,
147 std::vector< std::vector<float> >& k_sqr_distances)
const 149 if (indices.empty ())
151 k_indices.resize (cloud.size ());
152 k_sqr_distances.resize (cloud.size ());
154 if (! cloud.is_dense)
156 for (std::size_t i = 0; i < cloud.size(); i++)
158 assert (point_representation_->isValid (cloud[i]) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
162 bool can_cast = point_representation_->isTrivial ();
168 data =
new float[dim_*cloud.size ()];
169 for (std::size_t i = 0; i < cloud.size (); ++i)
171 float* out = data+i*dim_;
172 point_representation_->vectorize (cloud[i],out);
178 float* cdata = can_cast ?
const_cast<float*
> (
reinterpret_cast<const float*
> (&cloud[0])): data;
181 flann::SearchParams p;
182 p.sorted = sorted_results_;
185 index_->knnSearch (m,k_indices,k_sqr_distances,k, p);
191 k_indices.resize (indices.size ());
192 k_sqr_distances.resize (indices.size ());
194 if (! cloud.is_dense)
196 for (std::size_t i = 0; i < indices.size(); i++)
198 assert (point_representation_->isValid (cloud [indices[i]]) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
202 float* data=
new float [dim_*indices.size ()];
203 for (std::size_t i = 0; i < indices.size (); ++i)
205 float* out = data+i*dim_;
206 point_representation_->vectorize (cloud[indices[i]],out);
208 const flann::Matrix<float> m (data ,indices.size (), point_representation_->getNumberOfDimensions ());
210 flann::SearchParams p;
211 p.sorted = sorted_results_;
214 index_->knnSearch (m,k_indices,k_sqr_distances,k, p);
218 if (!identity_mapping_)
220 for (
auto &k_index : k_indices)
222 for (
auto &neighbor_index : k_index)
224 neighbor_index = index_mapping_[neighbor_index];
231 template <
typename Po
intT,
typename FlannDistance>
int 233 Indices &indices, std::vector<float> &distances,
234 unsigned int max_nn)
const 236 assert (point_representation_->isValid (point) &&
"Invalid (NaN, Inf) point coordinates given to radiusSearch!");
237 bool can_cast = point_representation_->isTrivial ();
239 float* data =
nullptr;
242 data =
new float [point_representation_->getNumberOfDimensions ()];
243 point_representation_->vectorize (point,data);
246 float* cdata = can_cast ?
const_cast<float*
> (
reinterpret_cast<const float*
> (&point)) : data;
249 flann::SearchParams p;
250 p.sorted = sorted_results_;
252 p.max_neighbors = max_nn > 0 ? max_nn : -1;
254 std::vector<Indices> i (1);
255 std::vector<std::vector<float> > d (1);
256 int result = index_->radiusSearch (m,i,d,static_cast<float> (radius * radius), p);
262 if (!identity_mapping_)
264 for (
auto &neighbor_index : indices)
266 neighbor_index = index_mapping_ [neighbor_index];
273 template <
typename Po
intT,
typename FlannDistance>
void 275 const PointCloud& cloud,
const Indices& indices,
double radius, std::vector<Indices>& k_indices,
276 std::vector< std::vector<float> >& k_sqr_distances,
unsigned int max_nn)
const 278 if (indices.empty ())
280 k_indices.resize (cloud.size ());
281 k_sqr_distances.resize (cloud.size ());
283 if (! cloud.is_dense)
285 for (std::size_t i = 0; i < cloud.size(); i++)
287 assert (point_representation_->isValid (cloud[i]) &&
"Invalid (NaN, Inf) point coordinates given to radiusSearch!");
291 bool can_cast = point_representation_->isTrivial ();
293 float* data =
nullptr;
296 data =
new float[dim_*cloud.size ()];
297 for (std::size_t i = 0; i < cloud.size (); ++i)
299 float* out = data+i*dim_;
300 point_representation_->vectorize (cloud[i],out);
304 float* cdata = can_cast ?
const_cast<float*
> (
reinterpret_cast<const float*
> (&cloud[0])) : data;
307 flann::SearchParams p;
308 p.sorted = sorted_results_;
312 p.max_neighbors = max_nn != 0 ? max_nn : -1;
313 index_->radiusSearch (m,k_indices,k_sqr_distances,static_cast<float> (radius * radius), p);
319 k_indices.resize (indices.size ());
320 k_sqr_distances.resize (indices.size ());
322 if (! cloud.is_dense)
324 for (std::size_t i = 0; i < indices.size(); i++)
326 assert (point_representation_->isValid (cloud [indices[i]]) &&
"Invalid (NaN, Inf) point coordinates given to radiusSearch!");
330 float* data =
new float [dim_ * indices.size ()];
331 for (std::size_t i = 0; i < indices.size (); ++i)
333 float* out = data+i*dim_;
334 point_representation_->vectorize (cloud[indices[i]], out);
336 const flann::Matrix<float> m (data, cloud.size (), point_representation_->getNumberOfDimensions ());
338 flann::SearchParams p;
339 p.sorted = sorted_results_;
343 p.max_neighbors = max_nn != 0 ? max_nn : -1;
344 index_->radiusSearch (m, k_indices, k_sqr_distances, static_cast<float> (radius * radius), p);
348 if (!identity_mapping_)
350 for (
auto &k_index : k_indices)
352 for (
auto &neighbor_index : k_index)
354 neighbor_index = index_mapping_[neighbor_index];
361 template <
typename Po
intT,
typename FlannDistance>
void 364 std::size_t original_no_of_points = indices_ && !indices_->empty () ? indices_->size () : input_->size ();
366 if (input_copied_for_flann_)
367 delete input_flann_->ptr();
368 input_copied_for_flann_ =
true;
369 index_mapping_.clear();
370 identity_mapping_ =
true;
376 if (!indices_ || indices_->empty ())
379 if (input_->is_dense && point_representation_->isTrivial ())
382 input_flann_ =
MatrixPtr (
new flann::Matrix<float> (const_cast<float*>(reinterpret_cast<const float*>(&(*input_) [0])), original_no_of_points, point_representation_->getNumberOfDimensions (),
sizeof (
PointT)));
383 input_copied_for_flann_ =
false;
387 input_flann_ =
MatrixPtr (
new flann::Matrix<float> (
new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ()));
388 float* cloud_ptr = input_flann_->ptr();
389 for (std::size_t i = 0; i < original_no_of_points; ++i)
391 const PointT& point = (*input_)[i];
393 if (!point_representation_->isValid (point))
395 identity_mapping_ =
false;
399 index_mapping_.push_back (static_cast<index_t> (i));
401 point_representation_->vectorize (point, cloud_ptr);
409 input_flann_ =
MatrixPtr (
new flann::Matrix<float> (
new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ()));
410 float* cloud_ptr = input_flann_->ptr();
411 for (std::size_t indices_index = 0; indices_index < original_no_of_points; ++indices_index)
413 index_t cloud_index = (*indices_)[indices_index];
414 const PointT& point = (*input_)[cloud_index];
416 if (!point_representation_->isValid (point))
418 identity_mapping_ =
false;
422 index_mapping_.push_back (static_cast<index_t> (indices_index));
424 point_representation_->vectorize (point, cloud_ptr);
428 if (input_copied_for_flann_)
429 input_flann_->rows = index_mapping_.size ();
432 #define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch<T>; shared_ptr< flann::NNIndex< FlannDistance > > IndexPtr
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.
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::Matrix< float > > MatrixPtr
IndexPtr index_
The FLANN index.
FlannIndexCreatorPtr creator_
The index creator, used to (re-) create the index when the search data is passed. ...
pcl::IndicesConstPtr IndicesConstPtr
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
FlannSearch(bool sorted=true, FlannIndexCreatorPtr creator=FlannIndexCreatorPtr(new KdTreeIndexCreator()))
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
bool input_copied_for_flann_
PointRepresentationConstPtr point_representation_
virtual IndexPtr createIndex(MatrixConstPtr data)
Create a FLANN Index from the input data.
typename Search< PointT >::PointCloudConstPtr PointCloudConstPtr
shared_ptr< FlannIndexCreator > FlannIndexCreatorPtr
IndicesAllocator<> Indices
Type used for indices in PCL.
shared_ptr< const flann::Matrix< float > > MatrixConstPtr
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
typename Search< PointT >::PointCloud PointCloud
~FlannSearch()
Destructor for FlannSearch.
IndexPtr createIndex(MatrixConstPtr data) override
Create a FLANN Index from the input data.
int checks_
Number of checks to perform for approximate NN search using the multiple randomized tree index...
virtual IndexPtr createIndex(MatrixConstPtr data)
Create a FLANN Index from the input data.
void convertInputToFlannMatrix()
converts the input data to a format usable by FLANN
A point structure representing Euclidean xyz coordinates, and the RGB color.
float eps_
Epsilon for approximate NN search.