Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/search/include/pcl/search/octree.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  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the copyright holder(s) nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id$
00037  */
00038 
00039 #ifndef PCL_SEARCH_OCTREE_H
00040 #define PCL_SEARCH_OCTREE_H
00041 
00042 #include <pcl/search/search.h>
00043 #include <pcl/octree/octree_search.h>
00044 
00045 namespace pcl
00046 {
00047   namespace search
00048   {
00049     /** \brief @b search::Octree is a wrapper class which implements nearest neighbor search operations based on the 
00050       * pcl::octree::Octree structure. 
00051       * 
00052       * The octree pointcloud class needs to be initialized with its voxel
00053       * resolution. Its bounding box is automatically adjusted according to the
00054       * pointcloud dimension or it can be predefined. Note: The tree depth
00055       * equates to the resolution and the bounding box dimensions of the
00056       * octree.
00057       *
00058       * \note typename: PointT: type of point used in pointcloud
00059       * \note typename: LeafT:  leaf node class (usuallt templated with integer indices values)
00060       * \note typename: OctreeT: octree implementation ()
00061       *
00062       * \author Julius Kammerl
00063       * \ingroup search
00064       */
00065     template<typename PointT,
00066              typename LeafTWrap = pcl::octree::OctreeContainerPointIndices,
00067              typename BranchTWrap = pcl::octree::OctreeContainerEmpty,
00068              typename OctreeT = pcl::octree::OctreeBase<LeafTWrap, BranchTWrap > >
00069     class Octree: public Search<PointT>
00070     {
00071       public:
00072         // public typedefs
00073         typedef boost::shared_ptr<pcl::search::Octree<PointT,LeafTWrap,BranchTWrap,OctreeT> > Ptr;
00074         typedef boost::shared_ptr<const pcl::search::Octree<PointT,LeafTWrap,BranchTWrap,OctreeT> > ConstPtr;
00075 
00076         typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
00077         typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
00078 
00079         typedef pcl::PointCloud<PointT> PointCloud;
00080         typedef boost::shared_ptr<PointCloud> PointCloudPtr;
00081         typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
00082 
00083         // Boost shared pointers
00084         typedef boost::shared_ptr<pcl::octree::OctreePointCloudSearch<PointT, LeafTWrap, BranchTWrap> > OctreePointCloudSearchPtr;
00085         typedef boost::shared_ptr<const pcl::octree::OctreePointCloudSearch<PointT, LeafTWrap, BranchTWrap> > OctreePointCloudSearchConstPtr;
00086         OctreePointCloudSearchPtr tree_;
00087 
00088         using pcl::search::Search<PointT>::input_;
00089         using pcl::search::Search<PointT>::indices_;
00090         using pcl::search::Search<PointT>::sorted_results_;
00091 
00092         /** \brief Octree constructor.
00093           * \param[in] resolution octree resolution at lowest octree level
00094           */
00095         Octree (const double resolution)
00096           : Search<PointT> ("Octree")
00097           , tree_ (new pcl::octree::OctreePointCloudSearch<PointT, LeafTWrap, BranchTWrap> (resolution))
00098         {
00099         }
00100 
00101         /** \brief Empty Destructor. */
00102         virtual
00103         ~Octree ()
00104         {
00105         }
00106 
00107         /** \brief Provide a pointer to the input dataset.
00108           * \param[in] cloud the const boost shared pointer to a PointCloud message
00109           */
00110         inline void
00111         setInputCloud (const PointCloudConstPtr &cloud)
00112         {
00113           tree_->deleteTree ();
00114           tree_->setInputCloud (cloud);
00115           tree_->addPointsFromInputCloud ();
00116           input_ = cloud;
00117         }
00118 
00119         /** \brief Provide a pointer to the input dataset.
00120           * \param[in] cloud the const boost shared pointer to a PointCloud message
00121           * \param[in] indices the point indices subset that is to be used from \a cloud 
00122           */
00123         inline void
00124         setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr& indices)
00125         {
00126           tree_->deleteTree ();
00127           tree_->setInputCloud (cloud, indices);
00128           tree_->addPointsFromInputCloud ();
00129           input_ = cloud;
00130           indices_ = indices;
00131         }
00132 
00133         /** \brief Search for the k-nearest neighbors for the given query point.
00134           * \param[in] cloud the point cloud data
00135           * \param[in] index the index in \a cloud representing the query point
00136           * \param[in] k the number of neighbors to search for
00137           * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
00138           * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
00139           * a priori!)
00140           * \return number of neighbors found
00141           */
00142         inline int
00143         nearestKSearch (const PointCloud &cloud, int index, int k, std::vector<int> &k_indices,
00144                         std::vector<float> &k_sqr_distances) const
00145         {
00146           return (tree_->nearestKSearch (cloud, index, k, k_indices, k_sqr_distances));
00147         }
00148 
00149         /** \brief Search for the k-nearest neighbors for the given query point.
00150           * \param[in] point the given query point
00151           * \param[in] k the number of neighbors to search for
00152           * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
00153           * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
00154           * a priori!)
00155           * \return number of neighbors found
00156           */
00157         inline int
00158         nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices,
00159                         std::vector<float> &k_sqr_distances) const
00160         {
00161           return (tree_->nearestKSearch (point, k, k_indices, k_sqr_distances));
00162         }
00163 
00164         /** \brief Search for the k-nearest neighbors for the given query point (zero-copy).
00165           *
00166           * \param[in] index the index representing the query point in the
00167           * dataset given by \a setInputCloud if indices were given in
00168           * setInputCloud, index will be the position in the indices vector
00169           * \param[in] k the number of neighbors to search for
00170           * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
00171           * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
00172           * a priori!)
00173           * \return number of neighbors found
00174           */
00175         inline int
00176         nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const
00177         {
00178           return (tree_->nearestKSearch (index, k, k_indices, k_sqr_distances));
00179         }
00180 
00181         /** \brief search for all neighbors of query point that are within a given radius.
00182          * \param cloud the point cloud data
00183          * \param index the index in \a cloud representing the query point
00184          * \param radius the radius of the sphere bounding all of p_q's neighbors
00185          * \param k_indices the resultant indices of the neighboring points
00186          * \param k_sqr_distances the resultant squared distances to the neighboring points
00187          * \param max_nn if given, bounds the maximum returned neighbors to this value
00188          * \return number of neighbors found in radius
00189          */
00190         inline int
00191         radiusSearch (const PointCloud &cloud, 
00192                       int index, 
00193                       double radius,
00194                       std::vector<int> &k_indices, 
00195                       std::vector<float> &k_sqr_distances, 
00196                       unsigned int max_nn = 0) const
00197         {
00198           tree_->radiusSearch (cloud, index, radius, k_indices, k_sqr_distances, max_nn);
00199           if (sorted_results_)
00200             this->sortResults (k_indices, k_sqr_distances);
00201           return (static_cast<int> (k_indices.size ()));
00202         }
00203 
00204         /** \brief search for all neighbors of query point that are within a given radius.
00205          * \param p_q the given query point
00206          * \param radius the radius of the sphere bounding all of p_q's neighbors
00207          * \param k_indices the resultant indices of the neighboring points
00208          * \param k_sqr_distances the resultant squared distances to the neighboring points
00209          * \param max_nn if given, bounds the maximum returned neighbors to this value
00210          * \return number of neighbors found in radius
00211          */
00212         inline int
00213         radiusSearch (const PointT &p_q, 
00214                       double radius, 
00215                       std::vector<int> &k_indices,
00216                       std::vector<float> &k_sqr_distances, 
00217                       unsigned int max_nn = 0) const
00218         {
00219           tree_->radiusSearch (p_q, radius, k_indices, k_sqr_distances, max_nn);
00220           if (sorted_results_)
00221             this->sortResults (k_indices, k_sqr_distances);
00222           return (static_cast<int> (k_indices.size ()));
00223         }
00224 
00225         /** \brief search for all neighbors of query point that are within a given radius.
00226          * \param index index representing the query point in the dataset given by \a setInputCloud.
00227          *        If indices were given in setInputCloud, index will be the position in the indices vector
00228          * \param radius radius of the sphere bounding all of p_q's neighbors
00229          * \param k_indices the resultant indices of the neighboring points
00230          * \param k_sqr_distances the resultant squared distances to the neighboring points
00231          * \param max_nn if given, bounds the maximum returned neighbors to this value
00232          * \return number of neighbors found in radius
00233          */
00234         inline int
00235         radiusSearch (int index, double radius, std::vector<int> &k_indices,
00236                       std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
00237         {
00238           tree_->radiusSearch (index, radius, k_indices, k_sqr_distances, max_nn);
00239           if (sorted_results_)
00240             this->sortResults (k_indices, k_sqr_distances);
00241           return (static_cast<int> (k_indices.size ()));
00242         }
00243 
00244 
00245         /** \brief Search for approximate nearest neighbor at the query point.
00246           * \param[in] cloud the point cloud data
00247           * \param[in] query_index the index in \a cloud representing the query point
00248           * \param[out] result_index the resultant index of the neighbor point
00249           * \param[out] sqr_distance the resultant squared distance to the neighboring point
00250           * \return number of neighbors found
00251           */
00252         inline void
00253         approxNearestSearch (const PointCloudConstPtr &cloud, int query_index, int &result_index,
00254                              float &sqr_distance)
00255         {
00256           return (tree_->approxNearestSearch (cloud->points[query_index], result_index, sqr_distance));
00257         }
00258 
00259         /** \brief Search for approximate nearest neighbor at the query point.
00260           * \param[in] p_q the given query point
00261           * \param[out] result_index the resultant index of the neighbor point
00262           * \param[out] sqr_distance the resultant squared distance to the neighboring point
00263           */
00264         inline void
00265         approxNearestSearch (const PointT &p_q, int &result_index, float &sqr_distance)
00266         {
00267           return (tree_->approxNearestSearch (p_q, result_index, sqr_distance));
00268         }
00269 
00270         /** \brief Search for approximate nearest neighbor at the query point.
00271           * \param query_index index representing the query point in the dataset given by \a setInputCloud.
00272           *        If indices were given in setInputCloud, index will be the position in the indices vector.
00273           * \param result_index the resultant index of the neighbor point
00274           * \param sqr_distance the resultant squared distance to the neighboring point
00275           * \return number of neighbors found
00276           */
00277         inline void
00278         approxNearestSearch (int query_index, int &result_index, float &sqr_distance)
00279         {
00280           return (tree_->approxNearestSearch (query_index, result_index, sqr_distance));
00281         }
00282 
00283     };
00284   }
00285 }
00286 
00287 #ifdef PCL_NO_PRECOMPILE
00288 #include <pcl/octree/impl/octree_search.hpp>
00289 #else
00290 #define PCL_INSTANTIATE_Octree(T) template class PCL_EXPORTS pcl::search::Octree<T>;
00291 #endif
00292 
00293 #endif    // PCL_SEARCH_OCTREE_H