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_lowmemory_base.h 3017 2011-11-01 03:24:04Z rusu $ 00037 */ 00038 00039 #ifndef OCTREE_LOWMEM_TREE_BASE_H 00040 #define OCTREE_LOWMEM_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 OctreeLowMemBase 00065 { 00066 00067 friend class OctreeNodeIterator<DataT, LeafT, OctreeLowMemBase> ; 00068 friend class OctreeLeafNodeIterator<DataT, LeafT, OctreeLowMemBase> ; 00069 00070 public: 00071 00072 // Octree iterators 00073 typedef OctreeNodeIterator<DataT, LeafT, OctreeLowMemBase> Iterator; 00074 typedef const OctreeNodeIterator<DataT, LeafT, OctreeLowMemBase> ConstIterator; 00075 00076 // Octree iterators 00077 typedef OctreeLeafNodeIterator<DataT, LeafT, OctreeLowMemBase> LeafNodeIterator; 00078 typedef const OctreeLeafNodeIterator<DataT, LeafT, OctreeLowMemBase> ConstLeafNodeIterator; 00079 00081 OctreeLowMemBase (); 00082 00084 virtual 00085 ~OctreeLowMemBase (); 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 () 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 00165 void 00166 deleteTree ( ); 00167 00172 void 00173 serializeTree (std::vector<char>& binaryTreeOut_arg, bool doXOREncoding_arg = false); 00174 00180 void 00181 serializeTree (std::vector<char>& binaryTreeOut_arg, std::vector<DataT>& dataVector_arg, 00182 bool doXOREncoding_arg = false); 00183 00187 void 00188 serializeLeafs (std::vector<DataT>& dataVector_arg); 00189 00194 void 00195 deserializeTree (std::vector<char>& binaryTreeIn_arg, bool doXORDecoding_arg = false); 00196 00202 void 00203 deserializeTree (std::vector<char>& binaryTreeIn_arg, std::vector<DataT>& dataVector_arg, 00204 bool doXORDecoding_arg = false); 00205 00210 void 00211 deserializeTreeAndOutputLeafData (std::vector<char>& binaryTreeIn_arg, std::vector<DataT>& dataVector_arg); 00212 00214 void 00215 switchBuffers () 00216 { 00217 this->deleteTree (); 00218 } 00219 00220 protected: 00221 00223 00227 00228 class OctreeKey 00229 { 00230 public: 00231 00235 bool 00236 operator == (const OctreeKey& b) const 00237 { 00238 return ((b.x == this->x) && (b.y == this->y) && (b.z == this->z)); 00239 } 00240 00241 // Indices addressing a voxel at (X, Y, Z) 00242 unsigned int x; 00243 unsigned int y; 00244 unsigned int z; 00245 }; 00246 00248 00252 00253 class OctreeBranch : public OctreeNode 00254 { 00255 // OctreeBase is a friend! 00256 friend class OctreeLowMemBase; 00257 00258 public: 00259 00261 OctreeBranch () 00262 { 00263 // initialize 00264 occupancyByte_ = 0; 00265 this->reset(); 00266 } 00267 00269 void 00270 reset () 00271 { 00272 // if pointers to childs exist, we delete them 00273 if (occupancyByte_) 00274 delete[] subNodes_; 00275 00276 // reset occupancy byte 00277 occupancyByte_ = 0; 00278 } 00279 00281 virtual 00282 ~OctreeBranch () 00283 { 00284 } 00285 00289 virtual node_type_t 00290 getNodeType () const 00291 { 00292 return BRANCH_NODE; 00293 } 00294 00295 private: 00296 00297 unsigned char occupancyByte_; 00298 OctreeNode** subNodes_; 00299 00300 }; 00301 00302 typedef LeafT OctreeLeaf; 00303 00305 // Protected octree methods based on octree keys 00307 00309 const OctreeNode* 00310 getRootNode () const 00311 { 00312 return this->rootNode_; 00313 } 00314 00320 virtual bool 00321 genOctreeKeyForDataT (const DataT& data_arg, OctreeKey & key_arg) const 00322 { 00323 // this class cannot relate DataT objects to octree keys 00324 return false; 00325 } 00326 00332 virtual bool 00333 genDataTByOctreeKey (const OctreeKey & key_arg, DataT& data_arg) const 00334 { 00335 // this class cannot relate DataT objects to octree keys 00336 return false; 00337 } 00338 00345 inline void 00346 genOctreeKeyByIntIdx (const unsigned int idxX_arg, const unsigned int idxY_arg, const unsigned int idxZ_arg, 00347 OctreeKey & key_arg) const 00348 { 00349 // copy data to octree key class 00350 key_arg.x = idxX_arg; 00351 key_arg.y = idxY_arg; 00352 key_arg.z = idxZ_arg; 00353 } 00354 00359 inline void 00360 add (const OctreeKey& key_arg, const DataT& data_arg) 00361 { 00362 // request a (new) leaf from tree 00363 LeafT* leaf = getLeaf (key_arg); 00364 00365 // assign data to leaf 00366 if (leaf) 00367 { 00368 leaf->setData (data_arg); 00369 objectCount_++; 00370 } 00371 } 00372 00377 void 00378 add (const std::vector<OctreeKey>& key_vector_arg, const std::vector<DataT>& data_vector_arg); 00379 00384 inline LeafT* 00385 findLeaf (const OctreeKey& key_arg) const 00386 { 00387 return findLeafRecursive (key_arg, depthMask_, rootNode_); 00388 } 00389 00395 inline LeafT* 00396 getLeaf (const OctreeKey& key_arg) 00397 { 00398 return getLeafRecursive (key_arg, depthMask_, rootNode_); 00399 } 00400 00405 inline bool 00406 existLeaf (const OctreeKey& key_arg) const 00407 { 00408 return (findLeafRecursive (key_arg, depthMask_, rootNode_) != 0); 00409 } 00410 00414 inline void 00415 removeLeaf (const OctreeKey& key_arg) 00416 { 00417 deleteLeafRecursive (key_arg, depthMask_, rootNode_); 00418 } 00419 00421 // Branch node accessor inline functions 00423 00429 inline const OctreeNode* 00430 getBranchChild (const OctreeBranch& branch_arg, const unsigned char childIdx_arg) const 00431 { 00432 // check if branch child exists according to occupancyBtye 00433 if (branch_arg.occupancyByte_ & (1 << childIdx_arg)) 00434 { 00435 unsigned int c, idx; 00436 00437 // retrieve array index of child pointer 00438 idx = 0; 00439 for (c = 0; c < childIdx_arg; c++) 00440 { 00441 if (branch_arg.occupancyByte_ & (1 << c)) 00442 idx++; 00443 } 00444 00445 return branch_arg.subNodes_[idx]; 00446 00447 } 00448 00449 return 0; 00450 } 00451 00457 inline bool 00458 branchHasChild (const OctreeBranch& branch_arg, const unsigned char childIdx_arg) const 00459 { 00460 // test occupancyByte for child existence 00461 return (branch_arg.occupancyByte_ & (1 << childIdx_arg)); 00462 } 00463 00468 inline char 00469 getBranchBitPattern (const OctreeBranch& branch_arg) const 00470 { 00471 return branch_arg.occupancyByte_; 00472 } 00473 00479 inline void 00480 setBranchChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg, OctreeNode * newChild_arg) 00481 { 00482 00483 if (!(branch_arg.occupancyByte_ & (1 << childIdx_arg))) 00484 { 00485 // branch child does not exists 00486 unsigned int c, idx, total; 00487 00488 // retrieve total amount of child pointers and retrieve index of new child pointer 00489 idx = 0; 00490 total = 0; 00491 for (c = 0; c < 8; c++) 00492 { 00493 if (branch_arg.occupancyByte_ & (1 << c)) 00494 { 00495 if (c < childIdx_arg) 00496 idx++; 00497 00498 total++; 00499 } 00500 } 00501 00502 if (newChild_arg != 0) 00503 { 00504 // a new child pointer is to be added to child array 00505 00506 // allocate new pointer array 00507 OctreeNode** newNodes = new OctreeNode*[total + 1]; 00508 00509 // copy pointers to new pointer array 00510 memcpy (&newNodes[0], &branch_arg.subNodes_[0], idx * sizeof(OctreeNode*)); 00511 newNodes[idx] = newChild_arg; 00512 memcpy (&newNodes[idx + 1], &branch_arg.subNodes_[idx], (total - idx) * sizeof(OctreeNode*)); 00513 00514 // delete old pointer array 00515 if (total > 0) 00516 delete[] branch_arg.subNodes_; 00517 00518 // set new child pointer array 00519 branch_arg.subNodes_ = newNodes; 00520 00521 // update occupancyByte 00522 branch_arg.occupancyByte_ |= (1 << childIdx_arg); 00523 } 00524 00525 } 00526 else 00527 { 00528 // child pointer already exists 00529 unsigned int c, idx, total; 00530 00531 // pointer set to 0 -> delete child pointer 00532 if (newChild_arg == 0) 00533 { 00534 00535 // retrieve total amount of child pointers and detect index of child pointer to be removed 00536 idx = 0; 00537 total = 0; 00538 for (c = 0; c < 8; c++) 00539 { 00540 if (branch_arg.occupancyByte_ & (1 << c)) 00541 { 00542 if (c < childIdx_arg) 00543 idx++; 00544 00545 total++; 00546 } 00547 } 00548 00549 if (total == 1) 00550 { 00551 // if only a single pointer exists, simply delete the data structure 00552 delete[] branch_arg.subNodes_; 00553 00554 branch_arg.subNodes_ = 0; 00555 branch_arg.occupancyByte_ = 0; 00556 } 00557 else 00558 { 00559 // allocate new pointer array with reduced size 00560 OctreeNode** newNodes = new OctreeNode*[total - 1]; 00561 00562 // copy old pointer references to new array 00563 memcpy (&newNodes[0], &branch_arg.subNodes_[0], idx * sizeof(OctreeNode*)); 00564 memcpy (&newNodes[idx], &branch_arg.subNodes_[idx + 1], (total - (idx + 1)) * sizeof(OctreeNode*)); 00565 00566 // delete previous pointer array and update "branch_arg.subNodes_" 00567 delete[] branch_arg.subNodes_; 00568 branch_arg.subNodes_ = newNodes; 00569 00570 // update occupancyByte by removing the corresponding bit 00571 branch_arg.occupancyByte_ &= ~(1 << childIdx_arg); 00572 00573 } 00574 00575 } 00576 else 00577 { 00578 // update existing child pointer 00579 00580 // retrieve index of child pointer to be updated 00581 idx = 0; 00582 for (c = 0; c < childIdx_arg; c++) 00583 { 00584 if (branch_arg.occupancyByte_ & (1 << c)) 00585 { 00586 idx++; 00587 } 00588 } 00589 00590 // update pointer 00591 branch_arg.subNodes_[idx] = newChild_arg; 00592 } 00593 00594 } 00595 00596 } 00597 00602 inline void 00603 deleteBranchChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg) 00604 { 00605 if (branchHasChild (branch_arg, childIdx_arg)) 00606 { 00607 const OctreeNode* branchChild; 00608 branchChild = getBranchChild (branch_arg, childIdx_arg); 00609 00610 switch (branchChild->getNodeType ()) 00611 { 00612 case BRANCH_NODE: 00613 // free child branch recursively 00614 deleteBranch (*(OctreeBranch*)branchChild); 00615 00616 break; 00617 00618 case LEAF_NODE: 00619 00620 break; 00621 } 00622 00623 // delete branch child 00624 delete (branchChild); 00625 00626 // set branch child pointer to 0 00627 setBranchChild (branch_arg, childIdx_arg, 0); 00628 } 00629 } 00630 00634 inline void 00635 deleteBranch (OctreeBranch& branch_arg) 00636 { 00637 char i; 00638 00639 // delete all branch node children 00640 for (i = 0; i < 8; i++) 00641 { 00642 deleteBranchChild (branch_arg, i); 00643 } 00644 } 00645 00649 inline void 00650 createBranch (OctreeBranch*& newBranchChild_arg) 00651 { 00652 00653 // we need to create a new octree branch class 00654 newBranchChild_arg = (OctreeBranch*)new OctreeBranch (); 00655 00656 } 00657 00663 inline void 00664 createBranchChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg, 00665 OctreeBranch*& newBranchChild_arg) 00666 { 00667 createBranch (newBranchChild_arg); 00668 setBranchChild (branch_arg, childIdx_arg, (OctreeNode*)newBranchChild_arg); 00669 } 00670 00676 inline void 00677 createLeafChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg, OctreeLeaf*& newLeafChild_arg) 00678 { 00679 00680 // we need to create a new octree leaf class 00681 newLeafChild_arg = (OctreeLeaf*)new OctreeLeaf (); 00682 newLeafChild_arg->reset(); 00683 00684 setBranchChild (branch_arg, childIdx_arg, (OctreeNode*)newLeafChild_arg); 00685 00686 } 00687 00691 inline void 00692 branchReset (OctreeBranch& branch_arg) 00693 { 00694 branch_arg.reset(); 00695 } 00696 00697 00699 // Recursive octree methods 00701 00702 00709 LeafT* 00710 getLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg, OctreeBranch* branch_arg); 00711 00719 LeafT* 00720 findLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg, OctreeBranch* branch_arg) const; 00721 00728 bool 00729 deleteLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg, OctreeBranch* branch_arg); 00730 00736 void 00737 serializeTreeRecursive (std::vector<char>& binaryTreeOut_arg, const OctreeBranch* branch_arg, const OctreeKey& key_arg); 00738 00745 void 00746 serializeTreeRecursive (std::vector<char>& binaryTreeOut_arg, const OctreeBranch* branch_arg, 00747 const OctreeKey& key_arg, typename std::vector<DataT>& dataVector_arg); 00748 00754 void 00755 serializeLeafsRecursive (const OctreeBranch* branch_arg, const OctreeKey& key_arg, 00756 typename std::vector<DataT>& dataVector_arg); 00757 00764 void 00765 deserializeTreeRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg, 00766 OctreeBranch* branch_arg, const unsigned int depthMask_arg, const OctreeKey& key_arg); 00767 00776 void 00777 deserializeTreeRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg, 00778 OctreeBranch* branch_arg, const unsigned int depthMask_arg, const OctreeKey& key_arg, 00779 typename std::vector<DataT>::const_iterator& dataVectorIterator_arg, 00780 typename std::vector<DataT>::const_iterator& dataVectorEndIterator_arg); 00781 00789 void 00790 deserializeTreeAndOutputLeafDataRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg, 00791 OctreeBranch* branch_arg, const unsigned int depthMask_arg, 00792 const OctreeKey& key_arg, 00793 typename std::vector<DataT>& dataVector_arg); 00794 00796 // Serialization callbacks 00798 00803 virtual void 00804 serializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg); 00805 00811 virtual void 00812 serializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg, std::vector<DataT>& dataVector_arg); 00813 00818 virtual void 00819 deserializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg); 00820 00827 virtual void 00828 deserializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg, 00829 typename std::vector<DataT>::const_iterator& dataVectorIterator_arg, 00830 typename std::vector<DataT>::const_iterator& dataVectorEndIterator_arg); 00831 00837 virtual void 00838 deserializeTreeAndSerializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey & key_arg, 00839 std::vector<DataT>& dataVector_arg); 00840 00842 // Helpers 00844 00849 inline double 00850 Log2 (double n_arg) 00851 { 00852 return log( n_arg ) / log( 2.0 ); 00853 } 00854 00858 inline bool 00859 octreeCanResize () 00860 { 00861 return (true); 00862 } 00863 00865 // Globals 00867 00869 unsigned int leafCount_; 00870 00872 unsigned int branchCount_; 00873 00875 unsigned int objectCount_; 00876 00878 OctreeBranch* rootNode_; 00879 00881 unsigned int depthMask_; 00882 00884 unsigned int octreeDepth_; 00885 00887 std::vector<OctreeBranch*> unusedBranchesPool_; 00888 00890 std::vector<LeafT*> unusedLeafsPool_; 00891 00892 }; 00893 } 00894 } 00895 00896 //#include "impl/octree_base.hpp" 00897 00898 #endif 00899