Point Cloud Library (PCL)
1.3.1
|
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_pointcloud.h 3017 2011-11-01 03:24:04Z rusu $ 00037 */ 00038 00039 #ifndef OCTREE_POINTCLOUD_H 00040 #define OCTREE_POINTCLOUD_H 00041 00042 #include "octree_base.h" 00043 #include "octree2buf_base.h" 00044 #include "octree_lowmemory_base.h" 00045 00046 #include <pcl/point_cloud.h> 00047 #include <pcl/point_types.h> 00048 00049 #include "octree_nodes.h" 00050 00051 #include <queue> 00052 #include <vector> 00053 #include <algorithm> 00054 #include <iostream> 00055 00056 namespace pcl 00057 { 00058 namespace octree 00059 { 00060 00062 00074 00075 template<typename PointT, typename LeafT = OctreeLeafDataTVector<int> , typename OctreeT = OctreeBase<int, LeafT> > 00076 class OctreePointCloud : public OctreeT 00077 { 00078 00079 public: 00080 00084 OctreePointCloud (const double resolution_arg); 00085 00087 virtual 00088 ~OctreePointCloud (); 00089 00090 // public typedefs 00091 typedef boost::shared_ptr<std::vector<int> > IndicesPtr; 00092 typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr; 00093 00094 typedef pcl::PointCloud<PointT> PointCloud; 00095 typedef boost::shared_ptr<PointCloud> PointCloudPtr; 00096 typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr; 00097 00098 // public typedefs for single/double buffering 00099 typedef OctreePointCloud<PointT, LeafT, OctreeBase<int, LeafT> > SingleBuffer; 00100 typedef OctreePointCloud<PointT, LeafT, Octree2BufBase<int, LeafT> > DoubleBuffer; 00101 typedef OctreePointCloud<PointT, LeafT, OctreeLowMemBase<int, LeafT> > LowMem; 00102 00103 // Boost shared pointers 00104 typedef boost::shared_ptr<OctreePointCloud<PointT, LeafT, OctreeT> > Ptr; 00105 typedef boost::shared_ptr<const OctreePointCloud<PointT, LeafT, OctreeT> > ConstPtr; 00106 00107 // Eigen aligned allocator 00108 typedef std::vector<PointT, Eigen::aligned_allocator<PointT> > AlignedPointTVector; 00109 00114 inline void 00115 setInputCloud (const PointCloudConstPtr &cloud_arg, const IndicesConstPtr &indices_arg = IndicesConstPtr ()) 00116 { 00117 assert (this->leafCount_==0); 00118 00119 if ((input_ != cloud_arg) && (this->leafCount_ == 0)) 00120 { 00121 input_ = cloud_arg; 00122 indices_ = indices_arg; 00123 } 00124 } 00125 00129 inline IndicesConstPtr const 00130 getIndices () 00131 { 00132 return (indices_); 00133 } 00134 00138 inline PointCloudConstPtr 00139 getInputCloud () 00140 { 00141 return (input_); 00142 } 00143 00147 inline void 00148 setEpsilon (double eps) 00149 { 00150 epsilon_ = eps; 00151 } 00152 00154 inline double 00155 getEpsilon () 00156 { 00157 return (epsilon_); 00158 } 00159 00163 inline void 00164 setResolution (double resolution_arg) 00165 { 00166 // octree needs to be empty to change its resolution 00167 assert( this->leafCount_ == 0 ); 00168 00169 resolution_ = resolution_arg; 00170 } 00171 00174 inline double 00175 getResolution () 00176 { 00177 return (resolution_); 00178 } 00179 00181 void 00182 addPointsFromInputCloud (); 00183 00188 void 00189 addPointFromCloud (const int pointIdx_arg, IndicesPtr indices_arg); 00190 00195 void 00196 addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg); 00197 00203 void 00204 addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg, IndicesPtr indices_arg); 00205 00210 bool 00211 isVoxelOccupiedAtPoint (const PointT& point_arg) const; 00212 00214 void 00215 deleteTree () 00216 { 00217 // reset bounding box 00218 minX_ = minY_ = maxY_ = minZ_ = maxZ_ = 0; 00219 maxKeys_ = 1; 00220 this->boundingBoxDefined_ = false; 00221 00222 OctreeT::deleteTree (); 00223 } 00224 00231 bool 00232 isVoxelOccupiedAtPoint (const double pointX_arg, const double pointY_arg, const double pointZ_arg) const; 00233 00238 bool 00239 isVoxelOccupiedAtPoint (const int& pointIdx_arg) const; 00240 00245 int 00246 getOccupiedVoxelCenters (AlignedPointTVector &voxelCenterList_arg) const; 00247 00251 void 00252 deleteVoxelAtPoint (const PointT& point_arg); 00253 00257 void 00258 deleteVoxelAtPoint (const int& pointIdx_arg); 00259 00261 // Bounding box methods 00263 00264 00267 void 00268 defineBoundingBox (); 00269 00279 void 00280 defineBoundingBox (const double minX_arg, const double minY_arg, const double minZ_arg, const double maxX_arg, 00281 const double maxY_arg, const double maxZ_arg); 00282 00290 void 00291 defineBoundingBox (const double maxX_arg, const double maxY_arg, const double maxZ_arg); 00292 00298 void 00299 defineBoundingBox (const double cubeLen_arg); 00300 00310 void 00311 getBoundingBox (double& minX_arg, double& minY_arg, double& minZ_arg, double& maxX_arg, double& maxY_arg, 00312 double& maxZ_arg) const ; 00313 00318 double 00319 getVoxelSquaredDiameter (unsigned int treeDepth_arg) const ; 00320 00324 inline double 00325 getVoxelSquaredDiameter () const 00326 { 00327 return getVoxelSquaredDiameter (this->octreeDepth_); 00328 } 00329 00334 double 00335 getVoxelSquaredSideLen (unsigned int treeDepth_arg) const ; 00336 00340 inline double 00341 getVoxelSquaredSideLen () const 00342 { 00343 return getVoxelSquaredSideLen (this->octreeDepth_); 00344 } 00345 00346 typedef typename OctreeT::OctreeLeaf OctreeLeaf; 00347 00348 protected: 00349 00353 void 00354 addPointIdx (const int pointIdx_arg); 00355 00360 const PointT& 00361 getPointByIndex (const unsigned int index_arg) const; 00362 00367 LeafT* 00368 findLeafAtPoint (const PointT& point_arg) const ; 00369 00371 // Protected octree methods based on octree keys 00373 00374 typedef typename OctreeT::OctreeKey OctreeKey; 00375 typedef typename OctreeT::OctreeBranch OctreeBranch; 00376 00379 void 00380 getKeyBitSize (); 00381 00385 void 00386 adoptBoundingBoxToPoint (const PointT& pointIdx_arg); 00387 00392 void 00393 genOctreeKeyforPoint (const PointT & point_arg, OctreeKey & key_arg) const ; 00394 00401 void 00402 genOctreeKeyforPoint (const double pointX_arg, const double pointY_arg, const double pointZ_arg, 00403 OctreeKey & key_arg) const; 00404 00411 virtual bool 00412 genOctreeKeyForDataT (const int& data_arg, OctreeKey & key_arg) const; 00413 00418 void 00419 genLeafNodeCenterFromOctreeKey (const OctreeKey & key_arg, PointT& point_arg) const ; 00420 00426 void 00427 genVoxelCenterFromOctreeKey (const OctreeKey & key_arg, unsigned int treeDepth_arg, PointT& point_arg) const ; 00428 00435 void 00436 genVoxelBoundsFromOctreeKey (const OctreeKey & key_arg, unsigned int treeDepth_arg, Eigen::Vector3f &min_pt, 00437 Eigen::Vector3f &max_pt) const; 00438 00445 int 00446 getOccupiedVoxelCentersRecursive (const OctreeBranch* node_arg, 00447 const OctreeKey& key_arg, 00448 std::vector<PointT, Eigen::aligned_allocator<PointT> > &voxelCenterList_arg) const; 00449 00451 // Globals 00453 00455 PointCloudConstPtr input_; 00456 00458 IndicesConstPtr indices_; 00459 00461 double epsilon_; 00462 00464 double resolution_; 00465 00466 // Octree bounding box coordinates 00467 double minX_; 00468 double maxX_; 00469 00470 double minY_; 00471 double maxY_; 00472 00473 double minZ_; 00474 double maxZ_; 00475 00477 unsigned int maxKeys_; 00478 00480 bool boundingBoxDefined_; 00481 00482 }; 00483 00484 } 00485 } 00486 00487 #endif 00488