Point Cloud Library (PCL)  1.12.0-dev
kdtree_flann.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-2011, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 
39 #ifndef PCL_KDTREE_KDTREE_IMPL_FLANN_H_
40 #define PCL_KDTREE_KDTREE_IMPL_FLANN_H_
41 
42 #include <flann/flann.hpp>
43 
44 #include <pcl/kdtree/kdtree_flann.h>
45 #include <pcl/console/print.h>
46 
47 ///////////////////////////////////////////////////////////////////////////////////////////
48 template <typename PointT, typename Dist>
50  : pcl::KdTree<PointT> (sorted)
51  , flann_index_ ()
52  , identity_mapping_ (false)
53  , dim_ (0), total_nr_points_ (0)
54  , param_k_ (::flann::SearchParams (-1 , epsilon_))
55  , param_radius_ (::flann::SearchParams (-1, epsilon_, sorted))
56 {
57  if (!std::is_same<std::size_t, pcl::index_t>::value) {
58  const auto message = "FLANN is not optimized for current index type. Will incur "
59  "extra allocations and copy\n";
60  if (std::is_same<int, pcl::index_t>::value) {
61  PCL_DEBUG(message); // since this has been the default behavior till PCL 1.12
62  }
63  else {
64  PCL_WARN(message);
65  }
66  }
67 }
68 
69 ///////////////////////////////////////////////////////////////////////////////////////////
70 template <typename PointT, typename Dist>
72  : pcl::KdTree<PointT> (false)
73  , flann_index_ ()
74  , identity_mapping_ (false)
75  , dim_ (0), total_nr_points_ (0)
76  , param_k_ (::flann::SearchParams (-1 , epsilon_))
77  , param_radius_ (::flann::SearchParams (-1, epsilon_, false))
78 {
79  *this = k;
80 }
81 
82 ///////////////////////////////////////////////////////////////////////////////////////////
83 template <typename PointT, typename Dist> void
85 {
86  epsilon_ = eps;
87  param_k_ = ::flann::SearchParams (-1 , epsilon_);
88  param_radius_ = ::flann::SearchParams (-1 , epsilon_, sorted_);
89 }
90 
91 ///////////////////////////////////////////////////////////////////////////////////////////
92 template <typename PointT, typename Dist> void
94 {
95  sorted_ = sorted;
96  param_k_ = ::flann::SearchParams (-1, epsilon_);
97  param_radius_ = ::flann::SearchParams (-1, epsilon_, sorted_);
98 }
99 
100 ///////////////////////////////////////////////////////////////////////////////////////////
101 template <typename PointT, typename Dist> void
103 {
104  cleanup (); // Perform an automatic cleanup of structures
105 
106  epsilon_ = 0.0f; // default error bound value
107  dim_ = point_representation_->getNumberOfDimensions (); // Number of dimensions - default is 3 = xyz
108 
109  input_ = cloud;
110  indices_ = indices;
111 
112  // Allocate enough data
113  if (!input_)
114  {
115  PCL_ERROR ("[pcl::KdTreeFLANN::setInputCloud] Invalid input!\n");
116  return;
117  }
118  if (indices != nullptr)
119  {
120  convertCloudToArray (*input_, *indices_);
121  }
122  else
123  {
124  convertCloudToArray (*input_);
125  }
126  total_nr_points_ = static_cast<uindex_t> (index_mapping_.size ());
127  if (total_nr_points_ == 0)
128  {
129  PCL_ERROR ("[pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!\n");
130  return;
131  }
132 
133  flann_index_.reset (new FLANNIndex (::flann::Matrix<float> (cloud_.get (),
134  index_mapping_.size (),
135  dim_),
136  ::flann::KDTreeSingleIndexParams (15))); // max 15 points/leaf
137  flann_index_->buildIndex ();
138 }
139 
140 ///////////////////////////////////////////////////////////////////////////////////////////
141 namespace pcl {
142 namespace detail {
143 // Replace using constexpr in C++17
144 template <class IndexT,
145  class A,
146  class B,
147  class C,
148  class D,
149  class F,
150  CompatWithFlann<IndexT> = true>
151 int
152 knn_search(A& index, B& query, C& k_indices, D& dists, unsigned int k, F& params)
153 {
154  // Wrap k_indices vector (no data allocation)
155  ::flann::Matrix<index_t> k_indices_mat(&k_indices[0], 1, k);
156  return index.knnSearch(query, k_indices_mat, dists, k, params);
157 }
158 
159 template <class IndexT,
160  class A,
161  class B,
162  class C,
163  class D,
164  class F,
165  NotCompatWithFlann<IndexT> = true>
166 int
167 knn_search(A& index, B& query, C& k_indices, D& dists, unsigned int k, F& params)
168 {
169  std::vector<std::size_t> indices(k);
170  k_indices.resize(k);
171  // Wrap indices vector (no data allocation)
172  ::flann::Matrix<std::size_t> indices_mat(&indices[0], 1, k);
173  auto ret = index.knnSearch(query, indices_mat, dists, k, params);
174  std::copy(indices.cbegin(), indices.cend(), k_indices.begin());
175  return ret;
176 }
177 
178 template <class IndexT, class A, class B, class F, CompatWithFlann<IndexT> = true>
179 int
180 knn_search(A& index,
181  B& query,
182  std::vector<Indices>& k_indices,
183  std::vector<std::vector<float>>& dists,
184  unsigned int k,
185  F& params)
186 {
187  return index.knnSearch(query, k_indices, dists, k, params);
188 }
189 
190 template <class IndexT, class A, class B, class F, NotCompatWithFlann<IndexT> = true>
191 int
192 knn_search(A& index,
193  B& query,
194  std::vector<Indices>& k_indices,
195  std::vector<std::vector<float>>& dists,
196  unsigned int k,
197  F& params)
198 {
199  std::vector<std::vector<std::size_t>> indices;
200  // flann will resize accordingly
201  auto ret = index.knnSearch(query, indices, dists, k, params);
202 
203  k_indices.resize(indices.size());
204  {
205  auto it = indices.cbegin();
206  auto jt = k_indices.begin();
207  for (; it != indices.cend(); ++it, ++jt) {
208  jt->resize(it->size());
209  std::copy(it->cbegin(), it->cend(), jt->begin());
210  }
211  }
212  return ret;
213 }
214 } // namespace detail
215 template <class FlannIndex,
216  class Query,
217  class Indices,
218  class Distances,
219  class SearchParams>
220 int
221 knn_search(const FlannIndex& index,
222  const Query& query,
223  Indices& indices,
224  Distances& dists,
225  unsigned int k,
226  const SearchParams& params)
227 {
228  return detail::knn_search<pcl::index_t>(index, query, indices, dists, k, params);
229 }
230 } // namespace pcl
231 
232 template <typename PointT, typename Dist> int
234  Indices &k_indices,
235  std::vector<float> &k_distances) const
236 {
237  assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
238 
239  if (k > total_nr_points_)
240  k = total_nr_points_;
241 
242  k_indices.resize (k);
243  k_distances.resize (k);
244 
245  if (k==0)
246  return 0;
247 
248  std::vector<float> query (dim_);
249  point_representation_->vectorize (static_cast<PointT> (point), query);
250 
251  // Wrap the k_distances vector (no data copy)
252  ::flann::Matrix<float> k_distances_mat (&k_distances[0], 1, k);
253 
254  knn_search(*flann_index_,
255  ::flann::Matrix<float>(&query[0], 1, dim_),
256  k_indices,
257  k_distances_mat,
258  k,
259  param_k_);
260 
261  // Do mapping to original point cloud
262  if (!identity_mapping_)
263  {
264  for (std::size_t i = 0; i < static_cast<std::size_t> (k); ++i)
265  {
266  auto& neighbor_index = k_indices[i];
267  neighbor_index = index_mapping_[neighbor_index];
268  }
269  }
270 
271  return (k);
272 }
273 
274 ///////////////////////////////////////////////////////////////////////////////////////////
275 namespace pcl {
276 namespace detail {
277 // Replace using constexpr in C++17
278 template <class IndexT,
279  class A,
280  class B,
281  class C,
282  class D,
283  class F,
284  CompatWithFlann<IndexT> = true>
285 int
286 radius_search(A& index, B& query, C& k_indices, D& dists, float radius, F& params)
287 {
288  std::vector<pcl::Indices> indices(1);
289  int neighbors_in_radius = index.radiusSearch(query, indices, dists, radius, params);
290  k_indices = std::move(indices[0]);
291  return neighbors_in_radius;
292 }
293 
294 template <class IndexT,
295  class A,
296  class B,
297  class C,
298  class D,
299  class F,
300  NotCompatWithFlann<IndexT> = true>
301 int
302 radius_search(A& index, B& query, C& k_indices, D& dists, float radius, F& params)
303 {
304  std::vector<std::vector<std::size_t>> indices(1);
305  int neighbors_in_radius = index.radiusSearch(query, indices, dists, radius, params);
306  k_indices.resize(indices[0].size());
307  std::copy(indices[0].cbegin(), indices[0].cend(), k_indices.begin());
308  return neighbors_in_radius;
309 }
310 
311 template <class IndexT, class A, class B, class F, CompatWithFlann<IndexT> = true>
312 int
313 radius_search(A& index,
314  B& query,
315  std::vector<Indices>& k_indices,
316  std::vector<std::vector<float>>& dists,
317  float radius,
318  F& params)
319 {
320  return index.radiusSearch(query, k_indices, dists, radius, params);
321 }
322 
323 template <class IndexT, class A, class B, class F, NotCompatWithFlann<IndexT> = true>
324 int
325 radius_search(A& index,
326  B& query,
327  std::vector<Indices>& k_indices,
328  std::vector<std::vector<float>>& dists,
329  float radius,
330  F& params)
331 {
332  std::vector<std::vector<std::size_t>> indices;
333  // flann will resize accordingly
334  auto ret = index.radiusSearch(query, indices, dists, radius, params);
335 
336  k_indices.resize(indices.size());
337  {
338  auto it = indices.cbegin();
339  auto jt = k_indices.begin();
340  for (; it != indices.cend(); ++it, ++jt) {
341  jt->resize(it->size());
342  std::copy(it->cbegin(), it->cend(), jt->begin());
343  }
344  }
345  return ret;
346 }
347 } // namespace detail
348 template <class FlannIndex,
349  class Query,
350  class Indices,
351  class Distances,
352  class SearchParams>
353 int
354 radius_search(const FlannIndex& index,
355  const Query& query,
356  Indices& indices,
357  Distances& dists,
358  float radius,
359  const SearchParams& params)
360 {
361  return detail::radius_search<pcl::index_t>(
362  index, query, indices, dists, radius, params);
363 }
364 } // namespace pcl
365 
366 template <typename PointT, typename Dist> int
367 pcl::KdTreeFLANN<PointT, Dist>::radiusSearch (const PointT &point, double radius, Indices &k_indices,
368  std::vector<float> &k_sqr_dists, unsigned int max_nn) const
369 {
370  assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
371 
372  std::vector<float> query (dim_);
373  point_representation_->vectorize (static_cast<PointT> (point), query);
374 
375  // Has max_nn been set properly?
376  if (max_nn == 0 || max_nn > total_nr_points_)
377  max_nn = total_nr_points_;
378 
379  std::vector<std::vector<float> > dists(1);
380 
381  ::flann::SearchParams params (param_radius_);
382  if (max_nn == total_nr_points_)
383  params.max_neighbors = -1; // return all neighbors in radius
384  else
385  params.max_neighbors = max_nn;
386 
387  auto query_mat = ::flann::Matrix<float>(&query[0], 1, dim_);
388  int neighbors_in_radius = radius_search(*flann_index_,
389  query_mat,
390  k_indices,
391  dists,
392  static_cast<float>(radius * radius),
393  params);
394 
395  k_sqr_dists = dists[0];
396 
397  // Do mapping to original point cloud
398  if (!identity_mapping_)
399  {
400  for (int i = 0; i < neighbors_in_radius; ++i)
401  {
402  auto& neighbor_index = k_indices[i];
403  neighbor_index = index_mapping_[neighbor_index];
404  }
405  }
406 
407  return (neighbors_in_radius);
408 }
409 
410 ///////////////////////////////////////////////////////////////////////////////////////////
411 template <typename PointT, typename Dist> void
413 {
414  // Data array cleanup
415  index_mapping_.clear ();
416 
417  if (indices_)
418  indices_.reset ();
419 }
420 
421 ///////////////////////////////////////////////////////////////////////////////////////////
422 template <typename PointT, typename Dist> void
424 {
425  // No point in doing anything if the array is empty
426  if (cloud.empty ())
427  {
428  cloud_.reset ();
429  return;
430  }
431 
432  const auto original_no_of_points = cloud.size ();
433 
434  cloud_.reset (new float[original_no_of_points * dim_], std::default_delete<float[]> ());
435  float* cloud_ptr = cloud_.get ();
436  index_mapping_.reserve (original_no_of_points);
437  identity_mapping_ = true;
438 
439  for (std::size_t cloud_index = 0; cloud_index < original_no_of_points; ++cloud_index)
440  {
441  // Check if the point is invalid
442  if (!point_representation_->isValid (cloud[cloud_index]))
443  {
444  identity_mapping_ = false;
445  continue;
446  }
447 
448  index_mapping_.push_back (cloud_index);
449 
450  point_representation_->vectorize (cloud[cloud_index], cloud_ptr);
451  cloud_ptr += dim_;
452  }
453 }
454 
455 ///////////////////////////////////////////////////////////////////////////////////////////
456 template <typename PointT, typename Dist> void
458 {
459  // No point in doing anything if the array is empty
460  if (cloud.empty ())
461  {
462  cloud_.reset ();
463  return;
464  }
465 
466  int original_no_of_points = static_cast<int> (indices.size ());
467 
468  cloud_.reset (new float[original_no_of_points * dim_], std::default_delete<float[]> ());
469  float* cloud_ptr = cloud_.get ();
470  index_mapping_.reserve (original_no_of_points);
471  // its a subcloud -> false
472  // true only identity:
473  // - indices size equals cloud size
474  // - indices only contain values between 0 and cloud.size - 1
475  // - no index is multiple times in the list
476  // => index is complete
477  // But we can not guarantee that => identity_mapping_ = false
478  identity_mapping_ = false;
479 
480  for (const auto &index : indices)
481  {
482  // Check if the point is invalid
483  if (!point_representation_->isValid (cloud[index]))
484  continue;
485 
486  // map from 0 - N -> indices [0] - indices [N]
487  index_mapping_.push_back (index); // If the returned index should be for the indices vector
488 
489  point_representation_->vectorize (cloud[index], cloud_ptr);
490  cloud_ptr += dim_;
491  }
492 }
493 
494 #define PCL_INSTANTIATE_KdTreeFLANN(T) template class PCL_EXPORTS pcl::KdTreeFLANN<T>;
495 
496 #endif //#ifndef _PCL_KDTREE_KDTREE_IMPL_FLANN_H_
497 
pcl
Definition: convolution.h:46
pcl::KdTreeFLANN::radiusSearch
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.
Definition: kdtree_flann.hpp:367
pcl::KdTree
KdTree represents the base spatial locator class for kd-tree implementations.
Definition: kdtree.h:54
pcl::PointCloud::empty
bool empty() const
Definition: point_cloud.h:446
pcl::radius_search
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.
Definition: kdtree_flann.hpp:354
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::detail::radius_search
int radius_search(A &index, B &query, C &k_indices, D &dists, float radius, F &params)
Definition: kdtree_flann.hpp:286
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:674
pcl::knn_search
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.
Definition: kdtree_flann.hpp:221
flann::Index
Definition: kdtree_flann.h:52
flann::Matrix
Definition: flann_search.h:50
pcl::KdTreeFLANN
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:132
pcl::KdTreeFLANN::nearestKSearch
int nearestKSearch(const PointT &point, unsigned int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const override
Search for k-nearest neighbors for the given query point.
Definition: kdtree_flann.hpp:233
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
pcl::detail::knn_search
int knn_search(A &index, B &query, C &k_indices, D &dists, unsigned int k, F &params)
Definition: kdtree_flann.hpp:152
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:443
flann
Definition: kdtree_flann.h:49
pcl::KdTreeFLANN::KdTreeFLANN
KdTreeFLANN(bool sorted=true)
Default Constructor for KdTreeFLANN.
Definition: kdtree_flann.hpp:49
pcl::B
@ B
Definition: norms.h:54
pcl::KdTreeFLANN::setEpsilon
void setEpsilon(float eps) override
Set the search epsilon precision (error bound) for nearest neighbors searches.
Definition: kdtree_flann.hpp:84
pcl::KdTree< FeatureT >::IndicesConstPtr
shared_ptr< const Indices > IndicesConstPtr
Definition: kdtree.h:58
pcl::uindex_t
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition: types.h:120
pcl::KdTreeFLANN::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
Definition: kdtree_flann.hpp:102
pcl::KdTreeFLANN::setSortedResults
void setSortedResults(bool sorted)
Definition: kdtree_flann.hpp:93
pcl::KdTree< FeatureT >::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: kdtree.h:62