Point Cloud Library (PCL)  1.14.1-dev
kdtree_flann.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, 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  * $Id: kdtree_flann.h 36261 2011-02-26 01:34:42Z mariusm $
38  *
39  */
40 
41 #pragma once
42 
43 #include <pcl/kdtree/kdtree.h>
44 #include <flann/util/params.h>
45 
46 #include <memory>
47 
48 // Forward declarations
49 namespace flann
50 {
51  template <typename T> struct L2_Simple;
52  template <typename T> class Index;
53 }
54 
55 namespace pcl
56 {
57 namespace detail {
58 // Helper struct to create a compatible Matrix and copy data back when needed
59 // Replace using if constexpr in C++17
60 template <typename IndexT>
61 struct compat_with_flann : std::false_type {};
62 
63 template <>
64 struct compat_with_flann<std::size_t> : std::true_type {};
65 
66 template <typename IndexT>
67 using CompatWithFlann = std::enable_if_t<compat_with_flann<IndexT>::value, bool>;
68 template <typename IndexT>
69 using NotCompatWithFlann = std::enable_if_t<!compat_with_flann<IndexT>::value, bool>;
70 } // namespace detail
71 
72 /**
73  * @brief Compatibility template function to allow use of various types of indices with
74  * FLANN
75  * @details Template is used for all params to not constrain any FLANN side capability
76  * @param[in,out] index A index searcher, of type ::flann::Index<Dist> or similar, where
77  * Dist is a template for computing distance between 2 points
78  * @param[in] query A ::flann::Matrix<float> or compatible matrix representation of the
79  * query point
80  * @param[out] indices Indices found in radius
81  * @param[out] dists Computed distance matrix
82  * @param[in] radius Threshold for consideration
83  * @param[in] params Any parameters to pass to the radius_search call
84  */
85 template <class FlannIndex,
86  class Query,
87  class Indices,
88  class Distances,
89  class SearchParams>
90 int
91 radius_search(const FlannIndex& index,
92  const Query& query,
93  Indices& indices,
94  Distances& dists,
95  float radius,
96  const SearchParams& params);
97 
98 /**
99  * @brief Compatibility template function to allow use of various types of indices with
100  * FLANN
101  * @details Template is used for all params to not constrain any FLANN side capability
102  * @param[in,out] index A index searcher, of type ::flann::Index<Dist> or similar, where
103  * Dist is a template for computing distance between 2 points
104  * @param[in] query A ::flann::Matrix<float> or compatible matrix representation of the
105  * query point
106  * @param[out] indices Neighboring k indices found
107  * @param[out] dists Computed distance matrix
108  * @param[in] k Number of neighbors to search for
109  * @param[in] params Any parameters to pass to the knn_search call
110  */
111 template <class FlannIndex,
112  class Query,
113  class Indices,
114  class Distances,
115  class SearchParams>
116 int
117 knn_search(const FlannIndex& index,
118  const Query& query,
119  Indices& indices,
120  Distances& dists,
121  unsigned int k,
122  const SearchParams& params);
123 
124 /** \brief KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
125  * The class is making use of the FLANN (Fast Library for Approximate Nearest Neighbor)
126  * project by Marius Muja and David Lowe.
127  *
128  * \author Radu B. Rusu, Marius Muja
129  * \ingroup kdtree
130  */
131 template <typename PointT, typename Dist = ::flann::L2_Simple<float>>
132 class KdTreeFLANN : public pcl::KdTree<PointT> {
133 public:
141 
144 
145  using IndicesPtr = shared_ptr<Indices>;
146  using IndicesConstPtr = shared_ptr<const Indices>;
147 
149 
150  // Boost shared pointers
151  using Ptr = shared_ptr<KdTreeFLANN<PointT, Dist>>;
152  using ConstPtr = shared_ptr<const KdTreeFLANN<PointT, Dist>>;
153 
154  /** \brief Default Constructor for KdTreeFLANN.
155  * \param[in] sorted set to true if the application that the tree will be used for
156  * requires sorted nearest neighbor indices (default). False otherwise.
157  *
158  * By setting sorted to false, the \ref radiusSearch operations will be faster.
159  */
160  KdTreeFLANN(bool sorted = true);
161 
162  /** \brief Copy constructor
163  * \param[in] k the tree to copy into this
164  */
166 
167  /** \brief Copy operator
168  * \param[in] k the tree to copy into this
169  */
172  {
174  flann_index_ = k.flann_index_;
175  cloud_ = k.cloud_;
176  index_mapping_ = k.index_mapping_;
177  identity_mapping_ = k.identity_mapping_;
178  dim_ = k.dim_;
179  total_nr_points_ = k.total_nr_points_;
180  param_k_ = k.param_k_;
181  param_radius_ = k.param_radius_;
182  return (*this);
183  }
184 
185  /** \brief Set the search epsilon precision (error bound) for nearest neighbors
186  * searches. \param[in] eps precision (error bound) for nearest neighbors searches
187  */
188  void
189  setEpsilon(float eps) override;
190 
191  void
192  setSortedResults(bool sorted);
193 
194  inline Ptr
196  {
197  return Ptr(new KdTreeFLANN<PointT, Dist>(*this));
198  }
199 
200  /** \brief Destructor for KdTreeFLANN.
201  * Deletes all allocated data arrays and destroys the kd-tree structures.
202  */
203  ~KdTreeFLANN() override
204  {
205  cleanup();
206  }
207 
208  /** \brief Provide a pointer to the input dataset.
209  * \param[in] cloud the const boost shared pointer to a PointCloud message
210  * \param[in] indices the point indices subset that is to be used from \a cloud - if
211  * NULL the whole cloud is used
212  */
213  void
214  setInputCloud(const PointCloudConstPtr& cloud,
215  const IndicesConstPtr& indices = IndicesConstPtr()) override;
216 
217  /** \brief Search for k-nearest neighbors for the given query point.
218  *
219  * \attention This method does not do any bounds checking for the input index
220  * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
221  *
222  * \param[in] point a given \a valid (i.e., finite) query point
223  * \param[in] k the number of neighbors to search for
224  * \param[out] k_indices the resultant indices of the neighboring points (must be
225  * resized to \a k a priori!) \param[out] k_sqr_distances the resultant squared
226  * distances to the neighboring points (must be resized to \a k a priori!) \return
227  * number of neighbors found
228  *
229  * \exception asserts in debug mode if the index is not between 0 and the maximum
230  * number of points
231  */
232  int
233  nearestKSearch(const PointT& point,
234  unsigned int k,
235  Indices& k_indices,
236  std::vector<float>& k_sqr_distances) const override;
237 
238  /** \brief Search for all the nearest neighbors of the query point in a given radius.
239  *
240  * \attention This method does not do any bounds checking for the input index
241  * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
242  *
243  * \param[in] point a given \a valid (i.e., finite) query point
244  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
245  * \param[out] k_indices the resultant indices of the neighboring points
246  * \param[out] k_sqr_distances the resultant squared distances to the neighboring
247  * points \param[in] max_nn if given, bounds the maximum returned neighbors to this
248  * value. If \a max_nn is set to 0 or to a number higher than the number of points in
249  * the input cloud, all neighbors in \a radius will be returned. \return number of
250  * neighbors found in radius
251  *
252  * \exception asserts in debug mode if the index is not between 0 and the maximum
253  * number of points
254  */
255  int
256  radiusSearch(const PointT& point,
257  double radius,
258  Indices& k_indices,
259  std::vector<float>& k_sqr_distances,
260  unsigned int max_nn = 0) const override;
261 
262 private:
263  /** \brief Internal cleanup method. */
264  void
265  cleanup();
266 
267  /** \brief Converts a PointCloud to the internal FLANN point array representation.
268  * Returns the number of points. \param cloud the PointCloud
269  */
270  void
271  convertCloudToArray(const PointCloud& cloud);
272 
273  /** \brief Converts a PointCloud with a given set of indices to the internal FLANN
274  * point array representation. Returns the number of points. \param[in] cloud the
275  * PointCloud data \param[in] indices the point cloud indices
276  */
277  void
278  convertCloudToArray(const PointCloud& cloud, const Indices& indices);
279 
280 private:
281  /** \brief Class getName method. */
282  std::string
283  getName() const override
284  {
285  return ("KdTreeFLANN");
286  }
287 
288  /** \brief A FLANN index object. */
289  std::shared_ptr<FLANNIndex> flann_index_;
290 
291  /** \brief Internal pointer to data. TODO: replace with std::shared_ptr<float[]> with
292  * C++17*/
293  std::shared_ptr<float> cloud_;
294 
295  /** \brief mapping between internal and external indices. */
296  std::vector<int> index_mapping_;
297 
298  /** \brief whether the mapping between internal and external indices is identity */
299  bool identity_mapping_{false};
300 
301  /** \brief Tree dimensionality (i.e. the number of dimensions per point). */
302  int dim_{0};
303 
304  /** \brief The total size of the data (either equal to the number of points in the
305  * input cloud or to the number of indices - if passed). */
306  uindex_t total_nr_points_{0};
307 
308  /** \brief The KdTree search parameters for K-nearest neighbors. */
309  ::flann::SearchParams param_k_;
310 
311  /** \brief The KdTree search parameters for radius search. */
312  ::flann::SearchParams param_radius_;
313  };
314 }
315 
316 #ifdef PCL_NO_PRECOMPILE
317 #include <pcl/kdtree/impl/kdtree_flann.hpp>
318 #endif
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:132
KdTreeFLANN< PointT, Dist > & operator=(const KdTreeFLANN< PointT, Dist > &k)
Copy operator.
Definition: kdtree_flann.h:171
void setEpsilon(float eps) override
Set the search epsilon precision (error bound) for nearest neighbors searches.
~KdTreeFLANN() override
Destructor for KdTreeFLANN.
Definition: kdtree_flann.h:203
shared_ptr< const Indices > IndicesConstPtr
Definition: kdtree_flann.h:146
shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
Definition: kdtree_flann.h:151
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
shared_ptr< Indices > IndicesPtr
Definition: kdtree.h:58
shared_ptr< KdTree< PointT > > Ptr
Definition: kdtree.h:69
shared_ptr< const KdTree< PointT > > ConstPtr
Definition: kdtree.h:70
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
std::enable_if_t< compat_with_flann< IndexT >::value, bool > CompatWithFlann
Definition: kdtree_flann.h:67
std::enable_if_t<!compat_with_flann< IndexT >::value, bool > NotCompatWithFlann
Definition: kdtree_flann.h:69
shared_ptr< const Indices > IndicesConstPtr
Definition: pcl_base.h:59
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.