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