Point Cloud Library (PCL)  1.12.0-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 Comaptibility 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 Comaptibility 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() { cleanup(); }
204 
205  /** \brief Provide a pointer to the input dataset.
206  * \param[in] cloud the const boost shared pointer to a PointCloud message
207  * \param[in] indices the point indices subset that is to be used from \a cloud - if
208  * NULL the whole cloud is used
209  */
210  void
211  setInputCloud(const PointCloudConstPtr& cloud,
212  const IndicesConstPtr& indices = IndicesConstPtr()) override;
213 
214  /** \brief Search for k-nearest neighbors for the given query point.
215  *
216  * \attention This method does not do any bounds checking for the input index
217  * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
218  *
219  * \param[in] point a given \a valid (i.e., finite) query point
220  * \param[in] k the number of neighbors to search for
221  * \param[out] k_indices the resultant indices of the neighboring points (must be
222  * resized to \a k a priori!) \param[out] k_sqr_distances the resultant squared
223  * distances to the neighboring points (must be resized to \a k a priori!) \return
224  * number of neighbors found
225  *
226  * \exception asserts in debug mode if the index is not between 0 and the maximum
227  * number of points
228  */
229  int
230  nearestKSearch(const PointT& point,
231  unsigned int k,
232  Indices& k_indices,
233  std::vector<float>& k_sqr_distances) const override;
234 
235  /** \brief Search for all the nearest neighbors of the query point in a given radius.
236  *
237  * \attention This method does not do any bounds checking for the input index
238  * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
239  *
240  * \param[in] point a given \a valid (i.e., finite) query point
241  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
242  * \param[out] k_indices the resultant indices of the neighboring points
243  * \param[out] k_sqr_distances the resultant squared distances to the neighboring
244  * points \param[in] max_nn if given, bounds the maximum returned neighbors to this
245  * value. If \a max_nn is set to 0 or to a number higher than the number of points in
246  * the input cloud, all neighbors in \a radius will be returned. \return number of
247  * neighbors found in radius
248  *
249  * \exception asserts in debug mode if the index is not between 0 and the maximum
250  * number of points
251  */
252  int
253  radiusSearch(const PointT& point,
254  double radius,
255  Indices& k_indices,
256  std::vector<float>& k_sqr_distances,
257  unsigned int max_nn = 0) const override;
258 
259 private:
260  /** \brief Internal cleanup method. */
261  void
262  cleanup();
263 
264  /** \brief Converts a PointCloud to the internal FLANN point array representation.
265  * Returns the number of points. \param cloud the PointCloud
266  */
267  void
268  convertCloudToArray(const PointCloud& cloud);
269 
270  /** \brief Converts a PointCloud with a given set of indices to the internal FLANN
271  * point array representation. Returns the number of points. \param[in] cloud the
272  * PointCloud data \param[in] indices the point cloud indices
273  */
274  void
275  convertCloudToArray(const PointCloud& cloud, const Indices& indices);
276 
277 private:
278  /** \brief Class getName method. */
279  std::string
280  getName() const override
281  {
282  return ("KdTreeFLANN");
283  }
284 
285  /** \brief A FLANN index object. */
286  std::shared_ptr<FLANNIndex> flann_index_;
287 
288  /** \brief Internal pointer to data. TODO: replace with std::shared_ptr<float[]> with
289  * C++17*/
290  std::shared_ptr<float> cloud_;
291 
292  /** \brief mapping between internal and external indices. */
293  std::vector<int> index_mapping_;
294 
295  /** \brief whether the mapping between internal and external indices is identity */
296  bool identity_mapping_;
297 
298  /** \brief Tree dimensionality (i.e. the number of dimensions per point). */
299  int dim_;
300 
301  /** \brief The total size of the data (either equal to the number of points in the
302  * input cloud or to the number of indices - if passed). */
303  uindex_t total_nr_points_;
304 
305  /** \brief The KdTree search parameters for K-nearest neighbors. */
306  ::flann::SearchParams param_k_;
307 
308  /** \brief The KdTree search parameters for radius search. */
309  ::flann::SearchParams param_radius_;
310  };
311 }
312 
313 #ifdef PCL_NO_PRECOMPILE
314 #include <pcl/kdtree/impl/kdtree_flann.hpp>
315 #endif
pcl
Definition: convolution.h:46
pcl::KdTreeFLANN::makeShared
Ptr makeShared()
Definition: kdtree_flann.h:195
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::KdTree< FeatureT >::ConstPtr
shared_ptr< const KdTree< FeatureT > > ConstPtr
Definition: kdtree.h:69
pcl::IndicesConstPtr
shared_ptr< const Indices > IndicesConstPtr
Definition: pcl_base.h:59
pcl::KdTreeFLANN::~KdTreeFLANN
~KdTreeFLANN()
Destructor for KdTreeFLANN.
Definition: kdtree_flann.h:203
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< FeatureT >
pcl::KdTree< FeatureT >::IndicesPtr
shared_ptr< Indices > IndicesPtr
Definition: kdtree.h:57
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
pcl::detail::CompatWithFlann
std::enable_if_t< compat_with_flann< IndexT >::value, bool > CompatWithFlann
Definition: kdtree_flann.h:67
pcl::KdTreeFLANN::operator=
KdTreeFLANN< PointT, Dist > & operator=(const KdTreeFLANN< PointT, Dist > &k)
Copy operator.
Definition: kdtree_flann.h:171
flann::Index
Definition: kdtree_flann.h:52
pcl::detail::NotCompatWithFlann
std::enable_if_t<!compat_with_flann< IndexT >::value, bool > NotCompatWithFlann
Definition: kdtree_flann.h:69
pcl::KdTreeFLANN
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:132
pcl::KdTreeFLANN::IndicesConstPtr
shared_ptr< const Indices > IndicesConstPtr
Definition: kdtree_flann.h:146
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::KdTree< FeatureT >::Ptr
shared_ptr< KdTree< FeatureT > > Ptr
Definition: kdtree.h:68
flann
Definition: kdtree_flann.h:49
pcl::detail::compat_with_flann
Definition: kdtree_flann.h:61
pcl::KdTreeFLANN::KdTreeFLANN
KdTreeFLANN(bool sorted=true)
Default Constructor for KdTreeFLANN.
Definition: kdtree_flann.hpp:49
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
flann::L2_Simple
Definition: kdtree_flann.h:51
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::Ptr
shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
Definition: kdtree_flann.h:151
pcl::KdTreeFLANN::setSortedResults
void setSortedResults(bool sorted)
Definition: kdtree_flann.hpp:93
pcl::KdTree< FeatureT >::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: kdtree.h:62