Point Cloud Library (PCL)  1.9.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 
49 //////////////////////////////////////////////////////////////////////////////////////////////
50 template <typename PointT, typename FlannDistance>
53 {
54  return (IndexPtr (new flann::KDTreeSingleIndex<FlannDistance> (*data,flann::KDTreeSingleIndexParams (max_leaf_size_))));
55 }
56 
57 //////////////////////////////////////////////////////////////////////////////////////////////
58 template <typename PointT, typename FlannDistance>
61 {
62  return (IndexPtr (new flann::KMeansIndex<FlannDistance> (*data,flann::KMeansIndexParams ())));
63 }
64 
65 //////////////////////////////////////////////////////////////////////////////////////////////
66 template <typename PointT, typename FlannDistance>
69 {
70  return (IndexPtr (new flann::KDTreeIndex<FlannDistance> (*data, flann::KDTreeIndexParams (trees_))));
71 }
72 
73 //////////////////////////////////////////////////////////////////////////////////////////////
74 template <typename PointT, typename FlannDistance>
77  dim_ (0), identity_mapping_()
78 {
79  dim_ = point_representation_->getNumberOfDimensions ();
80 }
81 
82 //////////////////////////////////////////////////////////////////////////////////////////////
83 template <typename PointT, typename FlannDistance>
85 {
87  delete [] input_flann_->ptr();
88 }
89 
90 //////////////////////////////////////////////////////////////////////////////////////////////
91 template <typename PointT, typename FlannDistance> void
93 {
94  input_ = cloud;
95  indices_ = indices;
97  index_ = creator_->createIndex (input_flann_);
98  index_->buildIndex ();
99 }
100 
101 //////////////////////////////////////////////////////////////////////////////////////////////
102 template <typename PointT, typename FlannDistance> int
103 pcl::search::FlannSearch<PointT, FlannDistance>::nearestKSearch (const PointT &point, int k, std::vector<int> &indices, std::vector<float> &dists) const
104 {
105  assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); // remove this check as soon as FLANN does NaN checks internally
106  bool can_cast = point_representation_->isTrivial ();
107 
108  float* data = nullptr;
109  if (!can_cast)
110  {
111  data = new float [point_representation_->getNumberOfDimensions ()];
112  point_representation_->vectorize (point,data);
113  }
114 
115  float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&point)): data;
116  const flann::Matrix<float> m (cdata ,1, point_representation_->getNumberOfDimensions ());
117 
118  flann::SearchParams p;
119  p.eps = eps_;
120  p.sorted = sorted_results_;
121  p.checks = checks_;
122  if (indices.size() != static_cast<unsigned int> (k))
123  indices.resize (k,-1);
124  if (dists.size() != static_cast<unsigned int> (k))
125  dists.resize (k);
126  flann::Matrix<int> i (&indices[0],1,k);
127  flann::Matrix<float> d (&dists[0],1,k);
128  int result = index_->knnSearch (m,i,d,k, p);
129 
130  delete [] data;
131 
132  if (!identity_mapping_)
133  {
134  for (size_t i = 0; i < static_cast<unsigned int> (k); ++i)
135  {
136  int& neighbor_index = indices[i];
137  neighbor_index = index_mapping_[neighbor_index];
138  }
139  }
140  return result;
141 }
142 
143 //////////////////////////////////////////////////////////////////////////////////////////////
144 template <typename PointT, typename FlannDistance> void
146  const PointCloud& cloud, const std::vector<int>& indices, int k, std::vector< std::vector<int> >& k_indices,
147  std::vector< std::vector<float> >& k_sqr_distances) const
148 {
149  if (indices.empty ())
150  {
151  k_indices.resize (cloud.size ());
152  k_sqr_distances.resize (cloud.size ());
153 
154  if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
155  {
156  for (size_t i = 0; i < cloud.size(); i++)
157  {
158  assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
159  }
160  }
161 
162  bool can_cast = point_representation_->isTrivial ();
163 
164  // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix!
165  float* data=nullptr;
166  if (!can_cast)
167  {
168  data = new float[dim_*cloud.size ()];
169  for (size_t i = 0; i < cloud.size (); ++i)
170  {
171  float* out = data+i*dim_;
172  point_representation_->vectorize (cloud[i],out);
173  }
174  }
175 
176  // const cast is evil, but the matrix constructor won't change the data, and the
177  // search won't change the matrix
178  float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&cloud[0])): data;
179  const flann::Matrix<float> m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float) );
180 
181  flann::SearchParams p;
182  p.sorted = sorted_results_;
183  p.eps = eps_;
184  p.checks = checks_;
185  index_->knnSearch (m,k_indices,k_sqr_distances,k, p);
186 
187  delete [] data;
188  }
189  else // if indices are present, the cloud has to be copied anyway. Only copy the relevant parts of the points here.
190  {
191  k_indices.resize (indices.size ());
192  k_sqr_distances.resize (indices.size ());
193 
194  if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
195  {
196  for (size_t i = 0; i < indices.size(); i++)
197  {
198  assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
199  }
200  }
201 
202  float* data=new float [dim_*indices.size ()];
203  for (size_t i = 0; i < indices.size (); ++i)
204  {
205  float* out = data+i*dim_;
206  point_representation_->vectorize (cloud[indices[i]],out);
207  }
208  const flann::Matrix<float> m (data ,indices.size (), point_representation_->getNumberOfDimensions ());
209 
210  flann::SearchParams p;
211  p.sorted = sorted_results_;
212  p.eps = eps_;
213  p.checks = checks_;
214  index_->knnSearch (m,k_indices,k_sqr_distances,k, p);
215 
216  delete[] data;
217  }
218  if (!identity_mapping_)
219  {
220  for (auto &k_index : k_indices)
221  {
222  for (int &neighbor_index : k_index)
223  {
224  neighbor_index = index_mapping_[neighbor_index];
225  }
226  }
227  }
228 }
229 
230 //////////////////////////////////////////////////////////////////////////////////////////////
231 template <typename PointT, typename FlannDistance> int
233  std::vector<int> &indices, std::vector<float> &distances,
234  unsigned int max_nn) const
235 {
236  assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); // remove this check as soon as FLANN does NaN checks internally
237  bool can_cast = point_representation_->isTrivial ();
238 
239  float* data = nullptr;
240  if (!can_cast)
241  {
242  data = new float [point_representation_->getNumberOfDimensions ()];
243  point_representation_->vectorize (point,data);
244  }
245 
246  float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&point)) : data;
247  const flann::Matrix<float> m (cdata ,1, point_representation_->getNumberOfDimensions ());
248 
249  flann::SearchParams p;
250  p.sorted = sorted_results_;
251  p.eps = eps_;
252  p.max_neighbors = max_nn > 0 ? max_nn : -1;
253  p.checks = checks_;
254  std::vector<std::vector<int> > i (1);
255  std::vector<std::vector<float> > d (1);
256  int result = index_->radiusSearch (m,i,d,static_cast<float> (radius * radius), p);
257 
258  delete [] data;
259  indices = i [0];
260  distances = d [0];
261 
262  if (!identity_mapping_)
263  {
264  for (int &neighbor_index : indices)
265  {
266  neighbor_index = index_mapping_ [neighbor_index];
267  }
268  }
269  return result;
270 }
271 
272 //////////////////////////////////////////////////////////////////////////////////////////////
273 template <typename PointT, typename FlannDistance> void
275  const PointCloud& cloud, const std::vector<int>& indices, double radius, std::vector< std::vector<int> >& k_indices,
276  std::vector< std::vector<float> >& k_sqr_distances, unsigned int max_nn) const
277 {
278  if (indices.empty ()) // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix!
279  {
280  k_indices.resize (cloud.size ());
281  k_sqr_distances.resize (cloud.size ());
282 
283  if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
284  {
285  for (size_t i = 0; i < cloud.size(); i++)
286  {
287  assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
288  }
289  }
290 
291  bool can_cast = point_representation_->isTrivial ();
292 
293  float* data = nullptr;
294  if (!can_cast)
295  {
296  data = new float[dim_*cloud.size ()];
297  for (size_t i = 0; i < cloud.size (); ++i)
298  {
299  float* out = data+i*dim_;
300  point_representation_->vectorize (cloud[i],out);
301  }
302  }
303 
304  float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&cloud[0])) : data;
305  const flann::Matrix<float> m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float));
306 
307  flann::SearchParams p;
308  p.sorted = sorted_results_;
309  p.eps = eps_;
310  p.checks = checks_;
311  // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors
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);
314 
315  delete [] data;
316  }
317  else // if indices are present, the cloud has to be copied anyway. Only copy the relevant parts of the points here.
318  {
319  k_indices.resize (indices.size ());
320  k_sqr_distances.resize (indices.size ());
321 
322  if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
323  {
324  for (size_t i = 0; i < indices.size(); i++)
325  {
326  assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
327  }
328  }
329 
330  float* data = new float [dim_ * indices.size ()];
331  for (size_t i = 0; i < indices.size (); ++i)
332  {
333  float* out = data+i*dim_;
334  point_representation_->vectorize (cloud[indices[i]], out);
335  }
336  const flann::Matrix<float> m (data, cloud.size (), point_representation_->getNumberOfDimensions ());
337 
338  flann::SearchParams p;
339  p.sorted = sorted_results_;
340  p.eps = eps_;
341  p.checks = checks_;
342  // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors
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);
345 
346  delete[] data;
347  }
348  if (!identity_mapping_)
349  {
350  for (auto &k_index : k_indices)
351  {
352  for (int &neighbor_index : k_index)
353  {
354  neighbor_index = index_mapping_[neighbor_index];
355  }
356  }
357  }
358 }
359 
360 //////////////////////////////////////////////////////////////////////////////////////////////
361 template <typename PointT, typename FlannDistance> void
363 {
364  size_t original_no_of_points = indices_ && !indices_->empty () ? indices_->size () : input_->size ();
365 
367  delete input_flann_->ptr();
369  index_mapping_.clear();
370  identity_mapping_ = true;
371 
372  //cloud_ = (float*)malloc (original_no_of_points * dim_ * sizeof (float));
373  //index_mapping_.reserve(original_no_of_points);
374  //identity_mapping_ = true;
375 
376  if (!indices_ || indices_->empty ())
377  {
378  // best case: all points can be passed to flann without any conversions
379  if (input_->is_dense && point_representation_->isTrivial ())
380  {
381  // const cast is evil, but flann won't change the data
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;
384  }
385  else
386  {
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 (size_t i = 0; i < original_no_of_points; ++i)
390  {
391  const PointT& point = (*input_)[i];
392  // Check if the point is invalid
393  if (!point_representation_->isValid (point))
394  {
395  identity_mapping_ = false;
396  continue;
397  }
398 
399  index_mapping_.push_back (static_cast<int> (i)); // If the returned index should be for the indices vector
400 
401  point_representation_->vectorize (point, cloud_ptr);
402  cloud_ptr += dim_;
403  }
404  }
405 
406  }
407  else
408  {
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 (size_t indices_index = 0; indices_index < original_no_of_points; ++indices_index)
412  {
413  int cloud_index = (*indices_)[indices_index];
414  const PointT& point = (*input_)[cloud_index];
415  // Check if the point is invalid
416  if (!point_representation_->isValid (point))
417  {
418  identity_mapping_ = false;
419  continue;
420  }
421 
422  index_mapping_.push_back (static_cast<int> (indices_index)); // If the returned index should be for the indices vector
423 
424  point_representation_->vectorize (point, cloud_ptr);
425  cloud_ptr += dim_;
426  }
427  }
429  input_flann_->rows = index_mapping_.size ();
430 }
431 
432 #define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch<T>;
433 
434 #endif
size_t size() const
Definition: point_cloud.h:447
boost::shared_ptr< flann::Matrix< float > > MatrixPtr
Definition: flann_search.h:116
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:44
std::vector< int > index_mapping_
Definition: flann_search.h:364
IndexPtr index_
The FLANN index.
Definition: flann_search.h:340
FlannIndexCreatorPtr creator_
The index creator, used to (re-) create the index when the search data is passed. ...
Definition: flann_search.h:344
PointCloudConstPtr input_
Definition: search.h:402
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.
Search< PointT >::PointCloudConstPtr PointCloudConstPtr
Definition: flann_search.h:111
PointRepresentationConstPtr point_representation_
Definition: flann_search.h:360
virtual IndexPtr createIndex(MatrixConstPtr data)
Create a FLANN Index from the input data.
IndicesConstPtr indices_
Definition: search.h:403
boost::shared_ptr< const flann::Matrix< float > > MatrixConstPtr
Definition: flann_search.h:117
bool sorted_results_
Definition: search.h:404
PointCloud represents the base class in PCL for storing collections of 3D points. ...
int radiusSearch(const PointT &point, double radius, std::vector< int > &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.
MatrixPtr input_flann_
Input data in FLANN format.
Definition: flann_search.h:348
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
~FlannSearch()
Destructor for FlannSearch.
IndexPtr createIndex(MatrixConstPtr data) override
Create a FLANN Index from the input data.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Definition: point_cloud.h:417
int checks_
Number of checks to perform for approximate NN search using the multiple randomized tree index...
Definition: flann_search.h:356
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.
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
Definition: flann_search.h:114
boost::shared_ptr< FlannIndexCreator > FlannIndexCreatorPtr
Definition: flann_search.h:144
boost::shared_ptr< flann::NNIndex< FlannDistance > > IndexPtr
Definition: flann_search.h:120
int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const override
Search for the k-nearest neighbors for the given query point.
float eps_
Epsilon for approximate NN search.
Definition: flann_search.h:352
Generic search class.
Definition: search.h:73