Point Cloud Library (PCL)  1.8.1
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 <pcl/search/flann_search.h>
44 #include <pcl/kdtree/flann.h>
45 
46 //////////////////////////////////////////////////////////////////////////////////////////////
47 template <typename PointT, typename FlannDistance>
50 {
51  return (IndexPtr (new flann::KDTreeSingleIndex<FlannDistance> (*data,flann::KDTreeSingleIndexParams (max_leaf_size_))));
52 }
53 
54 //////////////////////////////////////////////////////////////////////////////////////////////
55 template <typename PointT, typename FlannDistance>
58 {
59  return (IndexPtr (new flann::KMeansIndex<FlannDistance> (*data,flann::KMeansIndexParams ())));
60 }
61 
62 //////////////////////////////////////////////////////////////////////////////////////////////
63 template <typename PointT, typename FlannDistance>
66 {
67  return (IndexPtr (new flann::KDTreeIndex<FlannDistance> (*data, flann::KDTreeIndexParams (trees_))));
68 }
69 
70 //////////////////////////////////////////////////////////////////////////////////////////////
71 template <typename PointT, typename FlannDistance>
75 {
76  dim_ = point_representation_->getNumberOfDimensions ();
77 }
78 
79 //////////////////////////////////////////////////////////////////////////////////////////////
80 template <typename PointT, typename FlannDistance>
82 {
83  if (input_copied_for_flann_)
84  delete [] input_flann_->ptr();
85 }
86 
87 //////////////////////////////////////////////////////////////////////////////////////////////
88 template <typename PointT, typename FlannDistance> void
90 {
91  input_ = cloud;
92  indices_ = indices;
93  convertInputToFlannMatrix ();
94  index_ = creator_->createIndex (input_flann_);
95  index_->buildIndex ();
96 }
97 
98 //////////////////////////////////////////////////////////////////////////////////////////////
99 template <typename PointT, typename FlannDistance> int
100 pcl::search::FlannSearch<PointT, FlannDistance>::nearestKSearch (const PointT &point, int k, std::vector<int> &indices, std::vector<float> &dists) const
101 {
102  assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); // remove this check as soon as FLANN does NaN checks internally
103  bool can_cast = point_representation_->isTrivial ();
104 
105  float* data = 0;
106  if (!can_cast)
107  {
108  data = new float [point_representation_->getNumberOfDimensions ()];
109  point_representation_->vectorize (point,data);
110  }
111 
112  float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&point)): data;
113  const flann::Matrix<float> m (cdata ,1, point_representation_->getNumberOfDimensions ());
114 
115  flann::SearchParams p;
116  p.eps = eps_;
117  p.sorted = sorted_results_;
118  p.checks = checks_;
119  if (indices.size() != static_cast<unsigned int> (k))
120  indices.resize (k,-1);
121  if (dists.size() != static_cast<unsigned int> (k))
122  dists.resize (k);
123  flann::Matrix<int> i (&indices[0],1,k);
124  flann::Matrix<float> d (&dists[0],1,k);
125  int result = index_->knnSearch (m,i,d,k, p);
126 
127  delete [] data;
128 
129  if (!identity_mapping_)
130  {
131  for (size_t i = 0; i < static_cast<unsigned int> (k); ++i)
132  {
133  int& neighbor_index = indices[i];
134  neighbor_index = index_mapping_[neighbor_index];
135  }
136  }
137  return result;
138 }
139 
140 //////////////////////////////////////////////////////////////////////////////////////////////
141 template <typename PointT, typename FlannDistance> void
143  const PointCloud& cloud, const std::vector<int>& indices, int k, std::vector< std::vector<int> >& k_indices,
144  std::vector< std::vector<float> >& k_sqr_distances) const
145 {
146  if (indices.empty ())
147  {
148  k_indices.resize (cloud.size ());
149  k_sqr_distances.resize (cloud.size ());
150 
151  if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
152  {
153  for (size_t i = 0; i < cloud.size(); i++)
154  {
155  assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
156  }
157  }
158 
159  bool can_cast = point_representation_->isTrivial ();
160 
161  // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix!
162  float* data=0;
163  if (!can_cast)
164  {
165  data = new float[dim_*cloud.size ()];
166  for (size_t i = 0; i < cloud.size (); ++i)
167  {
168  float* out = data+i*dim_;
169  point_representation_->vectorize (cloud[i],out);
170  }
171  }
172 
173  // const cast is evil, but the matrix constructor won't change the data, and the
174  // search won't change the matrix
175  float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&cloud[0])): data;
176  const flann::Matrix<float> m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float) );
177 
178  flann::SearchParams p;
179  p.sorted = sorted_results_;
180  p.eps = eps_;
181  p.checks = checks_;
182  index_->knnSearch (m,k_indices,k_sqr_distances,k, p);
183 
184  delete [] data;
185  }
186  else // if indices are present, the cloud has to be copied anyway. Only copy the relevant parts of the points here.
187  {
188  k_indices.resize (indices.size ());
189  k_sqr_distances.resize (indices.size ());
190 
191  if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
192  {
193  for (size_t i = 0; i < indices.size(); i++)
194  {
195  assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
196  }
197  }
198 
199  float* data=new float [dim_*indices.size ()];
200  for (size_t i = 0; i < indices.size (); ++i)
201  {
202  float* out = data+i*dim_;
203  point_representation_->vectorize (cloud[indices[i]],out);
204  }
205  const flann::Matrix<float> m (data ,indices.size (), point_representation_->getNumberOfDimensions ());
206 
207  flann::SearchParams p;
208  p.sorted = sorted_results_;
209  p.eps = eps_;
210  p.checks = checks_;
211  index_->knnSearch (m,k_indices,k_sqr_distances,k, p);
212 
213  delete[] data;
214  }
215  if (!identity_mapping_)
216  {
217  for (size_t j = 0; j < k_indices.size (); ++j)
218  {
219  for (size_t i = 0; i < static_cast<unsigned int> (k); ++i)
220  {
221  int& neighbor_index = k_indices[j][i];
222  neighbor_index = index_mapping_[neighbor_index];
223  }
224  }
225  }
226 }
227 
228 //////////////////////////////////////////////////////////////////////////////////////////////
229 template <typename PointT, typename FlannDistance> int
231  std::vector<int> &indices, std::vector<float> &distances,
232  unsigned int max_nn) const
233 {
234  assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); // remove this check as soon as FLANN does NaN checks internally
235  bool can_cast = point_representation_->isTrivial ();
236 
237  float* data = 0;
238  if (!can_cast)
239  {
240  data = new float [point_representation_->getNumberOfDimensions ()];
241  point_representation_->vectorize (point,data);
242  }
243 
244  float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&point)) : data;
245  const flann::Matrix<float> m (cdata ,1, point_representation_->getNumberOfDimensions ());
246 
247  flann::SearchParams p;
248  p.sorted = sorted_results_;
249  p.eps = eps_;
250  p.max_neighbors = max_nn > 0 ? max_nn : -1;
251  p.checks = checks_;
252  std::vector<std::vector<int> > i (1);
253  std::vector<std::vector<float> > d (1);
254  int result = index_->radiusSearch (m,i,d,static_cast<float> (radius * radius), p);
255 
256  delete [] data;
257  indices = i [0];
258  distances = d [0];
259 
260  if (!identity_mapping_)
261  {
262  for (size_t i = 0; i < indices.size (); ++i)
263  {
264  int& neighbor_index = indices [i];
265  neighbor_index = index_mapping_ [neighbor_index];
266  }
267  }
268  return result;
269 }
270 
271 //////////////////////////////////////////////////////////////////////////////////////////////
272 template <typename PointT, typename FlannDistance> void
274  const PointCloud& cloud, const std::vector<int>& indices, double radius, std::vector< std::vector<int> >& k_indices,
275  std::vector< std::vector<float> >& k_sqr_distances, unsigned int max_nn) const
276 {
277  if (indices.empty ()) // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix!
278  {
279  k_indices.resize (cloud.size ());
280  k_sqr_distances.resize (cloud.size ());
281 
282  if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
283  {
284  for (size_t i = 0; i < cloud.size(); i++)
285  {
286  assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
287  }
288  }
289 
290  bool can_cast = point_representation_->isTrivial ();
291 
292  float* data = 0;
293  if (!can_cast)
294  {
295  data = new float[dim_*cloud.size ()];
296  for (size_t i = 0; i < cloud.size (); ++i)
297  {
298  float* out = data+i*dim_;
299  point_representation_->vectorize (cloud[i],out);
300  }
301  }
302 
303  float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&cloud[0])) : data;
304  const flann::Matrix<float> m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float));
305 
306  flann::SearchParams p;
307  p.sorted = sorted_results_;
308  p.eps = eps_;
309  p.checks = checks_;
310  // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors
311  p.max_neighbors = max_nn != 0 ? max_nn : -1;
312  index_->radiusSearch (m,k_indices,k_sqr_distances,static_cast<float> (radius * radius), p);
313 
314  delete [] data;
315  }
316  else // if indices are present, the cloud has to be copied anyway. Only copy the relevant parts of the points here.
317  {
318  k_indices.resize (indices.size ());
319  k_sqr_distances.resize (indices.size ());
320 
321  if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
322  {
323  for (size_t i = 0; i < indices.size(); i++)
324  {
325  assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
326  }
327  }
328 
329  float* data = new float [dim_ * indices.size ()];
330  for (size_t i = 0; i < indices.size (); ++i)
331  {
332  float* out = data+i*dim_;
333  point_representation_->vectorize (cloud[indices[i]], out);
334  }
335  const flann::Matrix<float> m (data, cloud.size (), point_representation_->getNumberOfDimensions ());
336 
337  flann::SearchParams p;
338  p.sorted = sorted_results_;
339  p.eps = eps_;
340  p.checks = checks_;
341  // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors
342  p.max_neighbors = max_nn != 0 ? max_nn : -1;
343  index_->radiusSearch (m, k_indices, k_sqr_distances, static_cast<float> (radius * radius), p);
344 
345  delete[] data;
346  }
347  if (!identity_mapping_)
348  {
349  for (size_t j = 0; j < k_indices.size (); ++j )
350  {
351  for (size_t i = 0; i < k_indices[j].size (); ++i)
352  {
353  int& neighbor_index = k_indices[j][i];
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 
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;
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  }
428  if (input_copied_for_flann_)
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
virtual IndexPtr createIndex(MatrixConstPtr data)
Create a FLANN Index from the input data.
boost::shared_ptr< flann::Matrix< float > > MatrixPtr
Definition: flann_search.h:117
int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const
Search for the k-nearest neighbors for the given query point.
std::vector< int > index_mapping_
Definition: flann_search.h:365
IndexPtr index_
The FLANN index.
Definition: flann_search.h:341
FlannIndexCreatorPtr creator_
The index creator, used to (re-) create the index when the search data is passed.
Definition: flann_search.h:345
FlannSearch(bool sorted=true, FlannIndexCreatorPtr creator=FlannIndexCreatorPtr(new KdTreeIndexCreator()))
Search< PointT >::PointCloudConstPtr PointCloudConstPtr
Definition: flann_search.h:112
PointRepresentationConstPtr point_representation_
Definition: flann_search.h:361
virtual IndexPtr createIndex(MatrixConstPtr data)
Create a FLANN Index from the input data.
boost::shared_ptr< const flann::Matrix< float > > MatrixConstPtr
Definition: flann_search.h:118
virtual void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
PointCloud represents the base class in PCL for storing collections of 3D points.
MatrixPtr input_flann_
Input data in FLANN format.
Definition: flann_search.h:349
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
virtual ~FlannSearch()
Destructor for FlannSearch.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
int checks_
Number of checks to perform for approximate NN search using the multiple randomized tree index.
Definition: flann_search.h:357
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:115
boost::shared_ptr< FlannIndexCreator > FlannIndexCreatorPtr
Definition: flann_search.h:145
boost::shared_ptr< flann::NNIndex< FlannDistance > > IndexPtr
Definition: flann_search.h:121
int radiusSearch(const PointT &point, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const
Search for all the nearest neighbors of the query point in a given radius.
float eps_
Epsilon for approximate NN search.
Definition: flann_search.h:353
Generic search class.
Definition: search.h:74
size_t size() const
Definition: point_cloud.h:448