Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2009-2011, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 */ 00038 00039 #ifndef PCL_KDTREE_KDTREE_H_ 00040 #define PCL_KDTREE_KDTREE_H_ 00041 00042 #include <limits.h> 00043 #include <pcl/pcl_macros.h> 00044 #include <pcl/point_cloud.h> 00045 #include <pcl/point_representation.h> 00046 #include <pcl/common/io.h> 00047 00048 namespace pcl 00049 { 00050 /** \brief KdTree represents the base spatial locator class for kd-tree implementations. 00051 * \author Radu B Rusu, Bastian Steder, Michael Dixon 00052 * \ingroup kdtree 00053 */ 00054 template <typename PointT> 00055 class KdTree 00056 { 00057 public: 00058 typedef boost::shared_ptr <std::vector<int> > IndicesPtr; 00059 typedef boost::shared_ptr <const std::vector<int> > IndicesConstPtr; 00060 00061 typedef pcl::PointCloud<PointT> PointCloud; 00062 typedef boost::shared_ptr<PointCloud> PointCloudPtr; 00063 typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr; 00064 00065 typedef pcl::PointRepresentation<PointT> PointRepresentation; 00066 //typedef boost::shared_ptr<PointRepresentation> PointRepresentationPtr; 00067 typedef boost::shared_ptr<const PointRepresentation> PointRepresentationConstPtr; 00068 00069 // Boost shared pointers 00070 typedef boost::shared_ptr<KdTree<PointT> > Ptr; 00071 typedef boost::shared_ptr<const KdTree<PointT> > ConstPtr; 00072 00073 /** \brief Empty constructor for KdTree. Sets some internal values to their defaults. 00074 * \param[in] sorted set to true if the application that the tree will be used for requires sorted nearest neighbor indices (default). False otherwise. 00075 */ 00076 KdTree (bool sorted = true) : input_(), indices_(), 00077 epsilon_(0.0f), min_pts_(1), sorted_(sorted), 00078 point_representation_ (new DefaultPointRepresentation<PointT>) 00079 { 00080 }; 00081 00082 /** \brief Provide a pointer to the input dataset. 00083 * \param[in] cloud the const boost shared pointer to a PointCloud message 00084 * \param[in] indices the point indices subset that is to be used from \a cloud - if NULL the whole cloud is used 00085 */ 00086 virtual void 00087 setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ()) 00088 { 00089 input_ = cloud; 00090 indices_ = indices; 00091 } 00092 00093 /** \brief Get a pointer to the vector of indices used. */ 00094 inline IndicesConstPtr 00095 getIndices () const 00096 { 00097 return (indices_); 00098 } 00099 00100 /** \brief Get a pointer to the input point cloud dataset. */ 00101 inline PointCloudConstPtr 00102 getInputCloud () const 00103 { 00104 return (input_); 00105 } 00106 00107 /** \brief Provide a pointer to the point representation to use to convert points into k-D vectors. 00108 * \param[in] point_representation the const boost shared pointer to a PointRepresentation 00109 */ 00110 inline void 00111 setPointRepresentation (const PointRepresentationConstPtr &point_representation) 00112 { 00113 point_representation_ = point_representation; 00114 if (!input_) return; 00115 setInputCloud (input_, indices_); // Makes sense in derived classes to reinitialize the tree 00116 } 00117 00118 /** \brief Get a pointer to the point representation used when converting points into k-D vectors. */ 00119 inline PointRepresentationConstPtr 00120 getPointRepresentation () const 00121 { 00122 return (point_representation_); 00123 } 00124 00125 /** \brief Destructor for KdTree. Deletes all allocated data arrays and destroys the kd-tree structures. */ 00126 virtual ~KdTree () {}; 00127 00128 /** \brief Search for k-nearest neighbors for the given query point. 00129 * \param[in] p_q the given query point 00130 * \param[in] k the number of neighbors to search for 00131 * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) 00132 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k 00133 * a priori!) 00134 * \return number of neighbors found 00135 */ 00136 virtual int 00137 nearestKSearch (const PointT &p_q, int k, 00138 std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const = 0; 00139 00140 /** \brief Search for k-nearest neighbors for the given query point. 00141 * 00142 * \attention This method does not do any bounds checking for the input index 00143 * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data. 00144 * 00145 * \param[in] cloud the point cloud data 00146 * \param[in] index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point 00147 * \param[in] k the number of neighbors to search for 00148 * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) 00149 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k 00150 * a priori!) 00151 * 00152 * \return number of neighbors found 00153 * 00154 * \exception asserts in debug mode if the index is not between 0 and the maximum number of points 00155 */ 00156 virtual int 00157 nearestKSearch (const PointCloud &cloud, int index, int k, 00158 std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const 00159 { 00160 assert (index >= 0 && index < static_cast<int> (cloud.points.size ()) && "Out-of-bounds error in nearestKSearch!"); 00161 return (nearestKSearch (cloud.points[index], k, k_indices, k_sqr_distances)); 00162 } 00163 00164 /** \brief Search for k-nearest neighbors for the given query point. 00165 * This method accepts a different template parameter for the point type. 00166 * \param[in] point the given query point 00167 * \param[in] k the number of neighbors to search for 00168 * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) 00169 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k 00170 * a priori!) 00171 * \return number of neighbors found 00172 */ 00173 template <typename PointTDiff> inline int 00174 nearestKSearchT (const PointTDiff &point, int k, 00175 std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const 00176 { 00177 PointT p; 00178 // Copy all the data fields from the input cloud to the output one 00179 typedef typename pcl::traits::fieldList<PointT>::type FieldListInT; 00180 typedef typename pcl::traits::fieldList<PointTDiff>::type FieldListOutT; 00181 typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList; 00182 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTDiff, PointT> (point, p)); 00183 return (nearestKSearch (p, k, k_indices, k_sqr_distances)); 00184 } 00185 00186 /** \brief Search for k-nearest neighbors for the given query point (zero-copy). 00187 * 00188 * \attention This method does not do any bounds checking for the input index 00189 * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data. 00190 * 00191 * \param[in] index a \a valid index representing a \a valid query point in the dataset given 00192 * by \a setInputCloud. If indices were given in setInputCloud, index will be the position in 00193 * the indices vector. 00194 * 00195 * \param[in] k the number of neighbors to search for 00196 * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!) 00197 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k 00198 * a priori!) 00199 * \return number of neighbors found 00200 * 00201 * \exception asserts in debug mode if the index is not between 0 and the maximum number of points 00202 */ 00203 virtual int 00204 nearestKSearch (int index, int k, 00205 std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const 00206 { 00207 if (indices_ == NULL) 00208 { 00209 assert (index >= 0 && index < static_cast<int> (input_->points.size ()) && "Out-of-bounds error in nearestKSearch!"); 00210 return (nearestKSearch (input_->points[index], k, k_indices, k_sqr_distances)); 00211 } 00212 else 00213 { 00214 assert (index >= 0 && index < static_cast<int> (indices_->size ()) && "Out-of-bounds error in nearestKSearch!"); 00215 return (nearestKSearch (input_->points[(*indices_)[index]], k, k_indices, k_sqr_distances)); 00216 } 00217 } 00218 00219 /** \brief Search for all the nearest neighbors of the query point in a given radius. 00220 * \param[in] p_q the given query point 00221 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors 00222 * \param[out] k_indices the resultant indices of the neighboring points 00223 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 00224 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to 00225 * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be 00226 * returned. 00227 * \return number of neighbors found in radius 00228 */ 00229 virtual int 00230 radiusSearch (const PointT &p_q, double radius, std::vector<int> &k_indices, 00231 std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const = 0; 00232 00233 /** \brief Search for all the nearest neighbors of the query point in a given radius. 00234 * 00235 * \attention This method does not do any bounds checking for the input index 00236 * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data. 00237 * 00238 * \param[in] cloud the point cloud data 00239 * \param[in] index a \a valid index in \a cloud representing a \a valid (i.e., finite) query point 00240 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors 00241 * \param[out] k_indices the resultant indices of the neighboring points 00242 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 00243 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to 00244 * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be 00245 * returned. 00246 * \return number of neighbors found in radius 00247 * 00248 * \exception asserts in debug mode if the index is not between 0 and the maximum number of points 00249 */ 00250 virtual int 00251 radiusSearch (const PointCloud &cloud, int index, double radius, 00252 std::vector<int> &k_indices, std::vector<float> &k_sqr_distances, 00253 unsigned int max_nn = 0) const 00254 { 00255 assert (index >= 0 && index < static_cast<int> (cloud.points.size ()) && "Out-of-bounds error in radiusSearch!"); 00256 return (radiusSearch(cloud.points[index], radius, k_indices, k_sqr_distances, max_nn)); 00257 } 00258 00259 /** \brief Search for all the nearest neighbors of the query point in a given radius. 00260 * \param[in] point the given query point 00261 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors 00262 * \param[out] k_indices the resultant indices of the neighboring points 00263 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 00264 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to 00265 * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be 00266 * returned. 00267 * \return number of neighbors found in radius 00268 */ 00269 template <typename PointTDiff> inline int 00270 radiusSearchT (const PointTDiff &point, double radius, std::vector<int> &k_indices, 00271 std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const 00272 { 00273 PointT p; 00274 // Copy all the data fields from the input cloud to the output one 00275 typedef typename pcl::traits::fieldList<PointT>::type FieldListInT; 00276 typedef typename pcl::traits::fieldList<PointTDiff>::type FieldListOutT; 00277 typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList; 00278 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTDiff, PointT> (point, p)); 00279 return (radiusSearch (p, radius, k_indices, k_sqr_distances, max_nn)); 00280 } 00281 00282 /** \brief Search for all the nearest neighbors of the query point in a given radius (zero-copy). 00283 * 00284 * \attention This method does not do any bounds checking for the input index 00285 * (i.e., index >= cloud.points.size () || index < 0), and assumes valid (i.e., finite) data. 00286 * 00287 * \param[in] index a \a valid index representing a \a valid query point in the dataset given 00288 * by \a setInputCloud. If indices were given in setInputCloud, index will be the position in 00289 * the indices vector. 00290 * 00291 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors 00292 * \param[out] k_indices the resultant indices of the neighboring points 00293 * \param[out] k_sqr_distances the resultant squared distances to the neighboring points 00294 * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to 00295 * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be 00296 * returned. 00297 * \return number of neighbors found in radius 00298 * 00299 * \exception asserts in debug mode if the index is not between 0 and the maximum number of points 00300 */ 00301 virtual int 00302 radiusSearch (int index, double radius, std::vector<int> &k_indices, 00303 std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const 00304 { 00305 if (indices_ == NULL) 00306 { 00307 assert (index >= 0 && index < static_cast<int> (input_->points.size ()) && "Out-of-bounds error in radiusSearch!"); 00308 return (radiusSearch (input_->points[index], radius, k_indices, k_sqr_distances, max_nn)); 00309 } 00310 else 00311 { 00312 assert (index >= 0 && index < static_cast<int> (indices_->size ()) && "Out-of-bounds error in radiusSearch!"); 00313 return (radiusSearch (input_->points[(*indices_)[index]], radius, k_indices, k_sqr_distances, max_nn)); 00314 } 00315 } 00316 00317 /** \brief Set the search epsilon precision (error bound) for nearest neighbors searches. 00318 * \param[in] eps precision (error bound) for nearest neighbors searches 00319 */ 00320 virtual inline void 00321 setEpsilon (float eps) 00322 { 00323 epsilon_ = eps; 00324 } 00325 00326 /** \brief Get the search epsilon precision (error bound) for nearest neighbors searches. */ 00327 inline float 00328 getEpsilon () const 00329 { 00330 return (epsilon_); 00331 } 00332 00333 /** \brief Minimum allowed number of k nearest neighbors points that a viable result must contain. 00334 * \param[in] min_pts the minimum number of neighbors in a viable neighborhood 00335 */ 00336 inline void 00337 setMinPts (int min_pts) 00338 { 00339 min_pts_ = min_pts; 00340 } 00341 00342 /** \brief Get the minimum allowed number of k nearest neighbors points that a viable result must contain. */ 00343 inline int 00344 getMinPts () const 00345 { 00346 return (min_pts_); 00347 } 00348 00349 protected: 00350 /** \brief The input point cloud dataset containing the points we need to use. */ 00351 PointCloudConstPtr input_; 00352 00353 /** \brief A pointer to the vector of point indices to use. */ 00354 IndicesConstPtr indices_; 00355 00356 /** \brief Epsilon precision (error bound) for nearest neighbors searches. */ 00357 float epsilon_; 00358 00359 /** \brief Minimum allowed number of k nearest neighbors points that a viable result must contain. */ 00360 int min_pts_; 00361 00362 /** \brief Return the radius search neighbours sorted **/ 00363 bool sorted_; 00364 00365 /** \brief For converting different point structures into k-dimensional vectors for nearest-neighbor search. */ 00366 PointRepresentationConstPtr point_representation_; 00367 00368 /** \brief Class getName method. */ 00369 virtual std::string 00370 getName () const = 0; 00371 }; 00372 } 00373 00374 #endif //#ifndef _PCL_KDTREE_KDTREE_H_