Point Cloud Library (PCL)  1.14.0-dev
flann_search.h
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 #pragma once
40 
41 #include <pcl/search/search.h>
42 #include <pcl/common/time.h>
43 #include <pcl/point_representation.h>
44 
45 namespace flann
46 {
47  template<typename T> class NNIndex;
48  template<typename T> struct L2;
49  template<typename T> struct L2_Simple;
50  template<typename T> class Matrix;
51 }
52 
53 namespace pcl
54 {
55  namespace search
56  {
57 
58  /** \brief @b search::FlannSearch is a generic FLANN wrapper class for the new search interface.
59  * It is able to wrap any FLANN index type, e.g. the kd tree as well as indices for high-dimensional
60  * searches and intended as a more powerful and cleaner successor to KdTreeFlann.
61  *
62  * By default, this class creates a single kd tree for indexing the input data. However, for high dimensions
63  * (> 10), it is often better to use the multiple randomized kd tree index provided by FLANN in combination with
64  * the \ref flann::L2 distance functor. During search in this type of index, the number of checks to perform before
65  * terminating the search can be controlled. Here is a code example if a high-dimensional 2-NN search:
66  *
67  * \code
68  * // Feature and distance type
69  * using FeatureT = SHOT352;
70  * using DistanceT = flann::L2<float>;
71  *
72  * // Search and index types
73  * using SearchT = search::FlannSearch<FeatureT, DistanceT>;
74  * using CreatorPtrT = typename SearchT::FlannIndexCreatorPtr;
75  * using IndexT = typename SearchT::KdTreeMultiIndexCreator;
76  * using RepresentationPtrT = typename SearchT::PointRepresentationPtr;
77  *
78  * // Features
79  * PointCloud<FeatureT>::Ptr query, target;
80  *
81  * // Fill query and target with calculated features...
82  *
83  * // Instantiate search object with 4 randomized trees and 256 checks
84  * SearchT search (true, CreatorPtrT (new IndexT (4)));
85  * search.setPointRepresentation (RepresentationPtrT (new DefaultFeatureRepresentation<FeatureT>));
86  * search.setChecks (256);
87  * search.setInputCloud (target);
88  *
89  * // Do search
90  * std::vector<Indices> k_indices;
91  * std::vector<std::vector<float> > k_sqr_distances;
92  * search.nearestKSearch (*query, Indices (), 2, k_indices, k_sqr_distances);
93  * \endcode
94  *
95  * \author Andreas Muetzel
96  * \author Anders Glent Buch (multiple randomized kd tree interface)
97  * \ingroup search
98  */
99  template<typename PointT, typename FlannDistance=flann::L2_Simple <float> >
100  class FlannSearch: public Search<PointT>
101  {
105 
106  public:
107  using Ptr = shared_ptr<FlannSearch<PointT, FlannDistance> >;
108  using ConstPtr = shared_ptr<const FlannSearch<PointT, FlannDistance> >;
109 
112 
113  using MatrixPtr = shared_ptr<flann::Matrix<float> >;
114  using MatrixConstPtr = shared_ptr<const flann::Matrix<float> >;
115 
117  using IndexPtr = shared_ptr<flann::NNIndex<FlannDistance> >;
118 
122 
123  /** \brief Helper class that creates a FLANN index from a given FLANN matrix. To
124  * use a FLANN index type with FlannSearch, implement this interface and
125  * pass an object of the new type to the FlannSearch constructor.
126  * See the implementation of KdTreeIndexCreator for an example.
127  */
129  {
130  public:
131  /** \brief Create a FLANN Index from the input data.
132  * \param[in] data The FLANN matrix containing the input.
133  * \return The FLANN index.
134  */
136 
137  /** \brief destructor
138  */
139  virtual ~FlannIndexCreator () = default;
140  };
141  using FlannIndexCreatorPtr = shared_ptr<FlannIndexCreator>;
142 
143  /** \brief Creates a FLANN KdTreeSingleIndex from the given input data.
144  */
146  {
147  public:
148  /** \param[in] max_leaf_size All FLANN kd trees created by this class will have
149  * a maximum of max_leaf_size points per leaf node. Higher values make index creation
150  * cheaper, but search more costly (and the other way around).
151  */
152  KdTreeIndexCreator (unsigned int max_leaf_size=15) : max_leaf_size_ (max_leaf_size){}
153 
154  /** \brief Empty destructor */
155  ~KdTreeIndexCreator () override = default;
156 
157  /** \brief Create a FLANN Index from the input data.
158  * \param[in] data The FLANN matrix containing the input.
159  * \return The FLANN index.
160  */
161  IndexPtr createIndex (MatrixConstPtr data) override;
162  private:
163  unsigned int max_leaf_size_;
164  };
165 
166  /** \brief Creates a FLANN KdTreeSingleIndex from the given input data.
167  */
169  {
170  public:
171  /** \brief All FLANN kd trees created by this class will have
172  * a maximum of max_leaf_size points per leaf node. Higher values make index creation
173  * cheaper, but search more costly (and the other way around).
174  */
175  KMeansIndexCreator () = default;
176 
177  /** \brief Empty destructor */
178  virtual ~KMeansIndexCreator () = default;
179 
180  /** \brief Create a FLANN Index from the input data.
181  * \param[in] data The FLANN matrix containing the input.
182  * \return The FLANN index.
183  */
184  virtual IndexPtr createIndex (MatrixConstPtr data);
185  private:
186  };
187 
188  /** \brief Creates a FLANN KdTreeIndex of multiple randomized trees from the given input data,
189  * suitable for feature matching. Note that in this case, it is often more efficient to use the
190  * \ref flann::L2 distance functor.
191  */
193  {
194  public:
195  /** \param[in] trees Number of randomized trees to create.
196  */
197  KdTreeMultiIndexCreator (int trees = 4) : trees_ (trees) {}
198 
199  /** \brief Empty destructor */
200  virtual ~KdTreeMultiIndexCreator () = default;
201 
202  /** \brief Create a FLANN Index from the input data.
203  * \param[in] data The FLANN matrix containing the input.
204  * \return The FLANN index.
205  */
206  virtual IndexPtr createIndex (MatrixConstPtr data);
207  private:
208  int trees_;
209  };
210 
211  FlannSearch (bool sorted = true, FlannIndexCreatorPtr creator = FlannIndexCreatorPtr (new KdTreeIndexCreator ()));
212 
213  /** \brief Destructor for FlannSearch. */
214 
215  ~FlannSearch () override;
216 
217 
218  //void
219  //setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ());
220 
221  /** \brief Set the search epsilon precision (error bound) for nearest neighbors searches.
222  * \param[in] eps precision (error bound) for nearest neighbors searches
223  */
224  inline void
225  setEpsilon (double eps)
226  {
227  eps_ = eps;
228  }
229 
230  /** \brief Get the search epsilon precision (error bound) for nearest neighbors searches. */
231  inline double
233  {
234  return (eps_);
235  }
236 
237  /** \brief Set the number of checks to perform during approximate searches in multiple randomized trees.
238  * \param[in] checks number of checks to perform during approximate searches in multiple randomized trees.
239  */
240  inline void
241  setChecks (int checks)
242  {
243  checks_ = checks;
244  }
245 
246  /** \brief Get the number of checks to perform during approximate searches in multiple randomized trees. */
247  inline int
249  {
250  return (checks_);
251  }
252 
253  /** \brief Provide a pointer to the input dataset.
254  * \param[in] cloud the const boost shared pointer to a PointCloud message
255  * \param[in] indices the point indices subset that is to be used from \a cloud
256  */
257  bool
258  setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr& indices = IndicesConstPtr ()) override;
259 
261 
262  /** \brief Search for the k-nearest neighbors for the given query point.
263  * \param[in] point the given query point
264  * \param[in] k the number of neighbors to search for
265  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
266  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
267  * a priori!)
268  * \return number of neighbors found
269  */
270  int
271  nearestKSearch (const PointT &point, int k, Indices &k_indices, std::vector<float> &k_sqr_distances) const override;
272 
273 
274  /** \brief Search for the k-nearest neighbors for the given query point.
275  * \param[in] cloud the point cloud data
276  * \param[in] indices a vector of point cloud indices to query for nearest neighbors
277  * \param[in] k the number of neighbors to search for
278  * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
279  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points, k_sqr_distances[i] corresponds to the neighbors of the query point i
280  */
281  void
282  nearestKSearch (const PointCloud& cloud, const Indices& indices, int k,
283  std::vector<Indices>& k_indices, std::vector< std::vector<float> >& k_sqr_distances) const override;
284 
285  /** \brief Search for all the nearest neighbors of the query point in a given radius.
286  * \param[in] point the given query point
287  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
288  * \param[out] k_indices the resultant indices of the neighboring points
289  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
290  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
291  * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
292  * returned.
293  * \return number of neighbors found in radius
294  */
295  int
296  radiusSearch (const PointT& point, double radius,
297  Indices &k_indices, std::vector<float> &k_sqr_distances,
298  unsigned int max_nn = 0) const override;
299 
300  /** \brief Search for the k-nearest neighbors for the given query point.
301  * \param[in] cloud the point cloud data
302  * \param[in] indices a vector of point cloud indices to query for nearest neighbors
303  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
304  * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
305  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points, k_sqr_distances[i] corresponds to the neighbors of the query point i
306  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
307  */
308  void
309  radiusSearch (const PointCloud& cloud, const Indices& indices, double radius, std::vector<Indices>& k_indices,
310  std::vector< std::vector<float> >& k_sqr_distances, unsigned int max_nn=0) const override;
311 
312  /** \brief Provide a pointer to the point representation to use to convert points into k-D vectors.
313  * \param[in] point_representation the const boost shared pointer to a PointRepresentation
314  */
315  inline void
317  {
318  point_representation_ = point_representation;
319  dim_ = point_representation->getNumberOfDimensions ();
320  if (input_) // re-create the tree, since point_representation might change things such as the scaling of the point clouds.
322  }
323 
324  /** \brief Get a pointer to the point representation used when converting points into k-D vectors. */
325  inline PointRepresentationConstPtr const
327  {
328  return (point_representation_);
329  }
330 
331  protected:
332 
333  /** \brief converts the input data to a format usable by FLANN
334  */
336 
337  /** The FLANN index.
338  */
340 
341  /** The index creator, used to (re-) create the index when the search data is passed.
342  */
344 
345  /** Input data in FLANN format.
346  */
348 
349  /** Epsilon for approximate NN search.
350  */
351  float eps_{0.0f};
352 
353  /** Number of checks to perform for approximate NN search using the multiple randomized tree index
354  */
355  int checks_{32};
356 
358 
360 
361  int dim_{0};
362 
364  bool identity_mapping_{false};
365 
366  std::size_t total_nr_points_{0};
367 
368  };
369  }
370 }
371 
372 // There is no cpp file containing template instantiations of FlannSearch
373 #include <pcl/search/impl/flann_search.hpp>
374 
375 #define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch<T>;
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
PointRepresentation provides a set of methods for converting a point structs/object into an n-dimensi...
shared_ptr< const PointRepresentation< PointT > > ConstPtr
shared_ptr< PointRepresentation< PointT > > Ptr
Helper class that creates a FLANN index from a given FLANN matrix.
Definition: flann_search.h:129
virtual IndexPtr createIndex(MatrixConstPtr data)=0
Create a FLANN Index from the input data.
virtual ~FlannIndexCreator()=default
destructor
Creates a FLANN KdTreeSingleIndex from the given input data.
Definition: flann_search.h:169
KMeansIndexCreator()=default
All FLANN kd trees created by this class will have a maximum of max_leaf_size points per leaf node.
virtual IndexPtr createIndex(MatrixConstPtr data)
Create a FLANN Index from the input data.
virtual ~KMeansIndexCreator()=default
Empty destructor.
Creates a FLANN KdTreeSingleIndex from the given input data.
Definition: flann_search.h:146
~KdTreeIndexCreator() override=default
Empty destructor.
KdTreeIndexCreator(unsigned int max_leaf_size=15)
Definition: flann_search.h:152
IndexPtr createIndex(MatrixConstPtr data) override
Create a FLANN Index from the input data.
Creates a FLANN KdTreeIndex of multiple randomized trees from the given input data,...
Definition: flann_search.h:193
virtual IndexPtr createIndex(MatrixConstPtr data)
Create a FLANN Index from the input data.
virtual ~KdTreeMultiIndexCreator()=default
Empty destructor.
search::FlannSearch is a generic FLANN wrapper class for the new search interface.
Definition: flann_search.h:101
void setEpsilon(double eps)
Set the search epsilon precision (error bound) for nearest neighbors searches.
Definition: flann_search.h:225
void convertInputToFlannMatrix()
converts the input data to a format usable by FLANN
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.
std::size_t total_nr_points_
Definition: flann_search.h:366
shared_ptr< flann::NNIndex< FlannDistance > > IndexPtr
Definition: flann_search.h:117
int checks_
Number of checks to perform for approximate NN search using the multiple randomized tree index.
Definition: flann_search.h:355
double getEpsilon()
Get the search epsilon precision (error bound) for nearest neighbors searches.
Definition: flann_search.h:232
shared_ptr< const flann::Matrix< float > > MatrixConstPtr
Definition: flann_search.h:114
FlannSearch(bool sorted=true, FlannIndexCreatorPtr creator=FlannIndexCreatorPtr(new KdTreeIndexCreator()))
~FlannSearch() override
Destructor for FlannSearch.
void setChecks(int checks)
Set the number of checks to perform during approximate searches in multiple randomized trees.
Definition: flann_search.h:241
MatrixPtr input_flann_
Input data in FLANN format.
Definition: flann_search.h:347
bool setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
shared_ptr< FlannSearch< PointT, FlannDistance > > Ptr
Definition: flann_search.h:107
PointRepresentationConstPtr point_representation_
Definition: flann_search.h:359
shared_ptr< FlannIndexCreator > FlannIndexCreatorPtr
Definition: flann_search.h:141
int nearestKSearch(const PointT &point, int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const override
Search for the k-nearest neighbors for the given query point.
typename Search< PointT >::PointCloudConstPtr PointCloudConstPtr
Definition: flann_search.h:111
typename Search< PointT >::PointCloud PointCloud
Definition: flann_search.h:110
FlannIndexCreatorPtr creator_
The index creator, used to (re-) create the index when the search data is passed.
Definition: flann_search.h:343
IndexPtr index_
The FLANN index.
Definition: flann_search.h:339
typename PointRepresentation::ConstPtr PointRepresentationConstPtr
Definition: flann_search.h:121
int getChecks()
Get the number of checks to perform during approximate searches in multiple randomized trees.
Definition: flann_search.h:248
void setPointRepresentation(const PointRepresentationConstPtr &point_representation)
Provide a pointer to the point representation to use to convert points into k-D vectors.
Definition: flann_search.h:316
PointRepresentationConstPtr const getPointRepresentation()
Get a pointer to the point representation used when converting points into k-D vectors.
Definition: flann_search.h:326
typename PointRepresentation::Ptr PointRepresentationPtr
Definition: flann_search.h:120
float eps_
Epsilon for approximate NN search.
Definition: flann_search.h:351
shared_ptr< flann::Matrix< float > > MatrixPtr
Definition: flann_search.h:113
shared_ptr< const FlannSearch< PointT, FlannDistance > > ConstPtr
Definition: flann_search.h:108
Generic search class.
Definition: search.h:75
PointCloudConstPtr input_
Definition: search.h:402
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: search.h:79
IndicesConstPtr indices_
Definition: search.h:403
Define methods for measuring time spent in code blocks.
shared_ptr< const Indices > IndicesConstPtr
Definition: pcl_base.h:59
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
A point structure representing Euclidean xyz coordinates, and the RGB color.