octree.h
Go to the documentation of this file.
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 Willow Garage, Inc. 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: octree.h 6031 2012-06-26 10:54:13Z jkammerl $
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   {
00065     template<typename PointT, typename LeafTWrap = pcl::octree::OctreeContainerDataTVector<int>, typename BranchTWrap = pcl::octree::OctreeContainerEmpty<int>,
00066              typename OctreeT = pcl::octree::OctreeBase<int, LeafTWrap, BranchTWrap > >
00067     class Octree: public Search<PointT>
00068     {
00069       public:
00070         // public typedefs
00071         typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
00072         typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
00073 
00074         typedef pcl::PointCloud<PointT> PointCloud;
00075         typedef boost::shared_ptr<PointCloud> PointCloudPtr;
00076         typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
00077 
00078         // Boost shared pointers
00079         typedef boost::shared_ptr<pcl::octree::OctreePointCloudSearch<PointT, LeafTWrap, BranchTWrap> > Ptr;
00080         typedef boost::shared_ptr<const pcl::octree::OctreePointCloudSearch<PointT, LeafTWrap, BranchTWrap> > ConstPtr;
00081         Ptr tree_;
00082 
00083         using pcl::search::Search<PointT>::input_;
00084         using pcl::search::Search<PointT>::indices_;
00085         using pcl::search::Search<PointT>::sorted_results_;
00086 
00090         Octree (const double resolution)
00091           : Search<PointT> ("Octree")
00092           , tree_ (new pcl::octree::OctreePointCloudSearch<PointT, LeafTWrap, BranchTWrap> (resolution))
00093         {
00094         }
00095 
00097         virtual
00098         ~Octree ()
00099         {
00100         }
00101 
00105         inline void
00106         setInputCloud (const PointCloudConstPtr &cloud)
00107         {
00108           tree_->deleteTree ();
00109           tree_->setInputCloud (cloud);
00110           tree_->addPointsFromInputCloud ();
00111           input_ = cloud;
00112         }
00113 
00118         inline void
00119         setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr& indices)
00120         {
00121           tree_->deleteTree ();
00122           tree_->setInputCloud (cloud, indices);
00123           tree_->addPointsFromInputCloud ();
00124           input_ = cloud;
00125           indices_ = indices;
00126         }
00127 
00137         inline int
00138         nearestKSearch (const PointCloud &cloud, int index, int k, std::vector<int> &k_indices,
00139                         std::vector<float> &k_sqr_distances) const
00140         {
00141           return (tree_->nearestKSearch (cloud, index, k, k_indices, k_sqr_distances));
00142         }
00143 
00152         inline int
00153         nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices,
00154                         std::vector<float> &k_sqr_distances) const
00155         {
00156           return (tree_->nearestKSearch (point, k, k_indices, k_sqr_distances));
00157         }
00158 
00170         inline int
00171         nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const
00172         {
00173           return (tree_->nearestKSearch (index, k, k_indices, k_sqr_distances));
00174         }
00175 
00185         inline int
00186         radiusSearch (const PointCloud &cloud, 
00187                       int index, 
00188                       double radius,
00189                       std::vector<int> &k_indices, 
00190                       std::vector<float> &k_sqr_distances, 
00191                       unsigned int max_nn = 0) const
00192         {
00193           tree_->radiusSearch (cloud, index, radius, k_indices, k_sqr_distances, max_nn);
00194           if (sorted_results_)
00195             this->sortResults (k_indices, k_sqr_distances);
00196           return (static_cast<int> (k_indices.size ()));
00197         }
00198 
00207         inline int
00208         radiusSearch (const PointT &p_q, 
00209                       double radius, 
00210                       std::vector<int> &k_indices,
00211                       std::vector<float> &k_sqr_distances, 
00212                       unsigned int max_nn = 0) const
00213         {
00214           tree_->radiusSearch (p_q, radius, k_indices, k_sqr_distances, max_nn);
00215           if (sorted_results_)
00216             this->sortResults (k_indices, k_sqr_distances);
00217           return (static_cast<int> (k_indices.size ()));
00218         }
00219 
00229         inline int
00230         radiusSearch (int index, double radius, std::vector<int> &k_indices,
00231                       std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
00232         {
00233           tree_->radiusSearch (index, radius, k_indices, k_sqr_distances, max_nn);
00234           if (sorted_results_)
00235             this->sortResults (k_indices, k_sqr_distances);
00236           return (static_cast<int> (k_indices.size ()));
00237         }
00238 
00239 
00247         inline void
00248         approxNearestSearch (const PointCloudConstPtr &cloud, int query_index, int &result_index,
00249                              float &sqr_distance)
00250         {
00251           return (tree_->approxNearestSearch (cloud->points[query_index], result_index, sqr_distance));
00252         }
00253 
00259         inline void
00260         approxNearestSearch (const PointT &p_q, int &result_index, float &sqr_distance)
00261         {
00262           return (tree_->approxNearestSearch (p_q, result_index, sqr_distance));
00263         }
00264 
00272         inline void
00273         approxNearestSearch (int query_index, int &result_index, float &sqr_distance)
00274         {
00275           return (tree_->approxNearestSearch (query_index, result_index, sqr_distance));
00276         }
00277 
00278     };
00279   }
00280 }
00281 
00282 #define PCL_INSTANTIATE_Octree(T) template class PCL_EXPORTS pcl::search::Octree<T>;
00283 
00284 #endif    // PCL_SEARCH_OCTREE_H


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:56