Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/kdtree/include/pcl/kdtree/kdtree.h
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_