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_base.h 3017 2011-11-01 03:24:04Z rusu $ 00037 */ 00038 00039 #ifndef OCTREE_TREE_BASE_H 00040 #define OCTREE_TREE_BASE_H 00041 00042 #include <cstddef> 00043 #include <vector> 00044 #include <math.h> 00045 00046 #include "octree_nodes.h" 00047 00048 #include "octree_iterator.h" 00049 00050 namespace pcl 00051 { 00052 namespace octree 00053 { 00055 00062 00063 template<typename DataT, typename LeafT = OctreeLeafDataT<DataT> > 00064 class OctreeBase 00065 { 00066 00067 friend class OctreeNodeIterator<DataT, LeafT, OctreeBase> ; 00068 friend class OctreeLeafNodeIterator<DataT, LeafT, OctreeBase> ; 00069 00070 public: 00071 00072 // Octree iterators 00073 typedef OctreeNodeIterator<DataT, LeafT, OctreeBase> Iterator; 00074 typedef const OctreeNodeIterator<DataT, LeafT, OctreeBase> ConstIterator; 00075 00076 // Octree iterators 00077 typedef OctreeLeafNodeIterator<DataT, LeafT, OctreeBase> LeafNodeIterator; 00078 typedef const OctreeLeafNodeIterator<DataT, LeafT, OctreeBase> ConstLeafNodeIterator; 00079 00081 OctreeBase (); 00082 00084 virtual 00085 ~OctreeBase (); 00086 00090 void 00091 setMaxVoxelIndex (unsigned int maxVoxelIndex_arg); 00092 00096 void 00097 setTreeDepth (unsigned int depth_arg); 00098 00102 inline unsigned int 00103 getTreeDepth () const 00104 { 00105 return this->octreeDepth_; 00106 } 00107 00114 void 00115 add (const unsigned int idxX_arg, const unsigned int idxY_arg, const unsigned int idxZ_arg, 00116 const DataT& data_arg); 00117 00125 bool 00126 get (const unsigned int idxX_arg, const unsigned int idxY_arg, const unsigned int idxZ_arg, DataT& data_arg) const ; 00127 00134 bool 00135 existLeaf (const unsigned int idxX_arg, const unsigned int idxY_arg, const unsigned int idxZ_arg) const ; 00136 00142 void 00143 removeLeaf (const unsigned int idxX_arg, const unsigned int idxY_arg, const unsigned int idxZ_arg); 00144 00148 inline unsigned int 00149 getLeafCount () const 00150 { 00151 return leafCount_; 00152 } 00153 00157 inline unsigned int 00158 getBranchCount () const 00159 { 00160 return branchCount_; 00161 } 00162 00166 void 00167 deleteTree ( bool freeMemory_arg = false ); 00168 00172 void 00173 serializeTree (std::vector<char>& binaryTreeOut_arg); 00174 00179 void 00180 serializeTree (std::vector<char>& binaryTreeOut_arg, std::vector<DataT>& dataVector_arg); 00181 00185 void 00186 serializeLeafs (std::vector<DataT>& dataVector_arg); 00187 00191 void 00192 deserializeTree (std::vector<char>& binaryTreeIn_arg); 00193 00198 void 00199 deserializeTree (std::vector<char>& binaryTreeIn_arg, std::vector<DataT>& dataVector_arg); 00200 00205 void 00206 deserializeTreeAndOutputLeafData (std::vector<char>& binaryTreeIn_arg, std::vector<DataT>& dataVector_arg); 00207 00208 protected: 00209 00211 00215 00216 class OctreeKey 00217 { 00218 public: 00219 00223 bool 00224 operator == (const OctreeKey& b) const 00225 { 00226 return ((b.x == this->x) && (b.y == this->y) && (b.z == this->z)); 00227 } 00228 00229 // Indices addressing a voxel at (X, Y, Z) 00230 unsigned int x;unsigned int y;unsigned int z; 00231 }; 00232 00234 00238 00239 class OctreeBranch : public OctreeNode 00240 { 00241 // OctreeBase is a friend! 00242 friend class OctreeBase; 00243 00244 public: 00245 00247 OctreeBranch () 00248 { 00249 memset (this->subNodes_, 0, sizeof(this->subNodes_)); 00250 } 00251 00253 virtual 00254 ~OctreeBranch () 00255 { 00256 } 00257 00261 virtual node_type_t 00262 getNodeType () const 00263 { 00264 return BRANCH_NODE; 00265 } 00266 00267 private: 00268 00270 const OctreeNode * subNodes_[8]; 00271 00272 }; 00273 00274 typedef LeafT OctreeLeaf; 00275 00277 // Protected octree methods based on octree keys 00279 00280 00282 const OctreeNode* 00283 getRootNode () const 00284 { 00285 return this->rootNode_; 00286 } 00287 00293 virtual bool 00294 genOctreeKeyForDataT (const DataT& data_arg, OctreeKey & key_arg) const 00295 { 00296 // this class cannot relate DataT objects to octree keys 00297 return false; 00298 } 00299 00305 virtual bool 00306 genDataTByOctreeKey (const OctreeKey & key_arg, DataT& data_arg) const 00307 { 00308 // this class cannot relate DataT objects to octree keys 00309 return false; 00310 } 00311 00318 inline void 00319 genOctreeKeyByIntIdx (const unsigned int idxX_arg, const unsigned int idxY_arg, const unsigned int idxZ_arg, 00320 OctreeKey & key_arg) const 00321 { 00322 // copy data to octree key class 00323 key_arg.x = idxX_arg; 00324 key_arg.y = idxY_arg; 00325 key_arg.z = idxZ_arg; 00326 } 00327 00332 inline void 00333 add (const OctreeKey& key_arg, const DataT& data_arg) 00334 { 00335 // request a (new) leaf from tree 00336 LeafT* leaf = getLeaf (key_arg); 00337 00338 // assign data to leaf 00339 if (leaf) 00340 { 00341 leaf->setData (data_arg); 00342 objectCount_++; 00343 } 00344 } 00345 00350 void 00351 add (const std::vector<OctreeKey>& key_vector_arg, const std::vector<DataT>& data_vector_arg); 00352 00357 inline LeafT* 00358 findLeaf (const OctreeKey& key_arg) const 00359 { 00360 return findLeafRecursive (key_arg, depthMask_, rootNode_); 00361 } 00362 00368 inline LeafT* 00369 getLeaf (const OctreeKey& key_arg) 00370 { 00371 return getLeafRecursive (key_arg, depthMask_, rootNode_); 00372 } 00373 00378 inline bool 00379 existLeaf (const OctreeKey& key_arg) const 00380 { 00381 return (findLeafRecursive (key_arg, depthMask_, rootNode_) != 0); 00382 } 00383 00387 inline void 00388 removeLeaf (const OctreeKey& key_arg) 00389 { 00390 deleteLeafRecursive (key_arg, depthMask_, rootNode_); 00391 } 00392 00394 // Branch node accessor inline functions 00396 00402 inline const OctreeNode* 00403 getBranchChild (const OctreeBranch& branch_arg, const unsigned char childIdx_arg) const 00404 { 00405 return branch_arg.subNodes_[childIdx_arg]; 00406 } 00407 00413 inline bool 00414 branchHasChild (const OctreeBranch& branch_arg, const unsigned char childIdx_arg) const 00415 { 00416 return (branch_arg.subNodes_[childIdx_arg] != 0); 00417 } 00418 00423 inline char 00424 getBranchBitPattern (const OctreeBranch& branch_arg) const 00425 { 00426 unsigned char i; 00427 char nodeBits; 00428 00429 // create bit pattern 00430 nodeBits = 0; 00431 for (i = 0; i < 8; i++) 00432 { 00433 nodeBits |= (!!branch_arg.subNodes_[i]) << i; 00434 } 00435 00436 return nodeBits; 00437 } 00438 00444 inline void 00445 setBranchChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg, const OctreeNode * newChild_arg) 00446 { 00447 branch_arg.subNodes_[childIdx_arg] = newChild_arg; 00448 } 00449 00454 inline void 00455 deleteBranchChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg) 00456 { 00457 if (branchHasChild (branch_arg, childIdx_arg)) 00458 { 00459 const OctreeNode* branchChild; 00460 branchChild = getBranchChild (branch_arg, childIdx_arg); 00461 00462 switch (branchChild->getNodeType ()) 00463 { 00464 case BRANCH_NODE: 00465 // free child branch recursively 00466 deleteBranch (*(OctreeBranch*)branchChild); 00467 00468 // push unused branch to branch pool 00469 unusedBranchesPool_.push_back ((OctreeBranch*)branchChild); 00470 break; 00471 00472 case LEAF_NODE: 00473 00474 // push unused leaf to branch pool 00475 unusedLeafsPool_.push_back ((OctreeLeaf*)branchChild); 00476 break; 00477 } 00478 00479 // set branch child pointer to 0 00480 setBranchChild (branch_arg, childIdx_arg, 0); 00481 } 00482 } 00483 00487 inline void 00488 deleteBranch (OctreeBranch& branch_arg) 00489 { 00490 char i; 00491 00492 // delete all branch node children 00493 for (i = 0; i < 8; i++) 00494 { 00495 deleteBranchChild (branch_arg, i); 00496 } 00497 } 00498 00502 inline void 00503 createBranch (OctreeBranch*& newBranchChild_arg) 00504 { 00505 if (!unusedBranchesPool_.size ()) 00506 { 00507 // branch pool is empty 00508 // we need to create a new octree branch class 00509 newBranchChild_arg = (OctreeBranch*)new OctreeBranch (); 00510 } 00511 else 00512 { 00513 // reuse branch from branch pool 00514 newBranchChild_arg = unusedBranchesPool_.back (); 00515 unusedBranchesPool_.pop_back (); 00516 branchReset (*newBranchChild_arg); 00517 } 00518 } 00519 00525 inline void 00526 createBranchChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg, 00527 OctreeBranch*& newBranchChild_arg) 00528 { 00529 createBranch ( newBranchChild_arg ); 00530 setBranchChild (branch_arg, childIdx_arg, (OctreeNode*)newBranchChild_arg); 00531 } 00532 00533 00539 inline void 00540 createLeafChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg, OctreeLeaf*& newLeafChild_arg) 00541 { 00542 00543 if (!unusedLeafsPool_.size ()) 00544 { 00545 // leaf pool is empty 00546 // we need to create a new octree leaf class 00547 newLeafChild_arg = (OctreeLeaf*)new OctreeLeaf (); 00548 } 00549 else 00550 { 00551 // reuse leaf node from branch pool 00552 newLeafChild_arg = unusedLeafsPool_.back (); 00553 unusedLeafsPool_.pop_back (); 00554 } 00555 00556 newLeafChild_arg->reset (); 00557 00558 setBranchChild (branch_arg, childIdx_arg, (OctreeNode*)newLeafChild_arg); 00559 00560 } 00561 00565 inline void 00566 branchReset (OctreeBranch& branch_arg) 00567 { 00568 memset (branch_arg.subNodes_, 0, sizeof(branch_arg.subNodes_)); 00569 } 00570 00573 inline void 00574 poolCleanUp () 00575 { 00576 // delete all branch instances from branch pool 00577 while (!unusedBranchesPool_.empty ()) 00578 { 00579 delete (unusedBranchesPool_.back ()); 00580 unusedBranchesPool_.pop_back (); 00581 } 00582 00583 // delete all leaf instances from leaf pool 00584 while (!unusedLeafsPool_.empty ()) 00585 { 00586 delete (unusedLeafsPool_.back ()); 00587 unusedLeafsPool_.pop_back (); 00588 } 00589 } 00590 00592 // Recursive octree methods 00594 00595 00602 LeafT* 00603 getLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg, OctreeBranch* branch_arg); 00604 00612 LeafT* 00613 findLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg, OctreeBranch* branch_arg) const; 00614 00621 bool 00622 deleteLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg, OctreeBranch* branch_arg); 00623 00629 void 00630 serializeTreeRecursive (std::vector<char>& binaryTreeOut_arg, const OctreeBranch* branch_arg, const OctreeKey& key_arg); 00631 00638 void 00639 serializeTreeRecursive (std::vector<char>& binaryTreeOut_arg, const OctreeBranch* branch_arg, 00640 const OctreeKey& key_arg, typename std::vector<DataT>& dataVector_arg); 00641 00647 void 00648 serializeLeafsRecursive (const OctreeBranch* branch_arg, const OctreeKey& key_arg, 00649 typename std::vector<DataT>& dataVector_arg); 00650 00657 void 00658 deserializeTreeRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg, 00659 OctreeBranch* branch_arg, const unsigned int depthMask_arg, const OctreeKey& key_arg); 00660 00669 void 00670 deserializeTreeRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg, 00671 OctreeBranch* branch_arg, const unsigned int depthMask_arg, const OctreeKey& key_arg, 00672 typename std::vector<DataT>::const_iterator& dataVectorIterator_arg, 00673 typename std::vector<DataT>::const_iterator& dataVectorEndIterator_arg); 00674 00682 void 00683 deserializeTreeAndOutputLeafDataRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg, 00684 OctreeBranch* branch_arg, const unsigned int depthMask_arg, 00685 const OctreeKey& key_arg, 00686 typename std::vector<DataT>& dataVector_arg); 00687 00689 // Serialization callbacks 00691 00696 virtual void 00697 serializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg); 00698 00704 virtual void 00705 serializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg, std::vector<DataT>& dataVector_arg); 00706 00711 virtual void 00712 deserializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg); 00713 00720 virtual void 00721 deserializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg, 00722 typename std::vector<DataT>::const_iterator& dataVectorIterator_arg, 00723 typename std::vector<DataT>::const_iterator& dataVectorEndIterator_arg); 00724 00730 virtual void 00731 deserializeTreeAndSerializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey & key_arg, 00732 std::vector<DataT>& dataVector_arg); 00733 00735 // Helpers 00737 00742 inline double 00743 Log2 (double n_arg) 00744 { 00745 return log( n_arg ) / log( 2.0 ); 00746 } 00747 00751 inline bool 00752 octreeCanResize () 00753 { 00754 return (true); 00755 } 00756 00758 // Globals 00760 00762 unsigned int leafCount_; 00763 00765 unsigned int branchCount_; 00766 00768 unsigned int objectCount_; 00769 00771 OctreeBranch* rootNode_; 00772 00774 unsigned int depthMask_; 00775 00777 unsigned int octreeDepth_; 00778 00780 std::vector<OctreeBranch*> unusedBranchesPool_; 00781 00783 std::vector<LeafT*> unusedLeafsPool_; 00784 00785 }; 00786 } 00787 } 00788 00789 //#include "impl/octree_base.hpp" 00790 00791 #endif 00792