Point Cloud Library (PCL)  1.13.1-dev
flann_search.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 
40 #ifndef PCL_SEARCH_IMPL_FLANN_SEARCH_H_
41 #define PCL_SEARCH_IMPL_FLANN_SEARCH_H_
42 
43 #include <flann/algorithms/kdtree_index.h>
44 #include <flann/algorithms/kdtree_single_index.h>
45 #include <flann/algorithms/kmeans_index.h>
46 
47 #include <pcl/search/flann_search.h>
48 #include <pcl/kdtree/kdtree_flann.h> // for radius_search, knn_search
49 // @TODO: remove once constexpr makes it easy to have the function in the header only
50 #include <pcl/kdtree/impl/kdtree_flann.hpp>
51 
52 //////////////////////////////////////////////////////////////////////////////////////////////
53 template <typename PointT, typename FlannDistance>
56 {
57  return (static_cast<IndexPtr> (new flann::KDTreeSingleIndex<FlannDistance> (*data,static_cast<flann::KDTreeSingleIndexParams> (max_leaf_size_))));
58 }
59 
60 //////////////////////////////////////////////////////////////////////////////////////////////
61 template <typename PointT, typename FlannDistance>
64 {
65  return (static_cast<IndexPtr> (new flann::KMeansIndex<FlannDistance> (*data,flann::KMeansIndexParams ())));
66 }
67 
68 //////////////////////////////////////////////////////////////////////////////////////////////
69 template <typename PointT, typename FlannDistance>
72 {
73  return (static_cast<IndexPtr> (new flann::KDTreeIndex<FlannDistance> (*data, static_cast<flann::KDTreeIndexParams> (trees_))));
74 }
75 
76 //////////////////////////////////////////////////////////////////////////////////////////////
77 template <typename PointT, typename FlannDistance>
80  dim_ (0), identity_mapping_()
81 {
82  dim_ = point_representation_->getNumberOfDimensions ();
83 }
84 
85 //////////////////////////////////////////////////////////////////////////////////////////////
86 template <typename PointT, typename FlannDistance>
88 {
89  if (input_copied_for_flann_)
90  delete [] input_flann_->ptr();
91 }
92 
93 //////////////////////////////////////////////////////////////////////////////////////////////
94 template <typename PointT, typename FlannDistance> void
96 {
97  input_ = cloud;
98  indices_ = indices;
99  convertInputToFlannMatrix ();
100  index_ = creator_->createIndex (input_flann_);
101  index_->buildIndex ();
102 }
103 
104 //////////////////////////////////////////////////////////////////////////////////////////////
105 template <typename PointT, typename FlannDistance> int
106 pcl::search::FlannSearch<PointT, FlannDistance>::nearestKSearch (const PointT &point, int k, Indices &indices, std::vector<float> &dists) const
107 {
108  assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); // remove this check as soon as FLANN does NaN checks internally
109  bool can_cast = point_representation_->isTrivial ();
110 
111  float* data = nullptr;
112  if (!can_cast)
113  {
114  data = new float [point_representation_->getNumberOfDimensions ()];
115  point_representation_->vectorize (point,data);
116  }
117 
118  float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&point)): data;
119  const flann::Matrix<float> m (cdata ,1, point_representation_->getNumberOfDimensions ());
120 
121  flann::SearchParams p;
122  p.eps = eps_;
123  p.sorted = sorted_results_;
124  p.checks = checks_;
125  if (indices.size() != static_cast<unsigned int> (k))
126  indices.resize (k,-1);
127  if (dists.size() != static_cast<unsigned int> (k))
128  dists.resize (k);
129  flann::Matrix<float> d (dists.data(),1,k);
130  int result = knn_search(*index_, m, indices, d, k, p);
131 
132  delete [] data;
133 
134  if (!identity_mapping_)
135  {
136  for (std::size_t i = 0; i < static_cast<unsigned int> (k); ++i)
137  {
138  auto& neighbor_index = indices[i];
139  neighbor_index = index_mapping_[neighbor_index];
140  }
141  }
142  return result;
143 }
144 
145 //////////////////////////////////////////////////////////////////////////////////////////////
146 template <typename PointT, 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
150 {
151  if (indices.empty ())
152  {
153  k_indices.resize (cloud.size ());
154  k_sqr_distances.resize (cloud.size ());
155 
156  if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
157  {
158  for (std::size_t i = 0; i < cloud.size(); i++)
159  {
160  assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
161  }
162  }
163 
164  bool can_cast = point_representation_->isTrivial ();
165 
166  // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix!
167  float* data=nullptr;
168  if (!can_cast)
169  {
170  data = new float[dim_*cloud.size ()];
171  for (std::size_t i = 0; i < cloud.size (); ++i)
172  {
173  float* out = data+i*dim_;
174  point_representation_->vectorize (cloud[i],out);
175  }
176  }
177 
178  // const cast is evil, but the matrix constructor won't change the data, and the
179  // search won't change the matrix
180  float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&cloud[0])): data;
181  const flann::Matrix<float> m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float) );
182 
183  flann::SearchParams p;
184  p.sorted = sorted_results_;
185  p.eps = eps_;
186  p.checks = checks_;
187  knn_search(*index_, m, k_indices, k_sqr_distances, k, p);
188 
189  delete [] data;
190  }
191  else // if indices are present, the cloud has to be copied anyway. Only copy the relevant parts of the points here.
192  {
193  k_indices.resize (indices.size ());
194  k_sqr_distances.resize (indices.size ());
195 
196  if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
197  {
198  for (std::size_t i = 0; i < indices.size(); i++)
199  {
200  assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
201  }
202  }
203 
204  float* data=new float [dim_*indices.size ()];
205  for (std::size_t i = 0; i < indices.size (); ++i)
206  {
207  float* out = data+i*dim_;
208  point_representation_->vectorize (cloud[indices[i]],out);
209  }
210  const flann::Matrix<float> m (data ,indices.size (), point_representation_->getNumberOfDimensions ());
211 
212  flann::SearchParams p;
213  p.sorted = sorted_results_;
214  p.eps = eps_;
215  p.checks = checks_;
216  knn_search(*index_, m, k_indices, k_sqr_distances, k, p);
217 
218  delete[] data;
219  }
220  if (!identity_mapping_)
221  {
222  for (auto &k_index : k_indices)
223  {
224  for (auto &neighbor_index : k_index)
225  {
226  neighbor_index = index_mapping_[neighbor_index];
227  }
228  }
229  }
230 }
231 
232 //////////////////////////////////////////////////////////////////////////////////////////////
233 template <typename PointT, typename FlannDistance> int
235  Indices &indices, std::vector<float> &distances,
236  unsigned int max_nn) const
237 {
238  assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); // remove this check as soon as FLANN does NaN checks internally
239  bool can_cast = point_representation_->isTrivial ();
240 
241  float* data = nullptr;
242  if (!can_cast)
243  {
244  data = new float [point_representation_->getNumberOfDimensions ()];
245  point_representation_->vectorize (point,data);
246  }
247 
248  float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&point)) : data;
249  const flann::Matrix<float> m (cdata ,1, point_representation_->getNumberOfDimensions ());
250 
251  flann::SearchParams p;
252  p.sorted = sorted_results_;
253  p.eps = eps_;
254  p.max_neighbors = max_nn > 0 ? max_nn : -1;
255  p.checks = checks_;
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);
259 
260  delete [] data;
261  indices = i [0];
262  distances = d [0];
263 
264  if (!identity_mapping_)
265  {
266  for (auto &neighbor_index : indices)
267  {
268  neighbor_index = index_mapping_ [neighbor_index];
269  }
270  }
271  return result;
272 }
273 
274 //////////////////////////////////////////////////////////////////////////////////////////////
275 template <typename PointT, 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
279 {
280  if (indices.empty ()) // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix!
281  {
282  k_indices.resize (cloud.size ());
283  k_sqr_distances.resize (cloud.size ());
284 
285  if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
286  {
287  for (std::size_t i = 0; i < cloud.size(); i++)
288  {
289  assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
290  }
291  }
292 
293  bool can_cast = point_representation_->isTrivial ();
294 
295  float* data = nullptr;
296  if (!can_cast)
297  {
298  data = new float[dim_*cloud.size ()];
299  for (std::size_t i = 0; i < cloud.size (); ++i)
300  {
301  float* out = data+i*dim_;
302  point_representation_->vectorize (cloud[i],out);
303  }
304  }
305 
306  float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&cloud[0])) : data;
307  const flann::Matrix<float> m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float));
308 
309  flann::SearchParams p;
310  p.sorted = sorted_results_;
311  p.eps = eps_;
312  p.checks = checks_;
313  // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors
314  p.max_neighbors = max_nn != 0 ? max_nn : -1;
316  *index_, m, k_indices, k_sqr_distances, static_cast<float>(radius * radius), p);
317 
318  delete [] data;
319  }
320  else // if indices are present, the cloud has to be copied anyway. Only copy the relevant parts of the points here.
321  {
322  k_indices.resize (indices.size ());
323  k_sqr_distances.resize (indices.size ());
324 
325  if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
326  {
327  for (std::size_t i = 0; i < indices.size(); i++)
328  {
329  assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
330  }
331  }
332 
333  float* data = new float [dim_ * indices.size ()];
334  for (std::size_t i = 0; i < indices.size (); ++i)
335  {
336  float* out = data+i*dim_;
337  point_representation_->vectorize (cloud[indices[i]], out);
338  }
339  const flann::Matrix<float> m (data, cloud.size (), point_representation_->getNumberOfDimensions ());
340 
341  flann::SearchParams p;
342  p.sorted = sorted_results_;
343  p.eps = eps_;
344  p.checks = checks_;
345  // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors
346  p.max_neighbors = max_nn != 0 ? max_nn : -1;
348  *index_, m, k_indices, k_sqr_distances, static_cast<float>(radius * radius), p);
349 
350  delete[] data;
351  }
352  if (!identity_mapping_)
353  {
354  for (auto &k_index : k_indices)
355  {
356  for (auto &neighbor_index : k_index)
357  {
358  neighbor_index = index_mapping_[neighbor_index];
359  }
360  }
361  }
362 }
363 
364 //////////////////////////////////////////////////////////////////////////////////////////////
365 template <typename PointT, typename FlannDistance> void
367 {
368  std::size_t original_no_of_points = indices_ && !indices_->empty () ? indices_->size () : input_->size ();
369 
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;
375 
376  //cloud_ = (float*)malloc (original_no_of_points * dim_ * sizeof (float));
377  //index_mapping_.reserve(original_no_of_points);
378  //identity_mapping_ = true;
379 
380  if (!indices_ || indices_->empty ())
381  {
382  // best case: all points can be passed to flann without any conversions
383  if (input_->is_dense && point_representation_->isTrivial ())
384  {
385  // const cast is evil, but flann won't change the data
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;
388  }
389  else
390  {
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)
394  {
395  const PointT& point = (*input_)[i];
396  // Check if the point is invalid
397  if (!point_representation_->isValid (point))
398  {
399  identity_mapping_ = false;
400  continue;
401  }
402 
403  index_mapping_.push_back (static_cast<index_t> (i)); // If the returned index should be for the indices vector
404 
405  point_representation_->vectorize (point, cloud_ptr);
406  cloud_ptr += dim_;
407  }
408  }
409 
410  }
411  else
412  {
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)
416  {
417  index_t cloud_index = (*indices_)[indices_index];
418  const PointT& point = (*input_)[cloud_index];
419  // Check if the point is invalid
420  if (!point_representation_->isValid (point))
421  {
422  identity_mapping_ = false;
423  continue;
424  }
425 
426  index_mapping_.push_back (static_cast<index_t> (indices_index)); // If the returned index should be for the indices vector
427 
428  point_representation_->vectorize (point, cloud_ptr);
429  cloud_ptr += dim_;
430  }
431  }
432  if (input_copied_for_flann_)
433  input_flann_->rows = index_mapping_.size ();
434 }
435 
436 #define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch<T>;
437 
438 #endif
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
Definition: flann_search.h:117
int checks_
Number of checks to perform for approximate NN search using the multiple randomized tree index.
Definition: flann_search.h:355
shared_ptr< const flann::Matrix< float > > MatrixConstPtr
Definition: flann_search.h:114
FlannSearch(bool sorted=true, FlannIndexCreatorPtr creator=FlannIndexCreatorPtr(new KdTreeIndexCreator()))
~FlannSearch() override
Destructor for FlannSearch.
PointRepresentationConstPtr point_representation_
Definition: flann_search.h:359
shared_ptr< FlannIndexCreator > FlannIndexCreatorPtr
Definition: flann_search.h:141
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
Definition: flann_search.h:111
typename Search< PointT >::PointCloud PointCloud
Definition: flann_search.h:110
FlannIndexCreatorPtr creator_
The index creator, used to (re-) create the index when the search data is passed.
Definition: flann_search.h:343
IndexPtr index_
The FLANN index.
Definition: flann_search.h:339
float eps_
Epsilon for approximate NN search.
Definition: flann_search.h:351
shared_ptr< flann::Matrix< float > > MatrixPtr
Definition: flann_search.h:113
Generic search class.
Definition: search.h:75
pcl::IndicesConstPtr IndicesConstPtr
Definition: search.h:85
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
int radius_search(const FlannIndex &index, const Query &query, Indices &indices, Distances &dists, float radius, const SearchParams &params)
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 &params)
Comaptibility template function to allow use of various types of indices with FLANN.
A point structure representing Euclidean xyz coordinates, and the RGB color.