Point Cloud Library (PCL)  1.7.1
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  * \author Andreas Muetzel
64  * \ingroup search
65  */
66  template<typename PointT, typename FlannDistance=flann::L2_Simple <float> >
67  class FlannSearch: public Search<PointT>
68  {
69  typedef typename Search<PointT>::PointCloud PointCloud;
70  typedef typename Search<PointT>::PointCloudConstPtr PointCloudConstPtr;
71 
72  typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
73  typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
75  typedef boost::shared_ptr<flann::NNIndex <FlannDistance > > IndexPtr;
76  typedef boost::shared_ptr<flann::Matrix <float> > MatrixPtr;
77  typedef boost::shared_ptr<const flann::Matrix <float> > MatrixConstPtr;
78 
80  //typedef boost::shared_ptr<PointRepresentation> PointRepresentationPtr;
81  typedef boost::shared_ptr<const PointRepresentation> PointRepresentationConstPtr;
82 
86 
87  public:
88  typedef boost::shared_ptr<FlannSearch<PointT, FlannDistance> > Ptr;
89  typedef boost::shared_ptr<const FlannSearch<PointT, FlannDistance> > ConstPtr;
90 
91  /** \brief Helper class that creates a FLANN index from a given FLANN matrix. To
92  * use a FLANN index type with FlannSearch, implement this interface and
93  * pass an object of the new type to the FlannSearch constructor.
94  * See the implementation of KdTreeIndexCreator for an example.
95  */
97  {
98  public:
99  /** \brief Create a FLANN Index from the input data.
100  * \param[in] data The FLANN matrix containing the input.
101  * \return The FLANN index.
102  */
103  virtual IndexPtr createIndex (MatrixConstPtr data)=0;
104 
105  /** \brief destructor
106  */
107  virtual ~FlannIndexCreator () {}
108  };
109  typedef boost::shared_ptr<FlannIndexCreator> FlannIndexCreatorPtr;
110 
111  /** \brief Creates a FLANN KdTreeSingleIndex from the given input data.
112  */
114  {
115  public:
116  /** \param[in] max_leaf_size All FLANN kd trees created by this class will have
117  * a maximum of max_leaf_size points per leaf node. Higher values make index creation
118  * cheaper, but search more costly (and the other way around).
119  */
120  KdTreeIndexCreator (unsigned int max_leaf_size=15) : max_leaf_size_ (max_leaf_size){}
121 
122  /** \brief Empty destructor */
123  virtual ~KdTreeIndexCreator () {}
124 
125  /** \brief Create a FLANN Index from the input data.
126  * \param[in] data The FLANN matrix containing the input.
127  * \return The FLANN index.
128  */
129  virtual IndexPtr createIndex (MatrixConstPtr data);
130  private:
131  unsigned int max_leaf_size_;
132  };
133 
134  /** \brief Creates a FLANN KdTreeSingleIndex from the given input data.
135  */
137  {
138  public:
139  /** \param[in] max_leaf_size All FLANN kd trees created by this class will have
140  * a maximum of max_leaf_size points per leaf node. Higher values make index creation
141  * cheaper, but search more costly (and the other way around).
142  */
144 
145  /** \brief Empty destructor */
146  virtual ~KMeansIndexCreator () {}
147 
148  /** \brief Create a FLANN Index from the input data.
149  * \param[in] data The FLANN matrix containing the input.
150  * \return The FLANN index.
151  */
152  virtual IndexPtr createIndex (MatrixConstPtr data);
153  private:
154  };
155 
156  FlannSearch (bool sorted = true, FlannIndexCreatorPtr creator = FlannIndexCreatorPtr (new KdTreeIndexCreator ()));
157 
158  /** \brief Destructor for FlannSearch. */
159  virtual
160  ~FlannSearch ();
161 
162 
163  //void
164  //setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ());
165 
166  /** \brief Set the search epsilon precision (error bound) for nearest neighbors searches.
167  * \param[in] eps precision (error bound) for nearest neighbors searches
168  */
169  inline void
170  setEpsilon (double eps)
171  {
172  eps_ = eps;
173  }
174 
175  /** \brief Get the search epsilon precision (error bound) for nearest neighbors searches. */
176  inline double
178  {
179  return (eps_);
180  }
181 
182  /** \brief Provide a pointer to the input dataset.
183  * \param[in] cloud the const boost shared pointer to a PointCloud message
184  * \param[in] indices the point indices subset that is to be used from \a cloud
185  */
186  virtual void
187  setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr& indices = IndicesConstPtr ());
188 
189  /** \brief Search for the k-nearest neighbors for the given query point.
190  * \param[in] point the given query point
191  * \param[in] k the number of neighbors to search for
192  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
193  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
194  * a priori!)
195  * \return number of neighbors found
196  */
197  int
198  nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const;
199 
200 
201  /** \brief Search for the k-nearest neighbors for the given query point.
202  * \param[in] cloud the point cloud data
203  * \param[in] indices a vector of point cloud indices to query for nearest neighbors
204  * \param[in] k the number of neighbors to search for
205  * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
206  * \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
207  */
208  virtual void
209  nearestKSearch (const PointCloud& cloud, const std::vector<int>& indices, int k,
210  std::vector< std::vector<int> >& k_indices, std::vector< std::vector<float> >& k_sqr_distances) const;
211 
212  /** \brief Search for all the nearest neighbors of the query point in a given radius.
213  * \param[in] point the given query point
214  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
215  * \param[out] k_indices the resultant indices of the neighboring points
216  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
217  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
218  * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
219  * returned.
220  * \return number of neighbors found in radius
221  */
222  int
223  radiusSearch (const PointT& point, double radius,
224  std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
225  unsigned int max_nn = 0) const;
226 
227  /** \brief Search for the k-nearest neighbors for the given query point.
228  * \param[in] cloud the point cloud data
229  * \param[in] indices a vector of point cloud indices to query for nearest neighbors
230  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
231  * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
232  * \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
233  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value
234  */
235  virtual void
236  radiusSearch (const PointCloud& cloud, const std::vector<int>& indices, double radius, std::vector< std::vector<int> >& k_indices,
237  std::vector< std::vector<float> >& k_sqr_distances, unsigned int max_nn=0) const;
238 
239  /** \brief Provide a pointer to the point representation to use to convert points into k-D vectors.
240  * \param[in] point_representation the const boost shared pointer to a PointRepresentation
241  */
242  inline void
243  setPointRepresentation (const PointRepresentationConstPtr &point_representation)
244  {
245  point_representation_ = point_representation;
246  dim_ = point_representation->getNumberOfDimensions ();
247  if (input_) // re-create the tree, since point_represenation might change things such as the scaling of the point clouds.
249  }
250 
251  /** \brief Get a pointer to the point representation used when converting points into k-D vectors. */
252  inline PointRepresentationConstPtr const
254  {
255  return (point_representation_);
256  }
257 
258  protected:
259 
260  /** \brief converts the input data to a format usable by FLANN
261  */
263 
264  /** The FLANN index.
265  */
266  IndexPtr index_;
267 
268  /** The index creator, used to (re-) create the index when the search data is passed.
269  */
271 
272  /** Input data in FLANN format.
273  */
274  MatrixPtr input_flann_;
275 
276  /** Epsilon for approximate NN search.
277  */
278  float eps_;
280 
281  PointRepresentationConstPtr point_representation_;
282 
283  int dim_;
284 
285  std::vector<int> index_mapping_;
287 
288  };
289  }
290 }
291 
292 #define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch<T>;
293 
294 #endif // PCL_SEARCH_KDTREE_H_
295