Point Cloud Library (PCL)  1.9.1-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<std::vector<int> > k_indices;
91  * std::vector<std::vector<float> > k_sqr_distances;
92  * search.nearestKSearch (*query, std::vector<int> (), 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 = boost::shared_ptr<FlannSearch<PointT, FlannDistance> >;
108  using ConstPtr = boost::shared_ptr<const FlannSearch<PointT, FlannDistance> >;
109 
112 
113  using IndicesPtr = boost::shared_ptr<std::vector<int> >;
114  using IndicesConstPtr = boost::shared_ptr<const std::vector<int> >;
115 
116  using MatrixPtr = boost::shared_ptr<flann::Matrix<float> >;
117  using MatrixConstPtr = boost::shared_ptr<const flann::Matrix<float> >;
118 
120  using IndexPtr = boost::shared_ptr<flann::NNIndex<FlannDistance> >;
121 
123  using PointRepresentationPtr = boost::shared_ptr<PointRepresentation>;
124  using PointRepresentationConstPtr = boost::shared_ptr<const PointRepresentation>;
125 
126  /** \brief Helper class that creates a FLANN index from a given FLANN matrix. To
127  * use a FLANN index type with FlannSearch, implement this interface and
128  * pass an object of the new type to the FlannSearch constructor.
129  * See the implementation of KdTreeIndexCreator for an example.
130  */
132  {
133  public:
134  /** \brief Create a FLANN Index from the input data.
135  * \param[in] data The FLANN matrix containing the input.
136  * \return The FLANN index.
137  */
138  virtual IndexPtr createIndex (MatrixConstPtr data)=0;
139 
140  /** \brief destructor
141  */
142  virtual ~FlannIndexCreator () {}
143  };
144  using FlannIndexCreatorPtr = boost::shared_ptr<FlannIndexCreator>;
145 
146  /** \brief Creates a FLANN KdTreeSingleIndex from the given input data.
147  */
149  {
150  public:
151  /** \param[in] max_leaf_size All FLANN kd trees created by this class will have
152  * a maximum of max_leaf_size points per leaf node. Higher values make index creation
153  * cheaper, but search more costly (and the other way around).
154  */
155  KdTreeIndexCreator (unsigned int max_leaf_size=15) : max_leaf_size_ (max_leaf_size){}
156 
157  /** \brief Empty destructor */
159 
160  /** \brief Create a FLANN Index from the input data.
161  * \param[in] data The FLANN matrix containing the input.
162  * \return The FLANN index.
163  */
164  IndexPtr createIndex (MatrixConstPtr data) override;
165  private:
166  unsigned int max_leaf_size_;
167  };
168 
169  /** \brief Creates a FLANN KdTreeSingleIndex from the given input data.
170  */
172  {
173  public:
174  /** \brief All FLANN kd trees created by this class will have
175  * a maximum of max_leaf_size points per leaf node. Higher values make index creation
176  * cheaper, but search more costly (and the other way around).
177  */
179 
180  /** \brief Empty destructor */
181  virtual ~KMeansIndexCreator () {}
182 
183  /** \brief Create a FLANN Index from the input data.
184  * \param[in] data The FLANN matrix containing the input.
185  * \return The FLANN index.
186  */
187  virtual IndexPtr createIndex (MatrixConstPtr data);
188  private:
189  };
190 
191  /** \brief Creates a FLANN KdTreeIndex of multiple randomized trees from the given input data,
192  * suitable for feature matching. Note that in this case, it is often more efficient to use the
193  * \ref flann::L2 distance functor.
194  */
196  {
197  public:
198  /** \param[in] trees Number of randomized trees to create.
199  */
200  KdTreeMultiIndexCreator (int trees = 4) : trees_ (trees) {}
201 
202  /** \brief Empty destructor */
204 
205  /** \brief Create a FLANN Index from the input data.
206  * \param[in] data The FLANN matrix containing the input.
207  * \return The FLANN index.
208  */
209  virtual IndexPtr createIndex (MatrixConstPtr data);
210  private:
211  int trees_;
212  };
213 
214  FlannSearch (bool sorted = true, FlannIndexCreatorPtr creator = FlannIndexCreatorPtr (new KdTreeIndexCreator ()));
215 
216  /** \brief Destructor for FlannSearch. */
217 
218  ~FlannSearch ();
219 
220 
221  //void
222  //setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ());
223 
224  /** \brief Set the search epsilon precision (error bound) for nearest neighbors searches.
225  * \param[in] eps precision (error bound) for nearest neighbors searches
226  */
227  inline void
228  setEpsilon (double eps)
229  {
230  eps_ = eps;
231  }
232 
233  /** \brief Get the search epsilon precision (error bound) for nearest neighbors searches. */
234  inline double
236  {
237  return (eps_);
238  }
239 
240  /** \brief Set the number of checks to perform during approximate searches in multiple randomized trees.
241  * \param[in] checks number of checks to perform during approximate searches in multiple randomized trees.
242  */
243  inline void
244  setChecks (int checks)
245  {
246  checks_ = checks;
247  }
248 
249  /** \brief Get the number of checks to perform during approximate searches in multiple randomized trees. */
250  inline int
252  {
253  return (checks_);
254  }
255 
256  /** \brief Provide a pointer to the input dataset.
257  * \param[in] cloud the const boost shared pointer to a PointCloud message
258  * \param[in] indices the point indices subset that is to be used from \a cloud
259  */
260  void
261  setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr& indices = IndicesConstPtr ()) override;
262 
263  /** \brief Search for the k-nearest neighbors for the given query point.
264  * \param[in] point the given query point
265  * \param[in] k the number of neighbors to search for
266  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
267  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
268  * a priori!)
269  * \return number of neighbors found
270  */
271  int
272  nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const override;
273 
274 
275  /** \brief Search for the k-nearest neighbors for the given query point.
276  * \param[in] cloud the point cloud data
277  * \param[in] indices a vector of point cloud indices to query for nearest neighbors
278  * \param[in] k the number of neighbors to search for
279  * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
280  * \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
281  */
282  void
283  nearestKSearch (const PointCloud& cloud, const std::vector<int>& indices, int k,
284  std::vector< std::vector<int> >& k_indices, std::vector< std::vector<float> >& k_sqr_distances) const override;
285 
286  /** \brief Search for all the nearest neighbors of the query point in a given radius.
287  * \param[in] point the given query point
288  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
289  * \param[out] k_indices the resultant indices of the neighboring points
290  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
291  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
292  * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
293  * returned.
294  * \return number of neighbors found in radius
295  */
296  int
297  radiusSearch (const PointT& point, double radius,
298  std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
299  unsigned int max_nn = 0) const override;
300 
301  /** \brief Search for the k-nearest neighbors for the given query point.
302  * \param[in] cloud the point cloud data
303  * \param[in] indices a vector of point cloud indices to query for nearest neighbors
304  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
305  * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
306  * \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
307  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
308  */
309  void
310  radiusSearch (const PointCloud& cloud, const std::vector<int>& indices, double radius, std::vector< std::vector<int> >& k_indices,
311  std::vector< std::vector<float> >& k_sqr_distances, unsigned int max_nn=0) const override;
312 
313  /** \brief Provide a pointer to the point representation to use to convert points into k-D vectors.
314  * \param[in] point_representation the const boost shared pointer to a PointRepresentation
315  */
316  inline void
318  {
319  point_representation_ = point_representation;
320  dim_ = point_representation->getNumberOfDimensions ();
321  if (input_) // re-create the tree, since point_representation might change things such as the scaling of the point clouds.
322  setInputCloud (input_, indices_);
323  }
324 
325  /** \brief Get a pointer to the point representation used when converting points into k-D vectors. */
326  inline PointRepresentationConstPtr const
328  {
329  return (point_representation_);
330  }
331 
332  protected:
333 
334  /** \brief converts the input data to a format usable by FLANN
335  */
336  void convertInputToFlannMatrix();
337 
338  /** The FLANN index.
339  */
341 
342  /** The index creator, used to (re-) create the index when the search data is passed.
343  */
345 
346  /** Input data in FLANN format.
347  */
349 
350  /** Epsilon for approximate NN search.
351  */
352  float eps_;
353 
354  /** Number of checks to perform for approximate NN search using the multiple randomized tree index
355  */
356  int checks_;
357 
359 
361 
362  int dim_;
363 
364  std::vector<int> index_mapping_;
366 
367  };
368  }
369 }
370 
371 #define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch<T>;
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: flann_search.h:113
boost::shared_ptr< FlannIndexCreator > FlannIndexCreatorPtr
Definition: flann_search.h:144
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
std::vector< int > index_mapping_
Definition: flann_search.h:364
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
Definition: flann_search.h:114
IndexPtr index_
The FLANN index.
Definition: flann_search.h:340
FlannIndexCreatorPtr creator_
The index creator, used to (re-) create the index when the search data is passed. ...
Definition: flann_search.h:344
Creates a FLANN KdTreeSingleIndex from the given input data.
Definition: flann_search.h:148
boost::shared_ptr< flann::Matrix< float > > MatrixPtr
Definition: flann_search.h:116
PointRepresentationConstPtr point_representation_
Definition: flann_search.h:360
Creates a FLANN KdTreeSingleIndex from the given input data.
Definition: flann_search.h:171
Helper class that creates a FLANN index from a given FLANN matrix.
Definition: flann_search.h:131
PointRepresentation provides a set of methods for converting a point structs/object into an n-dimensi...
typename Search< PointT >::PointCloudConstPtr PointCloudConstPtr
Definition: flann_search.h:111
boost::shared_ptr< const FlannSearch< PointT, FlannDistance > > ConstPtr
Definition: flann_search.h:108
Creates a FLANN KdTreeIndex of multiple randomized trees from the given input data, suitable for feature matching.
Definition: flann_search.h:195
PointRepresentationConstPtr const getPointRepresentation()
Get a pointer to the point representation used when converting points into k-D vectors.
Definition: flann_search.h:327
boost::shared_ptr< const PointRepresentation > PointRepresentationConstPtr
Definition: flann_search.h:124
boost::shared_ptr< PointRepresentation > PointRepresentationPtr
Definition: flann_search.h:123
boost::shared_ptr< FlannSearch< PointT, FlannDistance > > Ptr
Definition: flann_search.h:107
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:317
virtual ~KMeansIndexCreator()
Empty destructor.
Definition: flann_search.h:181
boost::shared_ptr< flann::NNIndex< FlannDistance > > IndexPtr
Definition: flann_search.h:120
double getEpsilon()
Get the search epsilon precision (error bound) for nearest neighbors searches.
Definition: flann_search.h:235
PointCloud represents the base class in PCL for storing collections of 3D points. ...
MatrixPtr input_flann_
Input data in FLANN format.
Definition: flann_search.h:348
typename Search< PointT >::PointCloud PointCloud
Definition: flann_search.h:110
int getChecks()
Get the number of checks to perform during approximate searches in multiple randomized trees...
Definition: flann_search.h:251
int checks_
Number of checks to perform for approximate NN search using the multiple randomized tree index...
Definition: flann_search.h:356
void setEpsilon(double eps)
Set the search epsilon precision (error bound) for nearest neighbors searches.
Definition: flann_search.h:228
void setChecks(int checks)
Set the number of checks to perform during approximate searches in multiple randomized trees...
Definition: flann_search.h:244
A point structure representing Euclidean xyz coordinates, and the RGB color.
virtual ~KdTreeMultiIndexCreator()
Empty destructor.
Definition: flann_search.h:203
KdTreeIndexCreator(unsigned int max_leaf_size=15)
Definition: flann_search.h:155
Define methods for measuring time spent in code blocks.
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: search.h:78
boost::shared_ptr< const flann::Matrix< float > > MatrixConstPtr
Definition: flann_search.h:117
search::FlannSearch is a generic FLANN wrapper class for the new search interface.
Definition: flann_search.h:100
float eps_
Epsilon for approximate NN search.
Definition: flann_search.h:352
KMeansIndexCreator()
All FLANN kd trees created by this class will have a maximum of max_leaf_size points per leaf node...
Definition: flann_search.h:178
Generic search class.
Definition: search.h:73