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