00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #ifndef OCTREE_TREE_2BUF_BASE_H
00040 #define OCTREE_TREE_2BUF_BASE_H
00041
00042 #include <vector>
00043
00044 #include "octree_nodes.h"
00045 #include "octree_container.h"
00046 #include "octree_key.h"
00047 #include "octree_iterator.h"
00048 #include "octree_node_pool.h"
00049
00050 #include <stdio.h>
00051 #include <string.h>
00052
00053 namespace pcl
00054 {
00055 namespace octree
00056 {
00057
00058 template<typename ContainerT>
00059 class BufferedBranchNode : public OctreeNode, ContainerT
00060 {
00061 using ContainerT::getSize;
00062 using ContainerT::getData;
00063 using ContainerT::setData;
00064
00065 public:
00067 BufferedBranchNode () : OctreeNode(), ContainerT(), preBuf(0xFFFFFF), postBuf(0xFFFFFF)
00068 {
00069 reset ();
00070 }
00071
00073 BufferedBranchNode (const BufferedBranchNode& source) : ContainerT(source)
00074 {
00075 *this = source;
00076 }
00077
00079 inline BufferedBranchNode&
00080 operator = (const BufferedBranchNode &source_arg)
00081 {
00082
00083 unsigned char i, b;
00084
00085 memset (childNodeArray_, 0, sizeof(childNodeArray_));
00086
00087 for (b = 0; b < 2; ++b)
00088 for (i = 0; i < 8; ++i)
00089 if (source_arg.childNodeArray_[b][i])
00090 childNodeArray_[b][i] = source_arg.childNodeArray_[b][i]->deepCopy ();
00091
00092 return (*this);
00093
00094 }
00095
00097 virtual ~BufferedBranchNode ()
00098 {
00099 }
00100
00102 virtual BufferedBranchNode*
00103 deepCopy () const
00104 {
00105 return new BufferedBranchNode (*this);
00106 }
00107
00113 inline OctreeNode*
00114 getChildPtr (unsigned char buffer_arg, unsigned char index_arg) const
00115 {
00116 assert( (buffer_arg<2) && (index_arg<8));
00117 return childNodeArray_[buffer_arg][index_arg];
00118 }
00119
00125 inline void setChildPtr (unsigned char buffer_arg, unsigned char index_arg,
00126 OctreeNode* newNode_arg)
00127 {
00128 assert( (buffer_arg<2) && (index_arg<8));
00129 childNodeArray_[buffer_arg][index_arg] = newNode_arg;
00130 }
00131
00137 inline bool hasChild (unsigned char buffer_arg, unsigned char index_arg) const
00138 {
00139 assert( (buffer_arg<2) && (index_arg<8));
00140 return (childNodeArray_[buffer_arg][index_arg] != 0);
00141 }
00142
00144 virtual node_type_t getNodeType () const
00145 {
00146 return BRANCH_NODE;
00147 }
00148
00150 inline void reset ()
00151 {
00152 memset (&childNodeArray_[0][0], 0, sizeof(OctreeNode*) * 8 * 2);
00153 ContainerT::reset ();
00154 }
00155
00156 protected:
00157 int preBuf;
00158 OctreeNode* childNodeArray_[2][8];
00159 int postBuf;
00160
00161 };
00162
00176 template<typename DataT, typename LeafT = OctreeContainerDataT<DataT>,
00177 typename BranchT = OctreeContainerEmpty<DataT> >
00178 class Octree2BufBase
00179 {
00180
00181 public:
00182
00183 typedef Octree2BufBase<DataT, LeafT, BranchT> OctreeT;
00184
00185
00186 friend class OctreeIteratorBase<DataT, OctreeT> ;
00187 friend class OctreeDepthFirstIterator<DataT, OctreeT> ;
00188 friend class OctreeBreadthFirstIterator<DataT, OctreeT> ;
00189 friend class OctreeLeafNodeIterator<DataT, OctreeT> ;
00190
00191 typedef BufferedBranchNode<BranchT> BranchNode;
00192 typedef OctreeLeafNode<LeafT> LeafNode;
00193
00194
00195 typedef OctreeDepthFirstIterator<DataT, OctreeT> Iterator;
00196 typedef const OctreeDepthFirstIterator<DataT, OctreeT> ConstIterator;
00197
00198 typedef OctreeLeafNodeIterator<DataT, OctreeT> LeafNodeIterator;
00199 typedef const OctreeLeafNodeIterator<DataT, OctreeT> ConstLeafNodeIterator;
00200
00201 typedef OctreeDepthFirstIterator<DataT, OctreeT> DepthFirstIterator;
00202 typedef const OctreeDepthFirstIterator<DataT, OctreeT> ConstDepthFirstIterator;
00203 typedef OctreeBreadthFirstIterator<DataT, OctreeT> BreadthFirstIterator;
00204 typedef const OctreeBreadthFirstIterator<DataT, OctreeT> ConstBreadthFirstIterator;
00205
00207 Octree2BufBase ();
00208
00210 virtual
00211 ~Octree2BufBase ();
00212
00214 Octree2BufBase (const Octree2BufBase& source) :
00215 leafCount_ (source.leafCount_), branchCount_ (source.branchCount_), objectCount_ (
00216 source.objectCount_), rootNode_ (
00217 new (BranchNode) (* (source.rootNode_))), depthMask_ (
00218 source.depthMask_), maxKey_ (source.maxKey_), branchNodePool_ (), leafNodePool_ (), bufferSelector_ (
00219 source.bufferSelector_), treeDirtyFlag_ (source.treeDirtyFlag_), octreeDepth_ (
00220 source.octreeDepth_)
00221 {
00222 }
00223
00225 inline Octree2BufBase&
00226 operator = (const Octree2BufBase& source)
00227 {
00228 leafCount_ = source.leafCount_;
00229 branchCount_ = source.branchCount_;
00230 objectCount_ = source.objectCount_;
00231 rootNode_ = new (BranchNode) (* (source.rootNode_));
00232 depthMask_ = source.depthMask_;
00233 maxKey_ = source.maxKey_;
00234 bufferSelector_ = source.bufferSelector_;
00235 treeDirtyFlag_ = source.treeDirtyFlag_;
00236 octreeDepth_ = source.octreeDepth_;
00237 return (*this);
00238 }
00239
00243 void
00244 setMaxVoxelIndex (unsigned int maxVoxelIndex_arg);
00245
00249 void
00250 setTreeDepth (unsigned int depth_arg);
00251
00255 inline unsigned int getTreeDepth () const
00256 {
00257 return this->octreeDepth_;
00258 }
00259
00266 void
00267 addData (unsigned int idxX_arg, unsigned int idxY_arg,
00268 unsigned int idxZ_arg, const DataT& data_arg);
00269
00277 bool
00278 getData (unsigned int idxX_arg, unsigned int idxY_arg,
00279 unsigned int idxZ_arg, DataT& data_arg) const;
00280
00287 bool
00288 existLeaf (unsigned int idxX_arg, unsigned int idxY_arg,
00289 unsigned int idxZ_arg) const;
00290
00296 void
00297 removeLeaf (unsigned int idxX_arg, unsigned int idxY_arg,
00298 unsigned int idxZ_arg);
00299
00303 inline unsigned int getLeafCount () const
00304 {
00305 return (static_cast<unsigned int> (leafCount_));
00306 }
00307
00311 inline unsigned int getBranchCount () const
00312 {
00313 return (static_cast<unsigned int> (branchCount_));
00314 }
00315
00319 void
00320 deleteTree (bool freeMemory_arg = false);
00321
00323 inline void deletePreviousBuffer ()
00324 {
00325 treeCleanUpRecursive (rootNode_);
00326 }
00327
00329 inline void deleteCurrentBuffer ()
00330 {
00331 bufferSelector_ = !bufferSelector_;
00332 treeCleanUpRecursive (rootNode_);
00333 leafCount_ = 0;
00334 }
00335
00337 void
00338 switchBuffers ();
00339
00344 void
00345 serializeTree (std::vector<char>& binaryTreeOut_arg,
00346 bool doXOREncoding_arg = false);
00347
00353 void
00354 serializeTree (std::vector<char>& binaryTreeOut_arg,
00355 std::vector<DataT>& dataVector_arg, bool doXOREncoding_arg = false);
00356
00360 void
00361 serializeLeafs (std::vector<DataT>& dataVector_arg);
00362
00367 void
00368 serializeNewLeafs (std::vector<DataT>& dataVector_arg,
00369 const int minPointsPerLeaf_arg = 0);
00370
00375 void
00376 deserializeTree (std::vector<char>& binaryTreeIn_arg,
00377 bool doXORDecoding_arg = false);
00378
00384 void
00385 deserializeTree (std::vector<char>& binaryTreeIn_arg,
00386 std::vector<DataT>& dataVector_arg, bool doXORDecoding_arg = false);
00387
00388 protected:
00389
00391
00393
00395 OctreeNode*
00396 getRootNode () const
00397 {
00398 return (this->rootNode_);
00399 }
00400
00406 virtual bool genOctreeKeyForDataT (const DataT &, OctreeKey &) const
00407 {
00408
00409 return (false);
00410 }
00411
00417 virtual bool genDataTByOctreeKey (const OctreeKey &, DataT &) const
00418 {
00419
00420 return (false);
00421 }
00422
00427 inline void addData (const OctreeKey& key_arg, const DataT& data_arg)
00428 {
00429
00430 LeafT* leaf = createLeaf (key_arg);
00431
00432
00433 if (leaf)
00434 {
00435 leaf->setData (data_arg);
00436 objectCount_++;
00437 }
00438 }
00439
00444 inline LeafT*
00445 findLeaf (const OctreeKey& key_arg) const
00446 {
00447 return findLeafRecursive (key_arg, depthMask_, rootNode_);
00448 }
00449
00455 inline LeafT*
00456 createLeaf (const OctreeKey& key_arg)
00457 {
00458 LeafT* result;
00459
00460 result = createLeafRecursive (key_arg, depthMask_, rootNode_, false);
00461
00462
00463 treeDirtyFlag_ = true;
00464
00465 return result;
00466 }
00467
00472 inline bool existLeaf (const OctreeKey& key_arg) const
00473 {
00474 return ( (key_arg <= maxKey_)
00475 && (findLeafRecursive (key_arg, depthMask_, rootNode_) != 0));
00476 }
00477
00481 inline void removeLeaf (const OctreeKey& key_arg)
00482 {
00483 if (key_arg <= maxKey_)
00484 {
00485 deleteLeafRecursive (key_arg, depthMask_, rootNode_);
00486
00487
00488 treeDirtyFlag_ = true;
00489 }
00490 }
00491
00493
00495
00501 inline bool
00502 branchHasChild (const BranchNode& branch_arg, unsigned char childIdx_arg) const
00503 {
00504
00505 return (branch_arg.getChildPtr(bufferSelector_, childIdx_arg) != 0);
00506 }
00507
00513 inline OctreeNode*
00514 getBranchChildPtr (const BranchNode& branch_arg,
00515 unsigned char childIdx_arg) const
00516 {
00517 return branch_arg.getChildPtr(bufferSelector_, childIdx_arg);
00518 }
00519
00525 inline void setBranchChildPtr (BranchNode& branch_arg,
00526 unsigned char childIdx_arg, OctreeNode* newChild_arg)
00527 {
00528 branch_arg.setChildPtr(bufferSelector_, childIdx_arg, newChild_arg);
00529 }
00530
00535 inline void getDataFromOctreeNode (const OctreeNode* node_arg,
00536 DataT& data_arg)
00537 {
00538 if (node_arg->getNodeType () == LEAF_NODE)
00539 {
00540 const LeafT* leafContainer = dynamic_cast<const LeafT*> (node_arg);
00541 leafContainer->getData (data_arg);
00542 }
00543 else
00544 {
00545 const BranchT* branchContainer =
00546 dynamic_cast<const BranchT*> (node_arg);
00547 branchContainer->getData (data_arg);
00548 }
00549 }
00550
00555 inline void getDataFromOctreeNode (const OctreeNode* node_arg,
00556 std::vector<DataT>& data_arg)
00557 {
00558 if (node_arg->getNodeType () == LEAF_NODE)
00559 {
00560 const LeafT* leafContainer = dynamic_cast<const LeafT*> (node_arg);
00561 leafContainer->getData (data_arg);
00562 }
00563 else
00564 {
00565 const BranchT* branchContainer =
00566 dynamic_cast<const BranchT*> (node_arg);
00567 branchContainer->getData (data_arg);
00568 }
00569 }
00570
00575 inline size_t getDataSizeFromOctreeNode (const OctreeNode* node_arg)
00576 {
00577 size_t nodeSize;
00578 if (node_arg->getNodeType () == LEAF_NODE)
00579 {
00580 const LeafT* leafContainer = dynamic_cast<const LeafT*> (node_arg);
00581 nodeSize = leafContainer->getSize ();
00582 }
00583 else
00584 {
00585 const BranchT* branchContainer =
00586 dynamic_cast<const BranchT*> (node_arg);
00587 nodeSize = branchContainer->getSize ();
00588 }
00589 return nodeSize;
00590 }
00591
00596 inline char getBranchBitPattern (const BranchNode& branch_arg) const
00597 {
00598 unsigned char i;
00599 char nodeBits;
00600
00601
00602 nodeBits = 0;
00603 for (i = 0; i < 8; i++)
00604 {
00605 nodeBits |= static_cast<char> ( (!!branch_arg.getChildPtr (
00606 bufferSelector_, i)) << i);
00607 }
00608
00609 return (nodeBits);
00610 }
00611
00617 inline char getBranchBitPattern (const BranchNode& branch_arg,
00618 unsigned char bufferSelector_arg) const
00619 {
00620 unsigned char i;
00621 char nodeBits;
00622
00623
00624 nodeBits = 0;
00625 for (i = 0; i < 8; i++)
00626 {
00627 nodeBits |= static_cast<char> ( (!!branch_arg.getChildPtr (
00628 bufferSelector_arg, i)) << i);
00629 }
00630
00631 return (nodeBits);
00632 }
00633
00638 inline char getBranchXORBitPattern (
00639 const BranchNode& branch_arg) const
00640 {
00641 unsigned char i;
00642 char nodeBits[2];
00643
00644
00645 nodeBits[0] = nodeBits[1] = 0;
00646
00647 for (i = 0; i < 8; i++)
00648 {
00649 nodeBits[0] |= static_cast<char> ( (!!branch_arg.getChildPtr (0, i))
00650 << i);
00651 nodeBits[1] |= static_cast<char> ( (!!branch_arg.getChildPtr (1, i))
00652 << i);
00653 }
00654
00655 return nodeBits[0] ^ nodeBits[1];
00656 }
00657
00662 inline bool hasBranchChanges (const BranchNode& branch_arg) const
00663 {
00664 return (getBranchXORBitPattern (branch_arg) > 0);
00665 }
00666
00672 inline void deleteBranchChild (BranchNode& branch_arg,
00673 unsigned char bufferSelector_arg,
00674 unsigned char childIdx_arg)
00675 {
00676 if (branch_arg.hasChild(bufferSelector_arg, childIdx_arg))
00677 {
00678 OctreeNode* branchChild = branch_arg.getChildPtr(bufferSelector_arg, childIdx_arg);
00679
00680 switch (branchChild->getNodeType ())
00681 {
00682 case BRANCH_NODE:
00683 {
00684
00685 deleteBranch (*static_cast<BranchNode*> (branchChild));
00686
00687
00688 branchNodePool_.pushNode (
00689 static_cast<BranchNode*> (branchChild));
00690 break;
00691 }
00692
00693 case LEAF_NODE:
00694 {
00695
00696 leafNodePool_.pushNode(
00697 static_cast<LeafNode*> (branchChild));
00698 break;
00699 }
00700 default:
00701 break;
00702 }
00703
00704
00705 branch_arg.setChildPtr(bufferSelector_arg, childIdx_arg, 0);
00706 }
00707 }
00708
00712 inline void deleteBranch (BranchNode& branch_arg)
00713 {
00714 char i;
00715
00716
00717 for (i = 0; i < 8; i++)
00718 {
00719
00720 if (branch_arg.getChildPtr(0, i) == branch_arg.getChildPtr(1, i))
00721 {
00722
00723 deleteBranchChild (branch_arg, 0, i);
00724
00725
00726 branch_arg.setChildPtr(0, i, 0);
00727 branch_arg.setChildPtr(1, i, 0);
00728 }
00729 else
00730 {
00731 deleteBranchChild (branch_arg, 0, i);
00732 deleteBranchChild (branch_arg, 1, i);
00733 }
00734 }
00735 }
00736
00742 inline void createBranchChild (BranchNode& branch_arg,
00743 unsigned char childIdx_arg, BranchNode*& newBranchChild_arg)
00744 {
00745
00746 newBranchChild_arg = branchNodePool_.popNode();
00747
00748 branch_arg.setChildPtr (bufferSelector_, childIdx_arg,
00749 static_cast<OctreeNode*> (newBranchChild_arg));
00750 }
00751
00757 inline void createLeafChild (BranchNode& branch_arg,
00758 unsigned char childIdx_arg, LeafNode*& newLeafChild_arg)
00759 {
00760 newLeafChild_arg = leafNodePool_.popNode();
00761
00762 branch_arg.setChildPtr(bufferSelector_, childIdx_arg, newLeafChild_arg);
00763 }
00764
00767 inline void poolCleanUp ()
00768 {
00769 branchNodePool_.deletePool();
00770 leafNodePool_.deletePool();
00771 }
00772
00774
00776
00784 LeafT*
00785 createLeafRecursive (const OctreeKey& key_arg,
00786 unsigned int depthMask_arg, BranchNode* branch_arg,
00787 bool branchReset_arg);
00788
00796 LeafT*
00797 findLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg,
00798 BranchNode* branch_arg) const;
00799
00806 bool
00807 deleteLeafRecursive (const OctreeKey& key_arg,
00808 unsigned int depthMask_arg, BranchNode* branch_arg);
00809
00819 void
00820 serializeTreeRecursive (BranchNode* branch_arg, OctreeKey& key_arg,
00821 std::vector<char>* binaryTreeOut_arg,
00822 typename std::vector<DataT>* dataVector_arg, bool doXOREncoding_arg = false,
00823 bool newLeafsFilter_arg = false, std::size_t minPointsPerLeaf_arg = 0);
00824
00835 void
00836 deserializeTreeRecursive (BranchNode* branch_arg,
00837 unsigned int depthMask_arg, OctreeKey& key_arg,
00838 typename std::vector<char>::const_iterator& binaryTreeIT_arg,
00839 typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
00840 typename std::vector<DataT>::const_iterator* dataVectorIterator_arg,
00841 typename std::vector<DataT>::const_iterator* dataVectorEndIterator_arg,
00842 bool branchReset_arg = false, bool doXORDecoding_arg = false);
00843
00844
00846
00848
00851 virtual void serializeTreeCallback (LeafNode &, const OctreeKey &)
00852 {
00853
00854 }
00855
00858 virtual void deserializeTreeCallback (LeafNode&, const OctreeKey&)
00859 {
00860
00861 }
00862
00864
00866
00870 void
00871 treeCleanUpRecursive (BranchNode* branch_arg);
00872
00877 inline double Log2 (double n_arg)
00878 {
00879 return log (n_arg) / log (2.0);
00880 }
00881
00885 inline bool octreeCanResize ()
00886 {
00887 return (false);
00888 }
00889
00893 inline void printBinary (char data_arg)
00894 {
00895 unsigned char mask = 1;
00896
00897
00898 for (int i = 0; i < 8; i++)
00899 {
00900
00901 std::cout << ((data_arg & (mask << i)) ? "1" : "0");
00902 }
00903 std::cout << std::endl;
00904 }
00905
00907
00909
00911 std::size_t leafCount_;
00912
00914 std::size_t branchCount_;
00915
00917 std::size_t objectCount_;
00918
00920 BranchNode* rootNode_;
00921
00923 unsigned int depthMask_;
00924
00926 OctreeKey maxKey_;
00927
00929 OctreeNodePool<BranchNode> branchNodePool_;
00930
00932 OctreeNodePool<LeafNode> leafNodePool_;
00933
00935 unsigned char bufferSelector_;
00936
00937
00938 bool treeDirtyFlag_;
00939
00941 unsigned int octreeDepth_;
00942 };
00943 }
00944 }
00945
00946
00947
00948 #endif
00949