Point Cloud Library (PCL)  1.7.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>
48 typename pcl::search::FlannSearch<PointT, FlannDistance>::IndexPtr
50 {
51  return (IndexPtr (new flann::KDTreeSingleIndex<FlannDistance> (*data,flann::KDTreeSingleIndexParams (max_leaf_size_))));
52 }
53 
54 //////////////////////////////////////////////////////////////////////////////////////////////
55 template <typename PointT, typename FlannDistance>
56 typename pcl::search::FlannSearch<PointT, FlannDistance>::IndexPtr
58 {
59  return (IndexPtr (new flann::KMeansIndex<FlannDistance> (*data,flann::KMeansIndexParams ())));
60 }
61 
62 //////////////////////////////////////////////////////////////////////////////////////////////
63 template <typename PointT, typename FlannDistance>
64 pcl::search::FlannSearch<PointT, FlannDistance>::FlannSearch(bool sorted, FlannIndexCreatorPtr creator) : pcl::search::Search<PointT> ("FlannSearch",sorted),
67 {
68  dim_ = point_representation_->getNumberOfDimensions ();
69 }
70 
71 //////////////////////////////////////////////////////////////////////////////////////////////
72 template <typename PointT, typename FlannDistance>
74 {
75  if (input_copied_for_flann_)
76  delete [] input_flann_->ptr();
77 }
78 
79 //////////////////////////////////////////////////////////////////////////////////////////////
80 template <typename PointT, typename FlannDistance> void
81 pcl::search::FlannSearch<PointT, FlannDistance>::setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr& indices)
82 {
83  input_ = cloud;
84  indices_ = indices;
85  convertInputToFlannMatrix ();
86  index_ = creator_->createIndex (input_flann_);
87  index_->buildIndex ();
88 }
89 
90 //////////////////////////////////////////////////////////////////////////////////////////////
91 template <typename PointT, typename FlannDistance> int
92 pcl::search::FlannSearch<PointT, FlannDistance>::nearestKSearch (const PointT &point, int k, std::vector<int> &indices, std::vector<float> &dists) const
93 {
94  assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); // remove this check as soon as FLANN does NaN checks internally
95  bool can_cast = point_representation_->isTrivial ();
96 
97  float* data = 0;
98  if (!can_cast)
99  {
100  data = new float [point_representation_->getNumberOfDimensions ()];
101  point_representation_->vectorize (point,data);
102  }
103 
104  float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&point)): data;
105  const flann::Matrix<float> m (cdata ,1, point_representation_->getNumberOfDimensions ());
106 
107  flann::SearchParams p(-1);
108  p.eps = eps_;
109  p.sorted = sorted_results_;
110  if (indices.size() != static_cast<unsigned int> (k))
111  indices.resize (k,-1);
112  if (dists.size() != static_cast<unsigned int> (k))
113  dists.resize (k);
114  flann::Matrix<int> i (&indices[0],1,k);
115  flann::Matrix<float> d (&dists[0],1,k);
116  int result = index_->knnSearch (m,i,d,k, p);
117 
118  delete [] data;
119 
120  if (!identity_mapping_)
121  {
122  for (size_t i = 0; i < static_cast<unsigned int> (k); ++i)
123  {
124  int& neighbor_index = indices[i];
125  neighbor_index = index_mapping_[neighbor_index];
126  }
127  }
128  return result;
129 }
130 
131 //////////////////////////////////////////////////////////////////////////////////////////////
132 template <typename PointT, typename FlannDistance> void
134  const PointCloud& cloud, const std::vector<int>& indices, int k, std::vector< std::vector<int> >& k_indices,
135  std::vector< std::vector<float> >& k_sqr_distances) const
136 {
137  if (indices.empty ())
138  {
139  k_indices.resize (cloud.size ());
140  k_sqr_distances.resize (cloud.size ());
141 
142  if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
143  {
144  for (size_t i = 0; i < cloud.size(); i++)
145  {
146  assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
147  }
148  }
149 
150  bool can_cast = point_representation_->isTrivial ();
151 
152  // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix!
153  float* data=0;
154  if (!can_cast)
155  {
156  data = new float[dim_*cloud.size ()];
157  for (size_t i = 0; i < cloud.size (); ++i)
158  {
159  float* out = data+i*dim_;
160  point_representation_->vectorize (cloud[i],out);
161  }
162  }
163 
164  // const cast is evil, but the matrix constructor won't change the data, and the
165  // search won't change the matrix
166  float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&cloud[0])): data;
167  const flann::Matrix<float> m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float) );
168 
169  flann::SearchParams p;
170  p.sorted = sorted_results_;
171  p.eps = eps_;
172  index_->knnSearch (m,k_indices,k_sqr_distances,k, p);
173 
174  delete [] data;
175  }
176  else // if indices are present, the cloud has to be copied anyway. Only copy the relevant parts of the points here.
177  {
178  k_indices.resize (indices.size ());
179  k_sqr_distances.resize (indices.size ());
180 
181  if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
182  {
183  for (size_t i = 0; i < indices.size(); i++)
184  {
185  assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
186  }
187  }
188 
189  float* data=new float [dim_*indices.size ()];
190  for (size_t i = 0; i < indices.size (); ++i)
191  {
192  float* out = data+i*dim_;
193  point_representation_->vectorize (cloud[indices[i]],out);
194  }
195  const flann::Matrix<float> m (data ,indices.size (), point_representation_->getNumberOfDimensions ());
196 
197  flann::SearchParams p;
198  p.sorted = sorted_results_;
199  p.eps = eps_;
200  index_->knnSearch (m,k_indices,k_sqr_distances,k, p);
201 
202  delete[] data;
203  }
204  if (!identity_mapping_)
205  {
206  for (size_t j = 0; j < k_indices.size (); ++j)
207  {
208  for (size_t i = 0; i < static_cast<unsigned int> (k); ++i)
209  {
210  int& neighbor_index = k_indices[j][i];
211  neighbor_index = index_mapping_[neighbor_index];
212  }
213  }
214  }
215 }
216 
217 //////////////////////////////////////////////////////////////////////////////////////////////
218 template <typename PointT, typename FlannDistance> int
220  std::vector<int> &indices, std::vector<float> &distances,
221  unsigned int max_nn) const
222 {
223  assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); // remove this check as soon as FLANN does NaN checks internally
224  bool can_cast = point_representation_->isTrivial ();
225 
226  float* data = 0;
227  if (!can_cast)
228  {
229  data = new float [point_representation_->getNumberOfDimensions ()];
230  point_representation_->vectorize (point,data);
231  }
232 
233  float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&point)) : data;
234  const flann::Matrix<float> m (cdata ,1, point_representation_->getNumberOfDimensions ());
235 
236  flann::SearchParams p;
237  p.sorted = sorted_results_;
238  p.eps = eps_;
239  p.max_neighbors = max_nn > 0 ? max_nn : -1;
240  std::vector<std::vector<int> > i (1);
241  std::vector<std::vector<float> > d (1);
242  int result = index_->radiusSearch (m,i,d,static_cast<float> (radius * radius), p);
243 
244  delete [] data;
245  indices = i [0];
246  distances = d [0];
247 
248  if (!identity_mapping_)
249  {
250  for (size_t i = 0; i < indices.size (); ++i)
251  {
252  int& neighbor_index = indices [i];
253  neighbor_index = index_mapping_ [neighbor_index];
254  }
255  }
256  return result;
257 }
258 
259 //////////////////////////////////////////////////////////////////////////////////////////////
260 template <typename PointT, typename FlannDistance> void
262  const PointCloud& cloud, const std::vector<int>& indices, double radius, std::vector< std::vector<int> >& k_indices,
263  std::vector< std::vector<float> >& k_sqr_distances, unsigned int max_nn) const
264 {
265  if (indices.empty ()) // full point cloud + trivial copy operation = no need to do any conversion/copying to the flann matrix!
266  {
267  k_indices.resize (cloud.size ());
268  k_sqr_distances.resize (cloud.size ());
269 
270  if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
271  {
272  for (size_t i = 0; i < cloud.size(); i++)
273  {
274  assert (point_representation_->isValid (cloud[i]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
275  }
276  }
277 
278  bool can_cast = point_representation_->isTrivial ();
279 
280  float* data = 0;
281  if (!can_cast)
282  {
283  data = new float[dim_*cloud.size ()];
284  for (size_t i = 0; i < cloud.size (); ++i)
285  {
286  float* out = data+i*dim_;
287  point_representation_->vectorize (cloud[i],out);
288  }
289  }
290 
291  float* cdata = can_cast ? const_cast<float*> (reinterpret_cast<const float*> (&cloud[0])) : data;
292  const flann::Matrix<float> m (cdata ,cloud.size (), dim_, can_cast ? sizeof (PointT) : dim_ * sizeof (float));
293 
294  flann::SearchParams p;
295  p.sorted = sorted_results_;
296  p.eps = eps_;
297  // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors
298  p.max_neighbors = max_nn != 0 ? max_nn : -1;
299  index_->radiusSearch (m,k_indices,k_sqr_distances,static_cast<float> (radius * radius), p);
300 
301  delete [] data;
302  }
303  else // if indices are present, the cloud has to be copied anyway. Only copy the relevant parts of the points here.
304  {
305  k_indices.resize (indices.size ());
306  k_sqr_distances.resize (indices.size ());
307 
308  if (! cloud.is_dense) // remove this check as soon as FLANN does NaN checks internally
309  {
310  for (size_t i = 0; i < indices.size(); i++)
311  {
312  assert (point_representation_->isValid (cloud [indices[i]]) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
313  }
314  }
315 
316  float* data = new float [dim_ * indices.size ()];
317  for (size_t i = 0; i < indices.size (); ++i)
318  {
319  float* out = data+i*dim_;
320  point_representation_->vectorize (cloud[indices[i]], out);
321  }
322  const flann::Matrix<float> m (data, cloud.size (), point_representation_->getNumberOfDimensions ());
323 
324  flann::SearchParams p;
325  p.sorted = sorted_results_;
326  p.eps = eps_;
327  // here: max_nn==0: take all neighbors. flann: max_nn==0: return no neighbors, only count them. max_nn==-1: return all neighbors
328  p.max_neighbors = max_nn != 0 ? max_nn : -1;
329  index_->radiusSearch (m, k_indices, k_sqr_distances, static_cast<float> (radius * radius), p);
330 
331  delete[] data;
332  }
333  if (!identity_mapping_)
334  {
335  for (size_t j = 0; j < k_indices.size (); ++j )
336  {
337  for (size_t i = 0; i < k_indices[j].size (); ++i)
338  {
339  int& neighbor_index = k_indices[j][i];
340  neighbor_index = index_mapping_[neighbor_index];
341  }
342  }
343  }
344 }
345 
346 //////////////////////////////////////////////////////////////////////////////////////////////
347 template <typename PointT, typename FlannDistance> void
349 {
350  size_t original_no_of_points = indices_ && !indices_->empty () ? indices_->size () : input_->size ();
351 
352  if (input_copied_for_flann_)
353  delete input_flann_->ptr();
354  input_copied_for_flann_ = true;
355  index_mapping_.clear();
356  identity_mapping_ = true;
357 
358  //cloud_ = (float*)malloc (original_no_of_points * dim_ * sizeof (float));
359  //index_mapping_.reserve(original_no_of_points);
360  //identity_mapping_ = true;
361 
362  if (!indices_ || indices_->empty ())
363  {
364  // best case: all points can be passed to flann without any conversions
365  if (input_->is_dense && point_representation_->isTrivial ())
366  {
367  // const cast is evil, but flann won't change the data
368  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)));
369  input_copied_for_flann_ = false;
370  }
371  else
372  {
373  input_flann_ = MatrixPtr (new flann::Matrix<float> (new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ()));
374  float* cloud_ptr = input_flann_->ptr();
375  for (size_t i = 0; i < original_no_of_points; ++i)
376  {
377  const PointT& point = (*input_)[i];
378  // Check if the point is invalid
379  if (!point_representation_->isValid (point))
380  {
381  identity_mapping_ = false;
382  continue;
383  }
384 
385  index_mapping_.push_back (static_cast<int> (i)); // If the returned index should be for the indices vector
386 
387  point_representation_->vectorize (point, cloud_ptr);
388  cloud_ptr += dim_;
389  }
390  }
391 
392  }
393  else
394  {
395  input_flann_ = MatrixPtr (new flann::Matrix<float> (new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ()));
396  float* cloud_ptr = input_flann_->ptr();
397  for (size_t indices_index = 0; indices_index < original_no_of_points; ++indices_index)
398  {
399  int cloud_index = (*indices_)[indices_index];
400  const PointT& point = (*input_)[cloud_index];
401  // Check if the point is invalid
402  if (!point_representation_->isValid (point))
403  {
404  identity_mapping_ = false;
405  continue;
406  }
407 
408  index_mapping_.push_back (static_cast<int> (indices_index)); // If the returned index should be for the indices vector
409 
410  point_representation_->vectorize (point, cloud_ptr);
411  cloud_ptr += dim_;
412  }
413  }
414  if (input_copied_for_flann_)
415  input_flann_->rows = index_mapping_.size ();
416 }
417 
418 #define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch<T>;
419 
420 #endif