Point Cloud Library (PCL)
1.7.0
|
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