Point Cloud Library (PCL)  1.14.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 {
81  dim_ = point_representation_->getNumberOfDimensions ();
82 }
83 
84 //////////////////////////////////////////////////////////////////////////////////////////////
85 template <typename PointT, typename FlannDistance>
87 {
88  if (input_copied_for_flann_)
89  delete [] input_flann_->ptr();
90 }
91 
92 //////////////////////////////////////////////////////////////////////////////////////////////
93 template <typename PointT, typename FlannDistance> bool
95 {
96  input_ = cloud;
97  indices_ = indices;
98  convertInputToFlannMatrix ();
99  index_ = creator_->createIndex (input_flann_);
100  index_->buildIndex ();
101  return true;
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  if (static_cast<unsigned int>(k) > total_nr_points_)
122  k = total_nr_points_;
123 
124  flann::SearchParams p;
125  p.eps = eps_;
126  p.sorted = sorted_results_;
127  p.checks = checks_;
128  if (indices.size() != static_cast<unsigned int> (k))
129  indices.resize (k,-1);
130  if (dists.size() != static_cast<unsigned int> (k))
131  dists.resize (k);
132  flann::Matrix<float> d (dists.data(),1,k);
133  int result = knn_search(*index_, m, indices, d, k, p);
134 
135  delete [] data;
136 
137  if (!identity_mapping_)
138  {
139  for (std::size_t i = 0; i < static_cast<unsigned int> (k); ++i)
140  {
141  auto& neighbor_index = indices[i];
142  neighbor_index = index_mapping_[neighbor_index];
143  }
144  }
145  return result;
146 }
147 
148 //////////////////////////////////////////////////////////////////////////////////////////////
149 template <typename PointT, typename FlannDistance> void
151  const PointCloud& cloud, const Indices& indices, int k, std::vector<Indices>& k_indices,
152  std::vector< std::vector<float> >& k_sqr_distances) const
153 {
154  if (indices.empty ())
155  {
156  k_indices.resize (cloud.size ());
157  k_sqr_distances.resize (cloud.size ());
158 
159  if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
160  {
161  for (std::size_t i = 0; i < cloud.size(); i++)
162  {
163  assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
164  }
165  }
166 
167  bool can_cast = point_representation_->isTrivial ();
168 
169  // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix!
170  float* data=nullptr;
171  if (!can_cast)
172  {
173  data = new float[dim_*cloud.size ()];
174  for (std::size_t i = 0; i < cloud.size (); ++i)
175  {
176  float* out = data+i*dim_;
177  point_representation_->vectorize (cloud[i],out);
178  }
179  }
180 
181  // const cast is evil, but the matrix constructor won't change the data, and the
182  // search won't change the matrix
183  float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&cloud[0])): data;
184  const flann::Matrix<float> m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float) );
185 
186  if (static_cast<unsigned int>(k) > total_nr_points_)
187  k = total_nr_points_;
188 
189  flann::SearchParams p;
190  p.sorted = sorted_results_;
191  p.eps = eps_;
192  p.checks = checks_;
193  knn_search(*index_, m, k_indices, k_sqr_distances, k, p);
194 
195  delete [] data;
196  }
197  else // if indices are present, the cloud has to be copied anyway. Only copy the relevant parts of the points here.
198  {
199  k_indices.resize (indices.size ());
200  k_sqr_distances.resize (indices.size ());
201 
202  if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
203  {
204  for (std::size_t i = 0; i < indices.size(); i++)
205  {
206  assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
207  }
208  }
209 
210  float* data=new float [dim_*indices.size ()];
211  for (std::size_t i = 0; i < indices.size (); ++i)
212  {
213  float* out = data+i*dim_;
214  point_representation_->vectorize (cloud[indices[i]],out);
215  }
216  const flann::Matrix<float> m (data ,indices.size (), point_representation_->getNumberOfDimensions ());
217 
218  flann::SearchParams p;
219  p.sorted = sorted_results_;
220  p.eps = eps_;
221  p.checks = checks_;
222  knn_search(*index_, m, k_indices, k_sqr_distances, k, p);
223 
224  delete[] data;
225  }
226  if (!identity_mapping_)
227  {
228  for (auto &k_index : k_indices)
229  {
230  for (auto &neighbor_index : k_index)
231  {
232  neighbor_index = index_mapping_[neighbor_index];
233  }
234  }
235  }
236 }
237 
238 //////////////////////////////////////////////////////////////////////////////////////////////
239 template <typename PointT, typename FlannDistance> int
241  Indices &indices, std::vector<float> &distances,
242  unsigned int max_nn) const
243 {
244  assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); // remove this check as soon as FLANN does NaN checks internally
245  bool can_cast = point_representation_->isTrivial ();
246 
247  float* data = nullptr;
248  if (!can_cast)
249  {
250  data = new float [point_representation_->getNumberOfDimensions ()];
251  point_representation_->vectorize (point,data);
252  }
253 
254  float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&point)) : data;
255  const flann::Matrix<float> m (cdata ,1, point_representation_->getNumberOfDimensions ());
256 
257  flann::SearchParams p;
258  p.sorted = sorted_results_;
259  p.eps = eps_;
260  p.max_neighbors = max_nn > 0 ? max_nn : -1;
261  p.checks = checks_;
262  std::vector<Indices> i (1);
263  std::vector<std::vector<float> > d (1);
264  int result = radius_search(*index_, m, i, d, static_cast<float>(radius * radius), p);
265 
266  delete [] data;
267  indices = i [0];
268  distances = d [0];
269 
270  if (!identity_mapping_)
271  {
272  for (auto &neighbor_index : indices)
273  {
274  neighbor_index = index_mapping_ [neighbor_index];
275  }
276  }
277  return result;
278 }
279 
280 //////////////////////////////////////////////////////////////////////////////////////////////
281 template <typename PointT, typename FlannDistance> void
283  const PointCloud& cloud, const Indices& indices, double radius, std::vector<Indices>& k_indices,
284  std::vector< std::vector<float> >& k_sqr_distances, unsigned int max_nn) const
285 {
286  if (indices.empty ()) // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix!
287  {
288  k_indices.resize (cloud.size ());
289  k_sqr_distances.resize (cloud.size ());
290 
291  if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
292  {
293  for (std::size_t i = 0; i < cloud.size(); i++)
294  {
295  assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
296  }
297  }
298 
299  bool can_cast = point_representation_->isTrivial ();
300 
301  float* data = nullptr;
302  if (!can_cast)
303  {
304  data = new float[dim_*cloud.size ()];
305  for (std::size_t i = 0; i < cloud.size (); ++i)
306  {
307  float* out = data+i*dim_;
308  point_representation_->vectorize (cloud[i],out);
309  }
310  }
311 
312  float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&cloud[0])) : data;
313  const flann::Matrix<float> m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float));
314 
315  flann::SearchParams p;
316  p.sorted = sorted_results_;
317  p.eps = eps_;
318  p.checks = checks_;
319  // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors
320  p.max_neighbors = max_nn != 0 ? max_nn : -1;
322  *index_, m, k_indices, k_sqr_distances, static_cast<float>(radius * radius), p);
323 
324  delete [] data;
325  }
326  else // if indices are present, the cloud has to be copied anyway. Only copy the relevant parts of the points here.
327  {
328  k_indices.resize (indices.size ());
329  k_sqr_distances.resize (indices.size ());
330 
331  if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
332  {
333  for (std::size_t i = 0; i < indices.size(); i++)
334  {
335  assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
336  }
337  }
338 
339  float* data = new float [dim_ * indices.size ()];
340  for (std::size_t i = 0; i < indices.size (); ++i)
341  {
342  float* out = data+i*dim_;
343  point_representation_->vectorize (cloud[indices[i]], out);
344  }
345  const flann::Matrix<float> m (data, cloud.size (), point_representation_->getNumberOfDimensions ());
346 
347  flann::SearchParams p;
348  p.sorted = sorted_results_;
349  p.eps = eps_;
350  p.checks = checks_;
351  // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors
352  p.max_neighbors = max_nn != 0 ? max_nn : -1;
354  *index_, m, k_indices, k_sqr_distances, static_cast<float>(radius * radius), p);
355 
356  delete[] data;
357  }
358  if (!identity_mapping_)
359  {
360  for (auto &k_index : k_indices)
361  {
362  for (auto &neighbor_index : k_index)
363  {
364  neighbor_index = index_mapping_[neighbor_index];
365  }
366  }
367  }
368 }
369 
370 //////////////////////////////////////////////////////////////////////////////////////////////
371 template <typename PointT, typename FlannDistance> void
373 {
374  std::size_t original_no_of_points = indices_ && !indices_->empty () ? indices_->size () : input_->size ();
375 
376  if (input_copied_for_flann_)
377  delete input_flann_->ptr();
378  input_copied_for_flann_ = true;
379  index_mapping_.clear();
380  identity_mapping_ = true;
381 
382  //cloud_ = (float*)malloc (original_no_of_points * dim_ * sizeof (float));
383  //index_mapping_.reserve(original_no_of_points);
384  //identity_mapping_ = true;
385 
386  if (!indices_ || indices_->empty ())
387  {
388  // best case: all points can be passed to flann without any conversions
389  if (input_->is_dense && point_representation_->isTrivial ())
390  {
391  // const cast is evil, but flann won't change the data
392  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)));
393  input_copied_for_flann_ = false;
394  total_nr_points_ = input_->points.size();
395  }
396  else
397  {
398  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 ()));
399  float* cloud_ptr = input_flann_->ptr();
400  for (std::size_t i = 0; i < original_no_of_points; ++i)
401  {
402  const PointT& point = (*input_)[i];
403  // Check if the point is invalid
404  if (!point_representation_->isValid (point))
405  {
406  identity_mapping_ = false;
407  continue;
408  }
409 
410  index_mapping_.push_back (static_cast<index_t> (i));
411 
412  point_representation_->vectorize (point, cloud_ptr);
413  cloud_ptr += dim_;
414  }
415  total_nr_points_ = index_mapping_.size();
416  }
417 
418  }
419  else
420  {
421  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 ()));
422  float* cloud_ptr = input_flann_->ptr();
423  identity_mapping_ = false;
424  for (std::size_t indices_index = 0; indices_index < original_no_of_points; ++indices_index)
425  {
426  index_t cloud_index = (*indices_)[indices_index];
427  const PointT& point = (*input_)[cloud_index];
428  // Check if the point is invalid
429  if (!point_representation_->isValid (point))
430  {
431  continue;
432  }
433 
434  index_mapping_.push_back (static_cast<index_t> (cloud_index));
435 
436  point_representation_->vectorize (point, cloud_ptr);
437  cloud_ptr += dim_;
438  }
439  total_nr_points_ = index_mapping_.size();
440  }
441  if (input_copied_for_flann_)
442  input_flann_->rows = index_mapping_.size ();
443 }
444 
445 #define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch<T>;
446 
447 #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 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
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.
bool setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
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
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)
Compatibility 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)
Compatibility template function to allow use of various types of indices with FLANN.
A point structure representing Euclidean xyz coordinates, and the RGB color.