Point Cloud Library (PCL)  1.14.1-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  ,
53  param_k_ (::flann::SearchParams (-1 , epsilon_))
54  , param_radius_ (::flann::SearchParams (-1, epsilon_, sorted))
55 {
56  if (!std::is_same<std::size_t, pcl::index_t>::value) {
57  const auto message = "FLANN is not optimized for current index type. Will incur "
58  "extra allocations and copy\n";
59  if (std::is_same<int, pcl::index_t>::value) {
60  PCL_DEBUG(message); // since this has been the default behavior till PCL 1.12
61  }
62  else {
63  PCL_WARN(message);
64  }
65  }
66 }
67 
68 ///////////////////////////////////////////////////////////////////////////////////////////
69 template <typename PointT, typename Dist>
71  : pcl::KdTree<PointT> (false)
72  , flann_index_ ()
73  ,
74  param_k_ (::flann::SearchParams (-1 , epsilon_))
75  , param_radius_ (::flann::SearchParams (-1, epsilon_, false))
76 {
77  *this = k;
78 }
79 
80 ///////////////////////////////////////////////////////////////////////////////////////////
81 template <typename PointT, typename Dist> void
83 {
84  epsilon_ = eps;
85  param_k_ = ::flann::SearchParams (-1 , epsilon_);
86  param_radius_ = ::flann::SearchParams (-1 , epsilon_, sorted_);
87 }
88 
89 ///////////////////////////////////////////////////////////////////////////////////////////
90 template <typename PointT, typename Dist> void
92 {
93  sorted_ = sorted;
94  param_k_ = ::flann::SearchParams (-1, epsilon_);
95  param_radius_ = ::flann::SearchParams (-1, epsilon_, sorted_);
96 }
97 
98 ///////////////////////////////////////////////////////////////////////////////////////////
99 template <typename PointT, typename Dist> void
101 {
102  cleanup (); // Perform an automatic cleanup of structures
103 
104  epsilon_ = 0.0f; // default error bound value
105  dim_ = point_representation_->getNumberOfDimensions (); // Number of dimensions - default is 3 = xyz
106 
107  input_ = cloud;
108  indices_ = indices;
109 
110  // Allocate enough data
111  if (!input_)
112  {
113  PCL_ERROR ("[pcl::KdTreeFLANN::setInputCloud] Invalid input!\n");
114  return;
115  }
116  if (indices != nullptr)
117  {
118  convertCloudToArray (*input_, *indices_);
119  }
120  else
121  {
122  convertCloudToArray (*input_);
123  }
124  total_nr_points_ = static_cast<uindex_t> (index_mapping_.size ());
125  if (total_nr_points_ == 0)
126  {
127  PCL_ERROR ("[pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!\n");
128  return;
129  }
130 
131  flann_index_.reset (new FLANNIndex (::flann::Matrix<float> (cloud_.get (),
132  index_mapping_.size (),
133  dim_),
134  ::flann::KDTreeSingleIndexParams (15))); // max 15 points/leaf
135  flann_index_->buildIndex ();
136 }
137 
138 ///////////////////////////////////////////////////////////////////////////////////////////
139 namespace pcl {
140 namespace detail {
141 // Replace using constexpr in C++17
142 template <class IndexT,
143  class A,
144  class B,
145  class C,
146  class D,
147  class F,
148  CompatWithFlann<IndexT> = true>
149 int
150 knn_search(A& index, B& query, C& k_indices, D& dists, unsigned int k, F& params)
151 {
152  // Wrap k_indices vector (no data allocation)
153  ::flann::Matrix<index_t> k_indices_mat(&k_indices[0], 1, k);
154  return index.knnSearch(query, k_indices_mat, dists, k, params);
155 }
156 
157 template <class IndexT,
158  class A,
159  class B,
160  class C,
161  class D,
162  class F,
163  NotCompatWithFlann<IndexT> = true>
164 int
165 knn_search(A& index, B& query, C& k_indices, D& dists, unsigned int k, F& params)
166 {
167  std::vector<std::size_t> indices(k);
168  k_indices.resize(k);
169  // Wrap indices vector (no data allocation)
170  ::flann::Matrix<std::size_t> indices_mat(indices.data(), 1, k);
171  auto ret = index.knnSearch(query, indices_mat, dists, k, params);
172  // cast appropriately
173  std::transform(indices.cbegin(),
174  indices.cend(),
175  k_indices.begin(),
176  [](const auto& x) { return static_cast<pcl::index_t>(x); });
177  return ret;
178 }
179 
180 template <class IndexT, class A, class B, class F, CompatWithFlann<IndexT> = true>
181 int
182 knn_search(A& index,
183  B& query,
184  std::vector<Indices>& k_indices,
185  std::vector<std::vector<float>>& dists,
186  unsigned int k,
187  F& params)
188 {
189  return index.knnSearch(query, k_indices, dists, k, params);
190 }
191 
192 template <class IndexT, class A, class B, class F, NotCompatWithFlann<IndexT> = true>
193 int
194 knn_search(A& index,
195  B& query,
196  std::vector<Indices>& k_indices,
197  std::vector<std::vector<float>>& dists,
198  unsigned int k,
199  F& params)
200 {
201  std::vector<std::vector<std::size_t>> indices;
202  // flann will resize accordingly
203  auto ret = index.knnSearch(query, indices, dists, k, params);
204 
205  k_indices.resize(indices.size());
206  {
207  auto it = indices.cbegin();
208  auto jt = k_indices.begin();
209  for (; it != indices.cend(); ++it, ++jt) {
210  jt->resize(it->size());
211  std::copy(it->cbegin(), it->cend(), jt->begin());
212  }
213  }
214  return ret;
215 }
216 } // namespace detail
217 template <class FlannIndex,
218  class Query,
219  class Indices,
220  class Distances,
221  class SearchParams>
222 int
223 knn_search(const FlannIndex& index,
224  const Query& query,
225  Indices& indices,
226  Distances& dists,
227  unsigned int k,
228  const SearchParams& params)
229 {
230  return detail::knn_search<pcl::index_t>(index, query, indices, dists, k, params);
231 }
232 } // namespace pcl
233 
234 template <typename PointT, typename Dist> int
236  Indices &k_indices,
237  std::vector<float> &k_distances) const
238 {
239  assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
240 
241  if (k > total_nr_points_)
242  k = total_nr_points_;
243 
244  k_indices.resize (k);
245  k_distances.resize (k);
246 
247  if (k==0)
248  return 0;
249 
250  std::vector<float> query (dim_);
251  point_representation_->vectorize (static_cast<PointT> (point), query);
252 
253  // Wrap the k_distances vector (no data copy)
254  ::flann::Matrix<float> k_distances_mat (k_distances.data(), 1, k);
255 
256  knn_search(*flann_index_,
257  ::flann::Matrix<float>(query.data(), 1, dim_),
258  k_indices,
259  k_distances_mat,
260  k,
261  param_k_);
262 
263  // Do mapping to original point cloud
264  if (!identity_mapping_)
265  {
266  for (std::size_t i = 0; i < static_cast<std::size_t> (k); ++i)
267  {
268  auto& neighbor_index = k_indices[i];
269  neighbor_index = index_mapping_[neighbor_index];
270  }
271  }
272 
273  return (k);
274 }
275 
276 ///////////////////////////////////////////////////////////////////////////////////////////
277 namespace pcl {
278 namespace detail {
279 // Replace using constexpr in C++17
280 template <class IndexT,
281  class A,
282  class B,
283  class C,
284  class D,
285  class F,
286  CompatWithFlann<IndexT> = true>
287 int
288 radius_search(A& index, B& query, C& k_indices, D& dists, float radius, F& params)
289 {
290  std::vector<pcl::Indices> indices(1);
291  int neighbors_in_radius = index.radiusSearch(query, indices, dists, radius, params);
292  k_indices = std::move(indices[0]);
293  return neighbors_in_radius;
294 }
295 
296 template <class IndexT,
297  class A,
298  class B,
299  class C,
300  class D,
301  class F,
302  NotCompatWithFlann<IndexT> = true>
303 int
304 radius_search(A& index, B& query, C& k_indices, D& dists, float radius, F& params)
305 {
306  std::vector<std::vector<std::size_t>> indices(1);
307  int neighbors_in_radius = index.radiusSearch(query, indices, dists, radius, params);
308  k_indices.resize(indices[0].size());
309  // cast appropriately
310  std::transform(indices[0].cbegin(),
311  indices[0].cend(),
312  k_indices.begin(),
313  [](const auto& x) { return static_cast<pcl::index_t>(x); });
314  return neighbors_in_radius;
315 }
316 
317 template <class IndexT, class A, class B, class F, CompatWithFlann<IndexT> = true>
318 int
319 radius_search(A& index,
320  B& query,
321  std::vector<Indices>& k_indices,
322  std::vector<std::vector<float>>& dists,
323  float radius,
324  F& params)
325 {
326  return index.radiusSearch(query, k_indices, dists, radius, params);
327 }
328 
329 template <class IndexT, class A, class B, class F, NotCompatWithFlann<IndexT> = true>
330 int
331 radius_search(A& index,
332  B& query,
333  std::vector<Indices>& k_indices,
334  std::vector<std::vector<float>>& dists,
335  float radius,
336  F& params)
337 {
338  std::vector<std::vector<std::size_t>> indices;
339  // flann will resize accordingly
340  auto ret = index.radiusSearch(query, indices, dists, radius, params);
341 
342  k_indices.resize(indices.size());
343  {
344  auto it = indices.cbegin();
345  auto jt = k_indices.begin();
346  for (; it != indices.cend(); ++it, ++jt) {
347  jt->resize(it->size());
348  std::copy(it->cbegin(), it->cend(), jt->begin());
349  }
350  }
351  return ret;
352 }
353 } // namespace detail
354 template <class FlannIndex,
355  class Query,
356  class Indices,
357  class Distances,
358  class SearchParams>
359 int
360 radius_search(const FlannIndex& index,
361  const Query& query,
362  Indices& indices,
363  Distances& dists,
364  float radius,
365  const SearchParams& params)
366 {
367  return detail::radius_search<pcl::index_t>(
368  index, query, indices, dists, radius, params);
369 }
370 } // namespace pcl
371 
372 template <typename PointT, typename Dist> int
373 pcl::KdTreeFLANN<PointT, Dist>::radiusSearch (const PointT &point, double radius, Indices &k_indices,
374  std::vector<float> &k_sqr_dists, unsigned int max_nn) const
375 {
376  assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!");
377 
378  std::vector<float> query (dim_);
379  point_representation_->vectorize (static_cast<PointT> (point), query);
380 
381  // Has max_nn been set properly?
382  if (max_nn == 0 || max_nn > total_nr_points_)
383  max_nn = total_nr_points_;
384 
385  std::vector<std::vector<float> > dists(1);
386 
387  ::flann::SearchParams params (param_radius_);
388  if (max_nn == total_nr_points_)
389  params.max_neighbors = -1; // return all neighbors in radius
390  else
391  params.max_neighbors = max_nn;
392 
393  auto query_mat = ::flann::Matrix<float>(query.data(), 1, dim_);
394  int neighbors_in_radius = radius_search(*flann_index_,
395  query_mat,
396  k_indices,
397  dists,
398  static_cast<float>(radius * radius),
399  params);
400 
401  k_sqr_dists = dists[0];
402 
403  // Do mapping to original point cloud
404  if (!identity_mapping_)
405  {
406  for (int i = 0; i < neighbors_in_radius; ++i)
407  {
408  auto& neighbor_index = k_indices[i];
409  neighbor_index = index_mapping_[neighbor_index];
410  }
411  }
412 
413  return (neighbors_in_radius);
414 }
415 
416 ///////////////////////////////////////////////////////////////////////////////////////////
417 template <typename PointT, typename Dist> void
419 {
420  // Data array cleanup
421  index_mapping_.clear ();
422 
423  if (indices_)
424  indices_.reset ();
425 }
426 
427 ///////////////////////////////////////////////////////////////////////////////////////////
428 template <typename PointT, typename Dist> void
430 {
431  // No point in doing anything if the array is empty
432  if (cloud.empty ())
433  {
434  cloud_.reset ();
435  return;
436  }
437 
438  const auto original_no_of_points = cloud.size ();
439 
440  cloud_.reset (new float[original_no_of_points * dim_], std::default_delete<float[]> ());
441  float* cloud_ptr = cloud_.get ();
442  index_mapping_.reserve (original_no_of_points);
443  identity_mapping_ = true;
444 
445  for (std::size_t cloud_index = 0; cloud_index < original_no_of_points; ++cloud_index)
446  {
447  // Check if the point is invalid
448  if (!point_representation_->isValid (cloud[cloud_index]))
449  {
450  identity_mapping_ = false;
451  continue;
452  }
453 
454  index_mapping_.push_back (cloud_index);
455 
456  point_representation_->vectorize (cloud[cloud_index], cloud_ptr);
457  cloud_ptr += dim_;
458  }
459 }
460 
461 ///////////////////////////////////////////////////////////////////////////////////////////
462 template <typename PointT, typename Dist> void
464 {
465  // No point in doing anything if the array is empty
466  if (cloud.empty ())
467  {
468  cloud_.reset ();
469  return;
470  }
471 
472  int original_no_of_points = static_cast<int> (indices.size ());
473 
474  cloud_.reset (new float[original_no_of_points * dim_], std::default_delete<float[]> ());
475  float* cloud_ptr = cloud_.get ();
476  index_mapping_.reserve (original_no_of_points);
477  // its a subcloud -> false
478  // true only identity:
479  // - indices size equals cloud size
480  // - indices only contain values between 0 and cloud.size - 1
481  // - no index is multiple times in the list
482  // => index is complete
483  // But we can not guarantee that => identity_mapping_ = false
484  identity_mapping_ = false;
485 
486  for (const auto &index : indices)
487  {
488  // Check if the point is invalid
489  if (!point_representation_->isValid (cloud[index]))
490  continue;
491 
492  // map from 0 - N -> indices [0] - indices [N]
493  index_mapping_.push_back (index); // If the returned index should be for the indices vector
494 
495  point_representation_->vectorize (cloud[index], cloud_ptr);
496  cloud_ptr += dim_;
497  }
498 }
499 
500 #define PCL_INSTANTIATE_KdTreeFLANN(T) template class PCL_EXPORTS pcl::KdTreeFLANN<T>;
501 
502 #endif //#ifndef _PCL_KDTREE_KDTREE_IMPL_FLANN_H_
503 
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:132
void setEpsilon(float eps) override
Set the search epsilon precision (error bound) for nearest neighbors searches.
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.
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.
void setSortedResults(bool sorted)
KdTreeFLANN(bool sorted=true)
Default Constructor for KdTreeFLANN.
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
KdTree represents the base spatial locator class for kd-tree implementations.
Definition: kdtree.h:56
shared_ptr< const Indices > IndicesConstPtr
Definition: kdtree.h:59
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: kdtree.h:63
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
bool empty() const
Definition: point_cloud.h:446
std::size_t size() const
Definition: point_cloud.h:443
@ B
Definition: norms.h:54
int knn_search(A &index, B &query, C &k_indices, D &dists, unsigned int k, F &params)
int radius_search(A &index, B &query, C &k_indices, D &dists, float radius, F &params)
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition: types.h:120
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.