Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/search/include/pcl/search/kdtree.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-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  * $Id$
00038  */
00039 
00040 #ifndef PCL_SEARCH_KDTREE_H_
00041 #define PCL_SEARCH_KDTREE_H_
00042 
00043 #include <pcl/search/search.h>
00044 #include <pcl/kdtree/kdtree_flann.h>
00045 
00046 namespace pcl
00047 {
00048   // Forward declarations
00049   template <typename T> class PointRepresentation;
00050 
00051   namespace search
00052   {
00053     /** \brief @b search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search 
00054       * functions using KdTree structure. KdTree is a generic type of 3D spatial locator using kD-tree structures. 
00055       * The class is making use of the FLANN (Fast Library for Approximate Nearest Neighbor) project 
00056       * by Marius Muja and David Lowe.
00057       *
00058       * \author Radu B. Rusu
00059       * \ingroup search
00060       */
00061     template<typename PointT>
00062     class KdTree: public Search<PointT>
00063     {
00064       public:
00065         typedef typename Search<PointT>::PointCloud PointCloud;
00066         typedef typename Search<PointT>::PointCloudConstPtr PointCloudConstPtr;
00067 
00068         typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
00069         typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
00070 
00071         using pcl::search::Search<PointT>::indices_;
00072         using pcl::search::Search<PointT>::input_;
00073         using pcl::search::Search<PointT>::getIndices;
00074         using pcl::search::Search<PointT>::getInputCloud;
00075         using pcl::search::Search<PointT>::nearestKSearch;
00076         using pcl::search::Search<PointT>::radiusSearch;
00077         using pcl::search::Search<PointT>::sorted_results_;
00078 
00079         typedef boost::shared_ptr<KdTree<PointT> > Ptr;
00080         typedef boost::shared_ptr<const KdTree<PointT> > ConstPtr;
00081 
00082         typedef boost::shared_ptr<pcl::KdTreeFLANN<PointT> > KdTreeFLANNPtr;
00083         typedef boost::shared_ptr<const pcl::KdTreeFLANN<PointT> > KdTreeFLANNConstPtr;
00084         typedef boost::shared_ptr<const PointRepresentation<PointT> > PointRepresentationConstPtr;
00085 
00086         /** \brief Constructor for KdTree. 
00087           *
00088           * \param[in] sorted set to true if the nearest neighbor search results
00089           * need to be sorted in ascending order based on their distance to the
00090           * query point
00091           *
00092           */
00093         KdTree (bool sorted = true); 
00094 
00095         /** \brief Destructor for KdTree. */
00096         virtual
00097         ~KdTree ()
00098         {
00099         }
00100 
00101         /** \brief Provide a pointer to the point representation to use to convert points into k-D vectors. 
00102           * \param[in] point_representation the const boost shared pointer to a PointRepresentation
00103           */
00104         void
00105         setPointRepresentation (const PointRepresentationConstPtr &point_representation);
00106 
00107         /** \brief Get a pointer to the point representation used when converting points into k-D vectors. */
00108         inline PointRepresentationConstPtr
00109         getPointRepresentation () const
00110         {
00111           return (tree_->getPointRepresentation ());
00112         }
00113 
00114         /** \brief Sets whether the results have to be sorted or not.
00115           * \param[in] sorted_results set to true if the radius search results should be sorted
00116           */
00117         void 
00118         setSortedResults (bool sorted_results);
00119         
00120         /** \brief Set the search epsilon precision (error bound) for nearest neighbors searches.
00121           * \param[in] eps precision (error bound) for nearest neighbors searches
00122           */
00123         void
00124         setEpsilon (float eps);
00125 
00126         /** \brief Get the search epsilon precision (error bound) for nearest neighbors searches. */
00127         inline float
00128         getEpsilon () const
00129         {
00130           return (tree_->getEpsilon ());
00131         }
00132 
00133         /** \brief Provide a pointer to the input dataset.
00134           * \param[in] cloud the const boost shared pointer to a PointCloud message
00135           * \param[in] indices the point indices subset that is to be used from \a cloud 
00136           */
00137         void
00138         setInputCloud (const PointCloudConstPtr& cloud, 
00139                        const IndicesConstPtr& indices = IndicesConstPtr ());
00140 
00141         /** \brief Search for the k-nearest neighbors for the given query point.
00142           * \param[in] point the given query point
00143           * \param[in] k the number of neighbors to search for
00144           * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
00145           * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
00146           * a priori!)
00147           * \return number of neighbors found
00148           */
00149         int
00150         nearestKSearch (const PointT &point, int k, 
00151                         std::vector<int> &k_indices, 
00152                         std::vector<float> &k_sqr_distances) const;
00153 
00154         /** \brief Search for all the nearest neighbors of the query point in a given radius.
00155           * \param[in] point the given query point
00156           * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
00157           * \param[out] k_indices the resultant indices of the neighboring points
00158           * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
00159           * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
00160           * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
00161           * returned.
00162           * \return number of neighbors found in radius
00163           */
00164         int
00165         radiusSearch (const PointT& point, double radius, 
00166                       std::vector<int> &k_indices, 
00167                       std::vector<float> &k_sqr_distances,
00168                       unsigned int max_nn = 0) const;
00169       protected:
00170         /** \brief A pointer to the internal KdTreeFLANN object. */
00171         KdTreeFLANNPtr tree_;
00172     };
00173   }
00174 }
00175 
00176 #define PCL_INSTANTIATE_KdTree(T) template class PCL_EXPORTS pcl::search::KdTree<T>;
00177 
00178 #endif    // PCL_SEARCH_KDTREE_H_
00179