kdtree.h

Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: kdtree.h 3020 2011-11-01 03:41:16Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_KDTREE_KDTREE_H_
00039 #define PCL_KDTREE_KDTREE_H_
00040 
00041 #include <limits.h>
00042 #include <pcl/pcl_macros.h>
00043 #include <pcl/point_cloud.h>
00044 #include <pcl/point_representation.h>
00045 #include <pcl/common/io.h>
00046 
00047 namespace pcl
00048 {
00054   template <typename PointT>
00055   class KdTree
00056   {
00057     typedef boost::shared_ptr <std::vector<int> > IndicesPtr;
00058     typedef boost::shared_ptr <const std::vector<int> > IndicesConstPtr;
00059 
00060     public:
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 
00076       KdTree (bool sorted = true) : input_(), indices_(), 
00077                                     epsilon_(0.0), min_pts_(1), sorted_(sorted)
00078       {
00079         point_representation_.reset (new DefaultPointRepresentation<PointT>);
00080       };
00081 
00082 
00087       virtual inline void
00088       setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ())
00089       {
00090         input_   = cloud;
00091         indices_ = indices;
00092       }
00093 
00095       inline IndicesConstPtr const
00096       getIndices ()
00097       {
00098         return (indices_);
00099       }
00100 
00102       inline PointCloudConstPtr
00103       getInputCloud ()
00104       {
00105         return (input_);
00106       }
00107 
00111       inline void
00112       setPointRepresentation (const PointRepresentationConstPtr &point_representation)
00113       {
00114         point_representation_ = point_representation;
00115         setInputCloud (input_, indices_);  // Makes sense in derived classes to reinitialize the tree
00116       }
00117 
00119       inline PointRepresentationConstPtr const
00120       getPointRepresentation ()
00121       {
00122         return (point_representation_);
00123       }
00124 
00126       virtual ~KdTree () {};
00127 
00137       virtual int 
00138       nearestKSearch (const PointCloud &cloud, int index, int k, 
00139                       std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) = 0;
00140 
00149       virtual int 
00150       nearestKSearch (const PointT &p_q, int k, 
00151                       std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) = 0;
00152 
00161       template <typename PointTDiff> inline int 
00162       nearestKSearchT (const PointTDiff &point, int k, 
00163                        std::vector<int> &k_indices, std::vector<float> &k_distances)
00164       {
00165         PointT p;
00166         // Copy all the data fields from the input cloud to the output one
00167         typedef typename pcl::traits::fieldList<PointT>::type FieldListInT;
00168         typedef typename pcl::traits::fieldList<PointTDiff>::type FieldListOutT;
00169         typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList;
00170         pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointT, PointTDiff> (
00171               point, p));
00172         return (nearestKSearch (p, k, k_indices, k_distances));
00173       }
00174 
00184       virtual int 
00185       nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) = 0;
00186 
00196       virtual int 
00197       radiusSearch (const PointCloud &cloud, int index, double radius, std::vector<int> &k_indices,
00198                     std::vector<float> &k_sqr_distances, int max_nn = INT_MAX) const = 0;
00199 
00208       virtual int 
00209       radiusSearch (const PointT &p_q, double radius, std::vector<int> &k_indices,
00210                     std::vector<float> &k_sqr_distances, int max_nn = INT_MAX) const = 0;
00211 
00220       template <typename PointTDiff> inline int 
00221       radiusSearchT (const PointTDiff &point, double radius, std::vector<int> &k_indices,
00222                      std::vector<float> &k_distances, int max_nn = -1) const
00223       {
00224         PointT p;
00225         // Copy all the data fields from the input cloud to the output one
00226         typedef typename pcl::traits::fieldList<PointT>::type FieldListInT;
00227         typedef typename pcl::traits::fieldList<PointTDiff>::type FieldListOutT;
00228         typedef typename pcl::intersect<FieldListInT, FieldListOutT>::type FieldList;
00229         pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointT, PointTDiff> (
00230               point, p));
00231         return (radiusSearch (p, radius, k_indices, k_distances, max_nn));
00232       }
00233 
00243       virtual int 
00244       radiusSearch (int index, double radius, std::vector<int> &k_indices,
00245                     std::vector<float> &k_sqr_distances, int max_nn = INT_MAX) const = 0;
00246 
00250       inline void
00251       setEpsilon (double eps)
00252       {
00253         epsilon_ = eps;
00254       }
00255 
00257       inline double
00258       getEpsilon ()
00259       {
00260         return (epsilon_);
00261       }
00262 
00266       inline void
00267       setMinPts (int min_pts)
00268       {
00269         min_pts_ = min_pts;
00270       }
00271 
00273       inline float
00274       getMinPts ()
00275       {
00276         return (min_pts_);
00277       }
00278 
00279     protected:
00281       PointCloudConstPtr input_;
00282 
00284       IndicesConstPtr indices_;
00285 
00287       double epsilon_;
00288 
00290       int min_pts_;
00291 
00293       bool sorted_;
00294 
00296       PointRepresentationConstPtr point_representation_;
00297 
00299       virtual std::string 
00300       getName () const = 0;
00301   };
00302 }
00303 
00304 #endif  //#ifndef _PCL_KDTREE_KDTREE_H_