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: octree2buf_base.h 3017 2011-11-01 03:24:04Z rusu $ 00037 */ 00038 00039 #ifndef OCTREE_TREE_2BUF_BASE_H 00040 #define OCTREE_TREE_2BUF_BASE_H 00041 00042 #include <vector> 00043 #include <math.h> 00044 00045 #include "octree_nodes.h" 00046 00047 #include "octree_iterator.h" 00048 00049 namespace pcl 00050 { 00051 namespace octree 00052 { 00066 template<typename DataT, typename LeafT = OctreeLeafDataT<DataT> > 00067 class Octree2BufBase 00068 { 00069 00070 friend class OctreeNodeIterator<DataT, LeafT, Octree2BufBase> ; 00071 friend class OctreeLeafNodeIterator<DataT, LeafT, Octree2BufBase> ; 00072 00073 public: 00074 00075 // Octree iterators 00076 typedef OctreeNodeIterator<DataT, LeafT, Octree2BufBase> Iterator; 00077 typedef const OctreeNodeIterator<DataT, LeafT, Octree2BufBase> ConstIterator; 00078 00079 // Octree iterators 00080 typedef OctreeLeafNodeIterator<DataT, LeafT, Octree2BufBase> LeafNodeIterator; 00081 typedef const OctreeLeafNodeIterator<DataT, LeafT, Octree2BufBase> ConstLeafNodeIterator; 00082 00084 Octree2BufBase (); 00085 00087 virtual 00088 ~Octree2BufBase (); 00089 00093 void 00094 setMaxVoxelIndex (unsigned int maxVoxelIndex_arg); 00095 00099 void 00100 setTreeDepth (unsigned int depth_arg); 00101 00105 inline unsigned int 00106 getTreeDepth () 00107 { 00108 return this->octreeDepth_; 00109 } 00110 00117 void 00118 add (const unsigned int idxX_arg, const unsigned int idxY_arg, const unsigned int idxZ_arg, 00119 const DataT& data_arg); 00120 00128 bool 00129 get (const unsigned int idxX_arg, const unsigned int idxY_arg, const unsigned int idxZ_arg, DataT& data_arg) const ; 00130 00137 bool 00138 existLeaf (const unsigned int idxX_arg, const unsigned int idxY_arg, const unsigned int idxZ_arg) const ; 00139 00145 void 00146 removeLeaf (const unsigned int idxX_arg, const unsigned int idxY_arg, const unsigned int idxZ_arg); 00147 00151 inline unsigned int 00152 getLeafCount () const 00153 { 00154 return leafCount_; 00155 } 00156 00160 inline unsigned int 00161 getBranchCount () const 00162 { 00163 return branchCount_; 00164 } 00165 00169 void 00170 deleteTree ( bool freeMemory_arg = false ); 00171 00173 inline void 00174 deletePreviousBuffer () 00175 { 00176 treeCleanUpRecursive (rootNode_); 00177 } 00178 00180 inline void 00181 deleteCurrentBuffer () 00182 { 00183 bufferSelector_ = !bufferSelector_; 00184 treeCleanUpRecursive (rootNode_); 00185 leafCount_ = 0; 00186 } 00187 00189 void 00190 switchBuffers (); 00191 00196 void 00197 serializeTree (std::vector<char>& binaryTreeOut_arg, bool doXOREncoding_arg = false); 00198 00204 void 00205 serializeTree (std::vector<char>& binaryTreeOut_arg, std::vector<DataT>& dataVector_arg, 00206 bool doXOREncoding_arg = false); 00207 00211 void 00212 serializeLeafs (std::vector<DataT>& dataVector_arg); 00213 00218 void 00219 serializeNewLeafs (std::vector<DataT>& dataVector_arg, const int minPointsPerLeaf_arg = 0); 00220 00225 void 00226 deserializeTree (std::vector<char>& binaryTreeIn_arg, bool doXORDecoding_arg = false); 00227 00233 void 00234 deserializeTree (std::vector<char>& binaryTreeIn_arg, std::vector<DataT>& dataVector_arg, 00235 bool doXORDecoding_arg = false); 00236 00242 void 00243 deserializeTreeAndOutputLeafData (std::vector<char>& binaryTreeIn_arg, std::vector<DataT>& dataVector_arg, 00244 bool doXORDecoding_arg = false); 00245 00246 protected: 00247 00249 00253 00254 class OctreeKey 00255 { 00256 public: 00257 00261 bool 00262 operator == (const OctreeKey& b) const 00263 { 00264 return ((b.x == this->x) && (b.y == this->y) && (b.z == this->z)); 00265 } 00266 00267 // Indices addressing a voxel at (X, Y, Z) 00268 unsigned int x;unsigned int y;unsigned int z; 00269 }; 00270 00272 00276 00277 class OctreeBranch : public OctreeNode 00278 { 00279 00280 // Octree2BufBase is a friend! 00281 friend class Octree2BufBase; 00282 00283 public: 00284 00286 OctreeBranch () 00287 { 00288 memset (this->subNodes_, 0, sizeof(this->subNodes_)); 00289 } 00290 00292 virtual 00293 ~OctreeBranch () 00294 { 00295 } 00296 00300 virtual node_type_t 00301 getNodeType () const 00302 { 00303 return BRANCH_NODE; 00304 } 00305 00306 private: 00307 00309 const OctreeNode * subNodes_[2][8]; 00310 00311 }; 00312 00313 typedef LeafT OctreeLeaf; 00314 00316 // Protected octree methods based on octree keys 00318 00320 const OctreeNode* 00321 getRootNode () const 00322 { 00323 return this->rootNode_; 00324 } 00325 00331 virtual bool 00332 genOctreeKeyForDataT (const DataT& data_arg, OctreeKey & key_arg) const 00333 { 00334 // this class cannot relate DataT objects to octree keys 00335 return false; 00336 } 00337 00343 virtual bool 00344 genDataTByOctreeKey (const OctreeKey & key_arg, DataT& data_arg) const 00345 { 00346 // this class cannot relate DataT objects to octree keys 00347 return false; 00348 } 00349 00356 inline void 00357 genOctreeKeyByIntIdx (const unsigned int idxX_arg, const unsigned int idxY_arg, const unsigned int idxZ_arg, 00358 OctreeKey & key_arg) const 00359 { 00360 // copy data to octree key class 00361 key_arg.x = idxX_arg; 00362 key_arg.y = idxY_arg; 00363 key_arg.z = idxZ_arg; 00364 } 00365 00370 inline void 00371 add (const OctreeKey& key_arg, const DataT& data_arg) 00372 { 00373 // request a (new) leaf from tree 00374 LeafT* leaf = getLeaf (key_arg); 00375 00376 // assign data to leaf 00377 if (leaf) 00378 { 00379 leaf->setData (data_arg); 00380 objectCount_++; 00381 } 00382 } 00383 00388 inline LeafT* 00389 findLeaf (const OctreeKey& key_arg) const 00390 { 00391 return findLeafRecursive (key_arg, depthMask_, rootNode_); 00392 } 00393 00399 inline LeafT* 00400 getLeaf (const OctreeKey& key_arg) 00401 { 00402 LeafT* result; 00403 00404 result = getLeafRecursive (key_arg, depthMask_, rootNode_, resetTree_); 00405 00406 // getLeafRecursive has changed the octree -> clean-up/tree-reset might be required 00407 resetTree_ = false; 00408 treeDirtyFlag_ = true; 00409 00410 return result; 00411 } 00412 00417 inline bool 00418 existLeaf (const OctreeKey& key_arg) const 00419 { 00420 return (findLeafRecursive (key_arg, depthMask_, rootNode_) != 0); 00421 } 00422 00426 inline void 00427 removeLeaf (const OctreeKey& key_arg) 00428 { 00429 deleteLeafRecursive (key_arg, depthMask_, rootNode_); 00430 00431 // we changed the octree structure -> dirty 00432 treeDirtyFlag_ = true; 00433 } 00434 00436 // Branch node accessor inline functions 00438 00439 00445 inline const OctreeNode* 00446 getBranchChild (const OctreeBranch& branch_arg, const unsigned char childIdx_arg) const 00447 { 00448 return branch_arg.subNodes_[bufferSelector_][childIdx_arg]; 00449 } 00450 00457 inline const OctreeNode* 00458 getBranchChild (const OctreeBranch& branch_arg, const unsigned char bufferSelector_arg, 00459 const unsigned char childIdx_arg) const 00460 { 00461 return branch_arg.subNodes_[bufferSelector_arg][childIdx_arg]; 00462 } 00463 00469 inline bool 00470 branchHasChild (const OctreeBranch& branch_arg, const unsigned char childIdx_arg) const 00471 { 00472 return (branch_arg.subNodes_[bufferSelector_][childIdx_arg] != 0); 00473 } 00474 00481 inline bool 00482 branchHasChild (const OctreeBranch& branch_arg, const unsigned char bufferSelector_arg, 00483 const unsigned char childIdx_arg) const 00484 { 00485 return (branch_arg.subNodes_[bufferSelector_arg][childIdx_arg] != 0); 00486 00487 } 00488 00493 inline char 00494 getBranchBitPattern (const OctreeBranch& branch_arg) const 00495 { 00496 unsigned char i; 00497 char nodeBits; 00498 00499 // create bit pattern 00500 nodeBits = 0; 00501 for (i = 0; i < 8; i++) 00502 { 00503 nodeBits |= (!!branch_arg.subNodes_[bufferSelector_][i]) << i; 00504 } 00505 00506 return nodeBits; 00507 } 00508 00514 inline char 00515 getBranchBitPattern (const OctreeBranch& branch_arg, const unsigned char bufferSelector_arg) const 00516 { 00517 unsigned char i; 00518 char nodeBits; 00519 00520 // create bit pattern 00521 nodeBits = 0; 00522 for (i = 0; i < 8; i++) 00523 { 00524 nodeBits |= (!!branch_arg.subNodes_[bufferSelector_arg][i]) << i; 00525 } 00526 00527 return nodeBits; 00528 } 00529 00534 inline char 00535 getBranchXORBitPattern (const OctreeBranch& branch_arg) const 00536 { 00537 unsigned char i; 00538 char nodeBits[2]; 00539 00540 // create bit pattern for both buffers 00541 nodeBits[0] = nodeBits[1] = 0; 00542 00543 for (i = 0; i < 8; i++) 00544 { 00545 nodeBits[0] |= (!!branch_arg.subNodes_[0][i]) << i; 00546 nodeBits[1] |= (!!branch_arg.subNodes_[1][i]) << i; 00547 } 00548 00549 return nodeBits[0] ^ nodeBits[1]; 00550 } 00551 00556 inline bool 00557 hasBranchChanges (const OctreeBranch& branch_arg) const 00558 { 00559 return (getBranchXORBitPattern (branch_arg) > 0); 00560 } 00561 00567 inline void 00568 setBranchChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg, const OctreeNode * newChild_arg) 00569 { 00570 branch_arg.subNodes_[bufferSelector_][childIdx_arg] = newChild_arg; 00571 } 00572 00579 inline void 00580 setBranchChild (OctreeBranch& branch_arg, const unsigned char bufferSelector_arg, 00581 const unsigned char childIdx_arg, const OctreeNode * newChild_arg) 00582 { 00583 branch_arg.subNodes_[bufferSelector_arg][childIdx_arg] = newChild_arg; 00584 } 00585 00590 inline void 00591 deleteBranchChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg) 00592 { 00593 if (branchHasChild (branch_arg, childIdx_arg)) 00594 { 00595 const OctreeNode* branchChild; 00596 branchChild = getBranchChild (branch_arg, childIdx_arg); 00597 00598 switch (branchChild->getNodeType ()) 00599 { 00600 case BRANCH_NODE: 00601 // free child branch recursively 00602 deleteBranch (*(OctreeBranch*)branchChild); 00603 00604 // push unused branch to branch pool 00605 unusedBranchesPool_.push_back ((OctreeBranch*)branchChild); 00606 break; 00607 00608 case LEAF_NODE: 00609 00610 // push unused leaf to branch pool 00611 unusedLeafsPool_.push_back ((OctreeLeaf*)branchChild); 00612 break; 00613 } 00614 00615 // set branch child pointer to 0 00616 setBranchChild (branch_arg, childIdx_arg, 0); 00617 } 00618 } 00619 00625 inline void 00626 deleteBranchChild (OctreeBranch& branch_arg, const unsigned char bufferSelector_arg, 00627 const unsigned char childIdx_arg) 00628 { 00629 if (branchHasChild (branch_arg, bufferSelector_arg, childIdx_arg)) 00630 { 00631 const OctreeNode* branchChild; 00632 branchChild = getBranchChild (branch_arg, bufferSelector_arg, childIdx_arg); 00633 00634 switch (branchChild->getNodeType ()) 00635 { 00636 case BRANCH_NODE: 00637 // free child branch recursively 00638 deleteBranch (*(OctreeBranch*)branchChild); 00639 00640 // push unused branch to branch pool 00641 unusedBranchesPool_.push_back ((OctreeBranch*)branchChild); 00642 break; 00643 00644 case LEAF_NODE: 00645 00646 // push unused leaf to branch pool 00647 unusedLeafsPool_.push_back ((OctreeLeaf*)branchChild); 00648 break; 00649 } 00650 00651 // set branch child pointer to 0 00652 setBranchChild (branch_arg, bufferSelector_arg, childIdx_arg, 0); 00653 00654 } 00655 } 00656 00660 inline void 00661 deleteBranch (OctreeBranch& branch_arg) 00662 { 00663 char i; 00664 00665 // delete all branch node children 00666 for (i = 0; i < 8; i++) 00667 { 00668 00669 if (getBranchChild (branch_arg, 0, i) == getBranchChild (branch_arg, 1, i)) 00670 { 00671 // reference was copied - there is only one child instance to be deleted 00672 deleteBranchChild (branch_arg, 0, i); 00673 00674 // remove pointers from both buffers 00675 setBranchChild (branch_arg, 0, i, 0); 00676 setBranchChild (branch_arg, 1, i, 0); 00677 00678 } 00679 else 00680 { 00681 deleteBranchChild (branch_arg, 0, i); 00682 deleteBranchChild (branch_arg, 1, i); 00683 } 00684 } 00685 } 00686 00692 inline void 00693 createBranchChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg, 00694 OctreeBranch*& newBranchChild_arg) 00695 { 00696 if (!unusedBranchesPool_.size ()) 00697 { 00698 newBranchChild_arg = (OctreeBranch*)new OctreeBranch (); 00699 } 00700 else 00701 { 00702 newBranchChild_arg = unusedBranchesPool_.back (); 00703 unusedBranchesPool_.pop_back (); 00704 branchReset (*newBranchChild_arg); 00705 } 00706 00707 setBranchChild (branch_arg, childIdx_arg, (OctreeNode*)newBranchChild_arg); 00708 } 00709 00713 inline void 00714 createBranch (OctreeBranch*& newBranchChild_arg) 00715 { 00716 00717 if (!unusedBranchesPool_.size ()) 00718 { 00719 // branch pool is empty 00720 // we need to create a new octree branch class 00721 newBranchChild_arg = (OctreeBranch*)new OctreeBranch (); 00722 } 00723 else 00724 { 00725 // reuse branch from branch pool 00726 newBranchChild_arg = unusedBranchesPool_.back (); 00727 unusedBranchesPool_.pop_back (); 00728 branchReset (*newBranchChild_arg); 00729 } 00730 } 00731 00737 inline void 00738 createLeafChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg, OctreeLeaf*& newLeafChild_arg) 00739 { 00740 if (!unusedLeafsPool_.size ()) 00741 { 00742 // leaf pool is empty 00743 // we need to create a new octree leaf class 00744 newLeafChild_arg = (OctreeLeaf*)new OctreeLeaf (); 00745 } 00746 else 00747 { 00748 // reuse leaf node from branch pool 00749 newLeafChild_arg = unusedLeafsPool_.back (); 00750 unusedLeafsPool_.pop_back (); 00751 } 00752 00753 newLeafChild_arg->reset (); 00754 00755 setBranchChild (branch_arg, childIdx_arg, (OctreeNode*)newLeafChild_arg); 00756 } 00757 00761 inline void 00762 branchReset (OctreeBranch& branch_arg) 00763 { 00764 memset (branch_arg.subNodes_, 0, sizeof(branch_arg.subNodes_)); 00765 } 00766 00769 inline void 00770 poolCleanUp () 00771 { 00772 // delete all branch instances from branch pool 00773 while (!unusedBranchesPool_.empty ()) 00774 { 00775 delete (unusedBranchesPool_.back ()); 00776 unusedBranchesPool_.pop_back (); 00777 } 00778 00779 // delete all leaf instances from leaf pool 00780 while (!unusedLeafsPool_.empty ()) 00781 { 00782 delete (unusedLeafsPool_.back ()); 00783 unusedLeafsPool_.pop_back (); 00784 } 00785 } 00786 00788 // Recursive octree methods 00790 00791 00799 LeafT* 00800 getLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg, OctreeBranch* branch_arg, 00801 bool branchReset_arg); 00802 00810 LeafT* 00811 findLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg, OctreeBranch* branch_arg) const; 00812 00819 bool 00820 deleteLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg, OctreeBranch* branch_arg); 00821 00828 void 00829 serializeTreeRecursive (std::vector<char>& binaryTreeOut_arg, OctreeBranch* branch_arg, const OctreeKey& key_arg, 00830 bool doXOREncoding_arg); 00831 00839 void 00840 serializeTreeRecursive (std::vector<char>& binaryTreeOut_arg, OctreeBranch* branch_arg, const OctreeKey& key_arg, 00841 typename std::vector<DataT>& dataVector_arg, bool doXOREncoding_arg); 00842 00848 void 00849 serializeLeafsRecursive (OctreeBranch* branch_arg, const OctreeKey& key_arg, 00850 typename std::vector<DataT>& dataVector_arg); 00851 00858 void 00859 serializeNewLeafsRecursive (OctreeBranch* branch_arg, const OctreeKey& key_arg, 00860 std::vector<DataT>& dataVector_arg, const int minPointsPerLeaf_arg = 0); 00861 00870 void 00871 deserializeTreeRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg, 00872 OctreeBranch* branch_arg, const unsigned int depthMask_arg, const OctreeKey& key_arg, 00873 bool branchReset_arg, bool doXORDecoding_arg); 00874 00885 void 00886 deserializeTreeRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg, 00887 OctreeBranch* branch_arg, const unsigned int depthMask_arg, const OctreeKey& key_arg, 00888 typename std::vector<DataT>::const_iterator& dataVectorIterator_arg, 00889 typename std::vector<DataT>::const_iterator& dataVectorEndIterator_arg, 00890 bool branchReset_arg, bool doXORDecoding_arg); 00891 00901 void 00902 deserializeTreeAndOutputLeafDataRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg, 00903 OctreeBranch* branch_arg, const unsigned int depthMask_arg, 00904 const OctreeKey& key_arg, 00905 typename std::vector<DataT>& dataVector_arg, 00906 bool branchReset_arg, bool doXORDecoding_arg); 00907 00909 // Serialization callbacks 00911 00916 virtual void 00917 serializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg); 00918 00924 virtual void 00925 serializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg, std::vector<DataT>& dataVector_arg); 00926 00933 virtual void 00934 serializeNewLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg, const int minPointsPerLeaf_arg, 00935 std::vector<DataT>& dataVector_arg); 00936 00941 virtual void 00942 deserializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg); 00943 00950 virtual void 00951 deserializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg, 00952 typename std::vector<DataT>::const_iterator& dataVectorIterator_arg, 00953 typename std::vector<DataT>::const_iterator& dataVectorEndIterator_arg); 00954 00960 virtual void 00961 deserializeTreeAndSerializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey & key_arg, 00962 std::vector<DataT>& dataVector_arg); 00963 00965 // Helpers 00967 00971 void 00972 treeCleanUpRecursive (OctreeBranch* branch_arg); 00973 00978 inline double 00979 Log2 (double n_arg) 00980 { 00981 return log (n_arg) / log (2.0); 00982 } 00983 00987 inline bool 00988 octreeCanResize () 00989 { 00990 return (false); 00991 } 00992 00994 // Globals 00996 00997 00999 unsigned int leafCount_; 01000 01002 unsigned int branchCount_; 01003 01005 unsigned int objectCount_; 01006 01008 OctreeBranch* rootNode_; 01009 01011 unsigned int depthMask_; 01012 01014 std::vector<OctreeBranch*> unusedBranchesPool_; 01015 01017 std::vector<LeafT*> unusedLeafsPool_; 01018 01020 unsigned char bufferSelector_; 01021 01022 // flags indicating if unused branches and leafs might exist in previous buffer 01023 bool resetTree_; 01024 bool treeDirtyFlag_; 01025 01027 unsigned int octreeDepth_; 01028 01029 }; 01030 } 01031 } 01032 01033 //#include "impl/octree2buf_base.hpp" 01034 01035 #endif 01036