Point Cloud Library (PCL)  1.11.1-dev
kdtree.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-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/memory.h>
42 #include <pcl/pcl_macros.h>
43 #include <pcl/point_cloud.h>
44 #include <pcl/point_representation.h>
45 #include <pcl/common/copy_point.h>
46 
47 namespace pcl
48 {
49  /** \brief KdTree represents the base spatial locator class for kd-tree implementations.
50  * \author Radu B Rusu, Bastian Steder, Michael Dixon
51  * \ingroup kdtree
52  */
53  template <typename PointT>
54  class KdTree
55  {
56  public:
57  using IndicesPtr = shared_ptr<std::vector<int> >;
58  using IndicesConstPtr = shared_ptr<const std::vector<int> >;
59 
61  using PointCloudPtr = typename PointCloud::Ptr;
63 
66 
67  // Boost shared pointers
68  using Ptr = shared_ptr<KdTree<PointT> >;
69  using ConstPtr = shared_ptr<const KdTree<PointT> >;
70 
71  /** \brief Empty constructor for KdTree. Sets some internal values to their defaults.
72  * \param[in] sorted set to true if the application that the tree will be used for requires sorted nearest neighbor indices (default). False otherwise.
73  */
74  KdTree (bool sorted = true) : input_(),
75  epsilon_(0.0f), min_pts_(1), sorted_(sorted),
77  {
78  };
79 
80  /** \brief Provide a pointer to the input dataset.
81  * \param[in] cloud the const boost shared pointer to a PointCloud message
82  * \param[in] indices the point indices subset that is to be used from \a cloud - if NULL the whole cloud is used
83  */
84  virtual void
86  {
87  input_ = cloud;
88  indices_ = indices;
89  }
90 
91  /** \brief Get a pointer to the vector of indices used. */
92  inline IndicesConstPtr
93  getIndices () const
94  {
95  return (indices_);
96  }
97 
98  /** \brief Get a pointer to the input point cloud dataset. */
99  inline PointCloudConstPtr
100  getInputCloud () const
101  {
102  return (input_);
103  }
104 
105  /** \brief Provide a pointer to the point representation to use to convert points into k-D vectors.
106  * \param[in] point_representation the const boost shared pointer to a PointRepresentation
107  */
108  inline void
110  {
111  point_representation_ = point_representation;
112  if (!input_) return;
113  setInputCloud (input_, indices_); // Makes sense in derived classes to reinitialize the tree
114  }
115 
116  /** \brief Get a pointer to the point representation used when converting points into k-D vectors. */
119  {
120  return (point_representation_);
121  }
122 
123  /** \brief Destructor for KdTree. Deletes all allocated data arrays and destroys the kd-tree structures. */
124  virtual ~KdTree () {};
125 
126  /** \brief Search for k-nearest neighbors for the given query point.
127  * \param[in] p_q the given query point
128  * \param[in] k the number of neighbors to search for
129  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
130  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
131  * a priori!)
132  * \return number of neighbors found
133  */
134  virtual int
135  nearestKSearch (const PointT &p_q, int k,
136  std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const = 0;
137 
138  /** \brief Search for k-nearest neighbors for the given query point.
139  *
140  * \attention This method does not do any bounds checking for the input index
141  * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
142  *
143  * \param[in] cloud the point cloud data
144  * \param[in] index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point
145  * \param[in] k the number of neighbors to search for
146  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
147  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
148  * a priori!)
149  *
150  * \return number of neighbors found
151  *
152  * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
153  */
154  virtual int
155  nearestKSearch (const PointCloud &cloud, int index, int k,
156  std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const
157  {
158  assert (index >= 0 && index < static_cast<int> (cloud.size ()) && "Out-of-bounds error in nearestKSearch!");
159  return (nearestKSearch (cloud[index], k, k_indices, k_sqr_distances));
160  }
161 
162  /** \brief Search for k-nearest neighbors for the given query point.
163  * This method accepts a different template parameter for the point type.
164  * \param[in] point the given query point
165  * \param[in] k the number of neighbors to search for
166  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
167  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
168  * a priori!)
169  * \return number of neighbors found
170  */
171  template <typename PointTDiff> inline int
172  nearestKSearchT (const PointTDiff &point, int k,
173  std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const
174  {
175  PointT p;
176  copyPoint (point, p);
177  return (nearestKSearch (p, k, k_indices, k_sqr_distances));
178  }
179 
180  /** \brief Search for k-nearest neighbors for the given query point (zero-copy).
181  *
182  * \attention This method does not do any bounds checking for the input index
183  * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
184  *
185  * \param[in] index a \a valid index representing a \a valid query point in the dataset given
186  * by \a setInputCloud. If indices were given in setInputCloud, index will be the position in
187  * the indices vector.
188  *
189  * \param[in] k the number of neighbors to search for
190  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
191  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
192  * a priori!)
193  * \return number of neighbors found
194  *
195  * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
196  */
197  virtual int
198  nearestKSearch (int index, int k,
199  std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const
200  {
201  if (indices_ == nullptr)
202  {
203  assert (index >= 0 && index < static_cast<int> (input_->size ()) && "Out-of-bounds error in nearestKSearch!");
204  return (nearestKSearch ((*input_)[index], k, k_indices, k_sqr_distances));
205  }
206  assert (index >= 0 && index < static_cast<int> (indices_->size ()) && "Out-of-bounds error in nearestKSearch!");
207 
208  return (nearestKSearch ((*input_)[(*indices_)[index]], k, k_indices, k_sqr_distances));
209  }
210 
211  /** \brief Search for all the nearest neighbors of the query point in a given radius.
212  * \param[in] p_q the given query point
213  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
214  * \param[out] k_indices the resultant indices of the neighboring points
215  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
216  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
217  * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
218  * returned.
219  * \return number of neighbors found in radius
220  */
221  virtual int
222  radiusSearch (const PointT &p_q, double radius, std::vector<int> &k_indices,
223  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const = 0;
224 
225  /** \brief Search for all the nearest neighbors of the query point in a given radius.
226  *
227  * \attention This method does not do any bounds checking for the input index
228  * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
229  *
230  * \param[in] cloud the point cloud data
231  * \param[in] index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point
232  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
233  * \param[out] k_indices the resultant indices of the neighboring points
234  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
235  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
236  * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
237  * returned.
238  * \return number of neighbors found in radius
239  *
240  * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
241  */
242  virtual int
243  radiusSearch (const PointCloud &cloud, int index, double radius,
244  std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
245  unsigned int max_nn = 0) const
246  {
247  assert (index >= 0 && index < static_cast<int> (cloud.size ()) && "Out-of-bounds error in radiusSearch!");
248  return (radiusSearch(cloud[index], radius, k_indices, k_sqr_distances, max_nn));
249  }
250 
251  /** \brief Search for all the nearest neighbors of the query point in a given radius.
252  * \param[in] point the given query point
253  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
254  * \param[out] k_indices the resultant indices of the neighboring points
255  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
256  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
257  * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
258  * returned.
259  * \return number of neighbors found in radius
260  */
261  template <typename PointTDiff> inline int
262  radiusSearchT (const PointTDiff &point, double radius, std::vector<int> &k_indices,
263  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
264  {
265  PointT p;
266  copyPoint (point, p);
267  return (radiusSearch (p, radius, k_indices, k_sqr_distances, max_nn));
268  }
269 
270  /** \brief Search for all the nearest neighbors of the query point in a given radius (zero-copy).
271  *
272  * \attention This method does not do any bounds checking for the input index
273  * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
274  *
275  * \param[in] index a \a valid index representing a \a valid query point in the dataset given
276  * by \a setInputCloud. If indices were given in setInputCloud, index will be the position in
277  * the indices vector.
278  *
279  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
280  * \param[out] k_indices the resultant indices of the neighboring points
281  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
282  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
283  * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
284  * returned.
285  * \return number of neighbors found in radius
286  *
287  * \exception asserts in debug mode if the index is not between 0 and the maximum number of points
288  */
289  virtual int
290  radiusSearch (int index, double radius, std::vector<int> &k_indices,
291  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
292  {
293  if (indices_ == nullptr)
294  {
295  assert (index >= 0 && index < static_cast<int> (input_->size ()) && "Out-of-bounds error in radiusSearch!");
296  return (radiusSearch ((*input_)[index], radius, k_indices, k_sqr_distances, max_nn));
297  }
298  assert (index >= 0 && index < static_cast<int> (indices_->size ()) && "Out-of-bounds error in radiusSearch!");
299  return (radiusSearch ((*input_)[(*indices_)[index]], radius, k_indices, k_sqr_distances, max_nn));
300  }
301 
302  /** \brief Set the search epsilon precision (error bound) for nearest neighbors searches.
303  * \param[in] eps precision (error bound) for nearest neighbors searches
304  */
305  virtual inline void
306  setEpsilon (float eps)
307  {
308  epsilon_ = eps;
309  }
310 
311  /** \brief Get the search epsilon precision (error bound) for nearest neighbors searches. */
312  inline float
313  getEpsilon () const
314  {
315  return (epsilon_);
316  }
317 
318  /** \brief Minimum allowed number of k nearest neighbors points that a viable result must contain.
319  * \param[in] min_pts the minimum number of neighbors in a viable neighborhood
320  */
321  inline void
322  setMinPts (int min_pts)
323  {
324  min_pts_ = min_pts;
325  }
326 
327  /** \brief Get the minimum allowed number of k nearest neighbors points that a viable result must contain. */
328  inline int
329  getMinPts () const
330  {
331  return (min_pts_);
332  }
333 
334  protected:
335  /** \brief The input point cloud dataset containing the points we need to use. */
337 
338  /** \brief A pointer to the vector of point indices to use. */
340 
341  /** \brief Epsilon precision (error bound) for nearest neighbors searches. */
342  float epsilon_;
343 
344  /** \brief Minimum allowed number of k nearest neighbors points that a viable result must contain. */
345  int min_pts_;
346 
347  /** \brief Return the radius search neighbours sorted **/
348  bool sorted_;
349 
350  /** \brief For converting different point structures into k-dimensional vectors for nearest-neighbor search. */
352 
353  /** \brief Class getName method. */
354  virtual std::string
355  getName () const = 0;
356  };
357 }
pcl::KdTree::min_pts_
int min_pts_
Minimum allowed number of k nearest neighbors points that a viable result must contain.
Definition: kdtree.h:345
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl::KdTree< FeatureT >::IndicesConstPtr
shared_ptr< const std::vector< int > > IndicesConstPtr
Definition: kdtree.h:58
pcl
Definition: convolution.h:46
pcl::KdTree::setEpsilon
virtual void setEpsilon(float eps)
Set the search epsilon precision (error bound) for nearest neighbors searches.
Definition: kdtree.h:306
pcl::KdTree
KdTree represents the base spatial locator class for kd-tree implementations.
Definition: kdtree.h:54
pcl::KdTree< FeatureT >::ConstPtr
shared_ptr< const KdTree< FeatureT > > ConstPtr
Definition: kdtree.h:69
pcl::KdTree::radiusSearchT
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: kdtree.h:262
pcl::IndicesConstPtr
shared_ptr< const Indices > IndicesConstPtr
Definition: pcl_base.h:59
pcl::DefaultPointRepresentation
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
Definition: point_representation.h:179
pcl::KdTree::indices_
IndicesConstPtr indices_
A pointer to the vector of point indices to use.
Definition: kdtree.h:339
pcl::PointRepresentation::ConstPtr
shared_ptr< const PointRepresentation< PointT > > ConstPtr
Definition: point_representation.h:79
pcl::KdTree::setInputCloud
virtual void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
Definition: kdtree.h:85
pcl::PointCloud< FeatureT >
pcl::KdTree::setMinPts
void setMinPts(int min_pts)
Minimum allowed number of k nearest neighbors points that a viable result must contain.
Definition: kdtree.h:322
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::KdTree< FeatureT >::IndicesPtr
shared_ptr< std::vector< int > > IndicesPtr
Definition: kdtree.h:57
pcl::KdTree::radiusSearch
virtual int radiusSearch(const PointCloud &cloud, int index, 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: kdtree.h:243
pcl::KdTree::getEpsilon
float getEpsilon() const
Get the search epsilon precision (error bound) for nearest neighbors searches.
Definition: kdtree.h:313
pcl::KdTree::getPointRepresentation
PointRepresentationConstPtr getPointRepresentation() const
Get a pointer to the point representation used when converting points into k-D vectors.
Definition: kdtree.h:118
pcl::copyPoint
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
pcl::KdTree::radiusSearch
virtual int radiusSearch(const PointT &p_q, 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.
pcl::KdTree::getName
virtual std::string getName() const =0
Class getName method.
pcl::KdTree< FeatureT >::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition: kdtree.h:61
pcl::KdTree::getIndices
IndicesConstPtr getIndices() const
Get a pointer to the vector of indices used.
Definition: kdtree.h:93
pcl::KdTree::getInputCloud
PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
Definition: kdtree.h:100
pcl::PointRepresentation< FeatureT >
pcl::KdTree::point_representation_
PointRepresentationConstPtr point_representation_
For converting different point structures into k-dimensional vectors for nearest-neighbor search.
Definition: kdtree.h:351
pcl::KdTree::sorted_
bool sorted_
Return the radius search neighbours sorted.
Definition: kdtree.h:348
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:436
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:406
pcl::KdTree::~KdTree
virtual ~KdTree()
Destructor for KdTree.
Definition: kdtree.h:124
pcl::KdTree< FeatureT >::Ptr
shared_ptr< KdTree< FeatureT > > Ptr
Definition: kdtree.h:68
pcl::KdTree::radiusSearch
virtual int radiusSearch(int index, 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 (zero-copy).
Definition: kdtree.h:290
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:407
pcl::KdTree::epsilon_
float epsilon_
Epsilon precision (error bound) for nearest neighbors searches.
Definition: kdtree.h:342
pcl::KdTree< FeatureT >::PointRepresentationConstPtr
typename PointRepresentation::ConstPtr PointRepresentationConstPtr
Definition: kdtree.h:65
pcl::KdTree::nearestKSearchT
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: kdtree.h:172
pcl::KdTree::input_
PointCloudConstPtr input_
The input point cloud dataset containing the points we need to use.
Definition: kdtree.h:336
pcl::KdTree::getMinPts
int getMinPts() const
Get the minimum allowed number of k nearest neighbors points that a viable result must contain.
Definition: kdtree.h:329
pcl::KdTree::nearestKSearch
virtual int nearestKSearch(const PointT &p_q, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const =0
Search for k-nearest neighbors for the given query point.
pcl::KdTree::nearestKSearch
virtual int nearestKSearch(int index, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const
Search for k-nearest neighbors for the given query point (zero-copy).
Definition: kdtree.h:198
memory.h
Defines functions, macros and traits for allocating and using memory.
pcl::KdTree::nearestKSearch
virtual int nearestKSearch(const PointCloud &cloud, int index, 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: kdtree.h:155
pcl::KdTree< FeatureT >::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: kdtree.h:62
pcl::KdTree::setPointRepresentation
void setPointRepresentation(const PointRepresentationConstPtr &point_representation)
Provide a pointer to the point representation to use to convert points into k-D vectors.
Definition: kdtree.h:109
pcl::KdTree::KdTree
KdTree(bool sorted=true)
Empty constructor for KdTree.
Definition: kdtree.h:74