Point Cloud Library (PCL)  1.9.1-dev
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  * 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  */
38 
39 #pragma once
40 
41 #include <pcl/point_cloud.h>
42 #include <pcl/for_each_type.h>
43 #include <pcl/common/concatenate.h>
44 #include <pcl/common/copy_point.h>
45 
46 namespace pcl
47 {
48  namespace search
49  {
50  /** \brief Generic search class. All search wrappers must inherit from this.
51  *
52  * Each search method must implement 2 different types of search:
53  * - \b nearestKSearch - search for K-nearest neighbors.
54  * - \b radiusSearch - search for all nearest neighbors in a sphere of a given radius
55  *
56  * The input to each search method can be given in 3 different ways:
57  * - as a query point
58  * - as a (cloud, index) pair
59  * - as an index
60  *
61  * For the latter option, it is assumed that the user specified the input
62  * via a \ref setInputCloud () method first.
63  *
64  * \note In case of an error, all methods are supposed to return 0 as the number of neighbors found.
65  *
66  * \note libpcl_search deals with three-dimensional search problems. For higher
67  * level dimensional search, please refer to the libpcl_kdtree module.
68  *
69  * \author Radu B. Rusu
70  * \ingroup search
71  */
72  template<typename PointT>
73  class Search
74  {
75  public:
77  using PointCloudPtr = typename PointCloud::Ptr;
79 
80  using Ptr = boost::shared_ptr<pcl::search::Search<PointT> >;
81  using ConstPtr = boost::shared_ptr<const pcl::search::Search<PointT> >;
82 
83  using IndicesPtr = boost::shared_ptr<std::vector<int> >;
84  using IndicesConstPtr = boost::shared_ptr<const std::vector<int> >;
85 
86  /** Constructor. */
87  Search (const std::string& name = "", bool sorted = false);
88 
89  /** Destructor. */
90  virtual
92  {
93  }
94 
95  /** \brief Returns the search method name
96  */
97  virtual const std::string&
98  getName () const;
99 
100  /** \brief sets whether the results should be sorted (ascending in the distance) or not
101  * \param[in] sorted should be true if the results should be sorted by the distance in ascending order.
102  * Otherwise the results may be returned in any order.
103  */
104  virtual void
105  setSortedResults (bool sorted);
106 
107  /** \brief Gets whether the results should be sorted (ascending in the distance) or not
108  * Otherwise the results may be returned in any order.
109  */
110  virtual bool
111  getSortedResults ();
112 
113 
114  /** \brief Pass the input dataset that the search will be performed on.
115  * \param[in] cloud a const pointer to the PointCloud data
116  * \param[in] indices the point indices subset that is to be used from the cloud
117  */
118  virtual void
119  setInputCloud (const PointCloudConstPtr& cloud,
120  const IndicesConstPtr &indices = IndicesConstPtr ());
121 
122  /** \brief Get a pointer to the input point cloud dataset. */
123  virtual PointCloudConstPtr
124  getInputCloud () const
125  {
126  return (input_);
127  }
128 
129  /** \brief Get a pointer to the vector of indices used. */
130  virtual IndicesConstPtr
131  getIndices () const
132  {
133  return (indices_);
134  }
135 
136  /** \brief Search for the k-nearest neighbors for the given query point.
137  * \param[in] point the given query point
138  * \param[in] k the number of neighbors to search for
139  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
140  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
141  * a priori!)
142  * \return number of neighbors found
143  */
144  virtual int
145  nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices,
146  std::vector<float> &k_sqr_distances) const = 0;
147 
148  /** \brief Search for k-nearest neighbors for the given query point.
149  * This method accepts a different template parameter for the point type.
150  * \param[in] point the given query point
151  * \param[in] k the number of neighbors to search for
152  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
153  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
154  * a priori!)
155  * \return number of neighbors found
156  */
157  template <typename PointTDiff> inline int
158  nearestKSearchT (const PointTDiff &point, int k,
159  std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const
160  {
161  PointT p;
162  copyPoint (point, p);
163  return (nearestKSearch (p, k, k_indices, k_sqr_distances));
164  }
165 
166  /** \brief Search for k-nearest neighbors for the given query point.
167  *
168  * \attention This method does not do any bounds checking for the input index
169  * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data.
170  *
171  * \param[in] cloud the point cloud data
172  * \param[in] index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point
173  * \param[in] k the number of neighbors to search for
174  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
175  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
176  * a priori!)
177  *
178  * \return number of neighbors found
179  *
180  * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
181  */
182  virtual int
183  nearestKSearch (const PointCloud &cloud, int index, int k,
184  std::vector<int> &k_indices,
185  std::vector<float> &k_sqr_distances) const;
186 
187  /** \brief Search for k-nearest neighbors for the given query point (zero-copy).
188  *
189  * \attention This method does not do any bounds checking for the input index
190  * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data.
191  *
192  * \param[in] index a \a valid index representing a \a valid query point in the dataset given
193  * by \a setInputCloud. If indices were given in setInputCloud, index will be the position in
194  * the indices vector.
195  *
196  * \param[in] k the number of neighbors to search for
197  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
198  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
199  * a priori!)
200  * \return number of neighbors found
201  *
202  * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
203  */
204  virtual int
205  nearestKSearch (int index, int k,
206  std::vector<int> &k_indices,
207  std::vector<float> &k_sqr_distances) const;
208 
209  /** \brief Search for the k-nearest neighbors for the given query point.
210  * \param[in] cloud the point cloud data
211  * \param[in] indices a vector of point cloud indices to query for nearest neighbors
212  * \param[in] k the number of neighbors to search for
213  * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
214  * \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
215  */
216  virtual void
217  nearestKSearch (const PointCloud& cloud, const std::vector<int>& indices,
218  int k, std::vector< std::vector<int> >& k_indices,
219  std::vector< std::vector<float> >& k_sqr_distances) const;
220 
221  /** \brief Search for the k-nearest neighbors for the given query point. Use this method if the query points are of a different type than the points in the data set (e.g. PointXYZRGBA instead of PointXYZ).
222  * \param[in] cloud the point cloud data
223  * \param[in] indices a vector of point cloud indices to query for nearest neighbors
224  * \param[in] k the number of neighbors to search for
225  * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
226  * \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
227  * \note This method copies the input point cloud of type PointTDiff to a temporary cloud of type PointT and performs the batch search on the new cloud. You should prefer the single-point search if you don't use a search algorithm that accelerates batch NN search.
228  */
229  template <typename PointTDiff> void
230  nearestKSearchT (const pcl::PointCloud<PointTDiff> &cloud, const std::vector<int>& indices, int k, std::vector< std::vector<int> > &k_indices,
231  std::vector< std::vector<float> > &k_sqr_distances) const
232  {
233  // Copy all the data fields from the input cloud to the output one
234  using FieldListInT = typename pcl::traits::fieldList<PointT>::type;
235  using FieldListOutT = typename pcl::traits::fieldList<PointTDiff>::type;
236  using FieldList = typename pcl::intersect<FieldListInT, FieldListOutT>::type;
237 
239  if (indices.empty ())
240  {
241  pc.resize (cloud.size());
242  for (size_t i = 0; i < cloud.size(); i++)
243  {
244  pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTDiff, PointT> (
245  cloud[i], pc[i]));
246  }
247  nearestKSearch (pc,std::vector<int>(),k,k_indices,k_sqr_distances);
248  }
249  else
250  {
251  pc.resize (indices.size());
252  for (size_t i = 0; i < indices.size(); i++)
253  {
254  pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTDiff, PointT> (
255  cloud[indices[i]], pc[i]));
256  }
257  nearestKSearch (pc,std::vector<int>(),k,k_indices,k_sqr_distances);
258  }
259  }
260 
261  /** \brief Search for all the nearest neighbors of the query point in a given radius.
262  * \param[in] point the given query point
263  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
264  * \param[out] k_indices the resultant indices of the neighboring points
265  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
266  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
267  * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
268  * returned.
269  * \return number of neighbors found in radius
270  */
271  virtual int
272  radiusSearch (const PointT& point, double radius, std::vector<int>& k_indices,
273  std::vector<float>& k_sqr_distances, unsigned int max_nn = 0) const = 0;
274 
275  /** \brief Search for all the nearest neighbors of the query point in a given radius.
276  * \param[in] point the given query point
277  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
278  * \param[out] k_indices the resultant indices of the neighboring points
279  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
280  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
281  * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
282  * returned.
283  * \return number of neighbors found in radius
284  */
285  template <typename PointTDiff> inline int
286  radiusSearchT (const PointTDiff &point, double radius, std::vector<int> &k_indices,
287  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
288  {
289  PointT p;
290  copyPoint (point, p);
291  return (radiusSearch (p, radius, k_indices, k_sqr_distances, max_nn));
292  }
293 
294  /** \brief Search for all the nearest neighbors of the query point in a given radius.
295  *
296  * \attention This method does not do any bounds checking for the input index
297  * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data.
298  *
299  * \param[in] cloud the point cloud data
300  * \param[in] index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point
301  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
302  * \param[out] k_indices the resultant indices of the neighboring points
303  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
304  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
305  * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
306  * returned.
307  * \return number of neighbors found in radius
308  *
309  * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
310  */
311  virtual int
312  radiusSearch (const PointCloud &cloud, int index, double radius,
313  std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
314  unsigned int max_nn = 0) const;
315 
316  /** \brief Search for all the nearest neighbors of the query point in a given radius (zero-copy).
317  *
318  * \attention This method does not do any bounds checking for the input index
319  * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data.
320  *
321  * \param[in] index a \a valid index representing a \a valid query point in the dataset given
322  * by \a setInputCloud. If indices were given in setInputCloud, index will be the position in
323  * the indices vector.
324  *
325  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
326  * \param[out] k_indices the resultant indices of the neighboring points
327  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
328  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
329  * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
330  * returned.
331  * \return number of neighbors found in radius
332  *
333  * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
334  */
335  virtual int
336  radiusSearch (int index, double radius, std::vector<int> &k_indices,
337  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const;
338 
339  /** \brief Search for all the nearest neighbors of the query point in a given radius.
340  * \param[in] cloud the point cloud data
341  * \param[in] indices the indices in \a cloud. If indices is empty, neighbors will be searched for all points.
342  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
343  * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
344  * \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
345  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
346  * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
347  * returned.
348  */
349  virtual void
350  radiusSearch (const PointCloud& cloud,
351  const std::vector<int>& indices,
352  double radius,
353  std::vector< std::vector<int> >& k_indices,
354  std::vector< std::vector<float> > &k_sqr_distances,
355  unsigned int max_nn = 0) const;
356 
357  /** \brief Search for all the nearest neighbors of the query points in a given radius.
358  * \param[in] cloud the point cloud data
359  * \param[in] indices a vector of point cloud indices to query for nearest neighbors
360  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
361  * \param[out] k_indices the resultant indices of the neighboring points, k_indices[i] corresponds to the neighbors of the query point i
362  * \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
363  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
364  * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
365  * returned.
366  * \note This method copies the input point cloud of type PointTDiff to a temporary cloud of type PointT and performs the batch search on the new cloud. You should prefer the single-point search if you don't use a search algorithm that accelerates batch NN search.
367  */
368  template <typename PointTDiff> void
370  const std::vector<int>& indices,
371  double radius,
372  std::vector< std::vector<int> > &k_indices,
373  std::vector< std::vector<float> > &k_sqr_distances,
374  unsigned int max_nn = 0) const
375  {
376  // Copy all the data fields from the input cloud to the output one
377  using FieldListInT = typename pcl::traits::fieldList<PointT>::type;
378  using FieldListOutT = typename pcl::traits::fieldList<PointTDiff>::type;
379  using FieldList = typename pcl::intersect<FieldListInT, FieldListOutT>::type;
380 
382  if (indices.empty ())
383  {
384  pc.resize (cloud.size ());
385  for (size_t i = 0; i < cloud.size (); ++i)
386  pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTDiff, PointT> (cloud[i], pc[i]));
387  radiusSearch (pc, std::vector<int> (), radius, k_indices, k_sqr_distances, max_nn);
388  }
389  else
390  {
391  pc.resize (indices.size ());
392  for (size_t i = 0; i < indices.size (); ++i)
393  pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTDiff, PointT> (cloud[indices[i]], pc[i]));
394  radiusSearch (pc, std::vector<int>(), radius, k_indices, k_sqr_distances, max_nn);
395  }
396  }
397 
398  protected:
399  void
400  sortResults (std::vector<int>& indices, std::vector<float>& distances) const;
401 
405  std::string name_;
406 
407  private:
408  struct Compare
409  {
410  Compare (const std::vector<float>& distances)
411  : distances_ (distances)
412  {
413  }
414 
415  bool
416  operator () (int first, int second) const
417  {
418  return (distances_ [first] < distances_[second]);
419  }
420 
421  const std::vector<float>& distances_;
422  };
423  }; // class Search
424  } // namespace search
425 } // namespace pcl
426 
427 #ifdef PCL_NO_PRECOMPILE
428 #include <pcl/search/impl/search.hpp>
429 #endif
void nearestKSearchT(const pcl::PointCloud< PointTDiff > &cloud, const std::vector< int > &indices, int k, std::vector< std::vector< int > > &k_indices, std::vector< std::vector< float > > &k_sqr_distances) const
Search for the k-nearest neighbors for the given query point.
Definition: search.h:230
size_t size() const
Definition: point_cloud.h:447
virtual const std::string & getName() const
Returns the search method name.
Definition: search.hpp:54
virtual bool getSortedResults()
Gets whether the results should be sorted (ascending in the distance) or not Otherwise the results ma...
Definition: search.hpp:68
typename boost::mpl::remove_if< Sequence1, boost::mpl::not_< boost::mpl::contains< Sequence2, boost::mpl::_1 > > >::type type
typename PointCloud::Ptr PointCloudPtr
Definition: search.h:77
virtual void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Pass the input dataset that the search will be performed on.
Definition: search.hpp:75
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:44
virtual int radiusSearch(const PointT &point, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const =0
Search for all the nearest neighbors of the query point in a given radius.
virtual ~Search()
Destructor.
Definition: search.h:91
int nearestKSearchT(const PointTDiff &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const
Search for k-nearest neighbors for the given query point.
Definition: search.h:158
PointCloudConstPtr input_
Definition: search.h:402
virtual IndicesConstPtr getIndices() const
Get a pointer to the vector of indices used.
Definition: search.h:131
virtual void setSortedResults(bool sorted)
sets whether the results should be sorted (ascending in the distance) or not
Definition: search.hpp:61
boost::shared_ptr< const pcl::search::Search< PointXYZRGB > > ConstPtr
Definition: search.h:81
IndicesConstPtr indices_
Definition: search.h:403
bool sorted_results_
Definition: search.h:404
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:427
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
Definition: search.h:124
std::string name_
Definition: search.h:405
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void radiusSearchT(const pcl::PointCloud< PointTDiff > &cloud, const std::vector< int > &indices, double radius, std::vector< std::vector< int > > &k_indices, std::vector< std::vector< float > > &k_sqr_distances, unsigned int max_nn=0) const
Search for all the nearest neighbors of the query points in a given radius.
Definition: search.h:369
virtual int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const =0
Search for the k-nearest neighbors for the given query point.
void sortResults(std::vector< int > &indices, std::vector< float > &distances) const
Definition: search.hpp:188
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:428
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
Definition: search.h:84
int radiusSearchT(const PointTDiff &point, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const
Search for all the nearest neighbors of the query point in a given radius.
Definition: search.h:286
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:454
A point structure representing Euclidean xyz coordinates, and the RGB color.
Search(const std::string &name="", bool sorted=false)
Constructor.
Definition: search.hpp:45
boost::shared_ptr< pcl::search::Search< PointXYZRGB > > Ptr
Definition: search.h:80
Helper functor structure for concatenate.
Definition: concatenate.h:51
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
Definition: copy_point.hpp:138
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: search.h:78
Generic search class.
Definition: search.h:73
boost::shared_ptr< std::vector< int > > IndicesPtr
Definition: search.h:83