Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
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
47namespace 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<Indices >;
58 using IndicesConstPtr = shared_ptr<const Indices >;
59
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
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, unsigned int k,
136 Indices &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, unsigned int k,
156 Indices &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, unsigned int k,
173 Indices &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, unsigned int k,
199 Indices &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, Indices &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 Indices &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, Indices &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, Indices &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. */
346
347 /** \brief Return the radius search neighbours 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}
DefaultPointRepresentation extends PointRepresentation to define default behavior for common point ty...
KdTree represents the base spatial locator class for kd-tree implementations.
Definition kdtree.h:55
virtual int nearestKSearch(int index, unsigned int k, Indices &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
shared_ptr< const Indices > IndicesConstPtr
Definition kdtree.h:58
PointRepresentationConstPtr point_representation_
For converting different point structures into k-dimensional vectors for nearest-neighbor search.
Definition kdtree.h:351
PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
Definition kdtree.h:100
KdTree(bool sorted=true)
Empty constructor for KdTree.
Definition kdtree.h:74
typename PointRepresentation::ConstPtr PointRepresentationConstPtr
Definition kdtree.h:65
virtual int radiusSearch(int index, 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 (zero-copy).
Definition kdtree.h:290
IndicesConstPtr getIndices() const
Get a pointer to the vector of indices used.
Definition kdtree.h:93
typename PointCloud::ConstPtr PointCloudConstPtr
Definition kdtree.h:62
typename PointCloud::Ptr PointCloudPtr
Definition kdtree.h:61
virtual int radiusSearch(const PointCloud &cloud, int index, 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 kdtree.h:243
PointCloudConstPtr input_
The input point cloud dataset containing the points we need to use.
Definition kdtree.h:336
float getEpsilon() const
Get the search epsilon precision (error bound) for nearest neighbors searches.
Definition kdtree.h:313
virtual void setEpsilon(float eps)
Set the search epsilon precision (error bound) for nearest neighbors searches.
Definition kdtree.h:306
float epsilon_
Epsilon precision (error bound) for nearest neighbors searches.
Definition kdtree.h:342
int min_pts_
Minimum allowed number of k nearest neighbors points that a viable result must contain.
Definition kdtree.h:345
shared_ptr< Indices > IndicesPtr
Definition kdtree.h:57
int nearestKSearchT(const PointTDiff &point, unsigned int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const
Search for k-nearest neighbors for the given query point.
Definition kdtree.h:172
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
void setMinPts(int min_pts)
Minimum allowed number of k nearest neighbors points that a viable result must contain.
Definition kdtree.h:322
virtual void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
Definition kdtree.h:85
bool sorted_
Return the radius search neighbours sorted.
Definition kdtree.h:348
int getMinPts() const
Get the minimum allowed number of k nearest neighbors points that a viable result must contain.
Definition kdtree.h:329
virtual int nearestKSearch(const PointCloud &cloud, int index, unsigned int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const
Search for k-nearest neighbors for the given query point.
Definition kdtree.h:155
virtual int radiusSearch(const PointT &p_q, 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.
IndicesConstPtr indices_
A pointer to the vector of point indices to use.
Definition kdtree.h:339
shared_ptr< KdTree< PointT > > Ptr
Definition kdtree.h:68
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 kdtree.h:262
virtual ~KdTree()
Destructor for KdTree.
Definition kdtree.h:124
virtual int nearestKSearch(const PointT &p_q, unsigned int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const =0
Search for k-nearest neighbors for the given query point.
shared_ptr< const KdTree< PointT > > ConstPtr
Definition kdtree.h:69
virtual std::string getName() const =0
Class getName method.
PointRepresentationConstPtr getPointRepresentation() const
Get a pointer to the point representation used when converting points into k-D vectors.
Definition kdtree.h:118
shared_ptr< const PointCloud< PointT > > ConstPtr
std::size_t size() const
shared_ptr< PointCloud< PointT > > Ptr
PointRepresentation provides a set of methods for converting a point structs/object into an n-dimensi...
Definition kdtree.h:48
shared_ptr< const PointRepresentation< PointT > > ConstPtr
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
Defines functions, macros and traits for allocating and using memory.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
shared_ptr< const Indices > IndicesConstPtr
Definition pcl_base.h:59
Defines all the PCL and non-PCL macros used.
A point structure representing Euclidean xyz coordinates, and the RGB color.