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