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_BASE_H
00040 #define OCTREE_TREE_BASE_H
00041
00042 #include <cstddef>
00043 #include <vector>
00044
00045 #include "octree_nodes.h"
00046 #include "octree_container.h"
00047 #include "octree_key.h"
00048 #include "octree_iterator.h"
00049 #include "octree_node_pool.h"
00050
00051 namespace pcl
00052 {
00053 namespace octree
00054 {
00062 template<typename DataT, typename LeafT = OctreeContainerDataT<DataT>,
00063 typename BranchT = OctreeContainerEmpty<DataT> >
00064 class OctreeBase
00065 {
00066
00067 public:
00068
00069 typedef OctreeBase<DataT, OctreeContainerDataT<DataT>, OctreeContainerEmpty<DataT> > SingleObjLeafContainer;
00070 typedef OctreeBase<DataT, OctreeContainerDataTVector<DataT>, OctreeContainerEmpty<DataT> > MultipleObjsLeafContainer;
00071
00072 typedef OctreeBase<DataT, LeafT, BranchT> OctreeT;
00073
00074
00075 friend class OctreeIteratorBase<DataT, OctreeT> ;
00076 friend class OctreeDepthFirstIterator<DataT, OctreeT> ;
00077 friend class OctreeBreadthFirstIterator<DataT, OctreeT> ;
00078 friend class OctreeLeafNodeIterator<DataT, OctreeT> ;
00079
00080 typedef OctreeBranchNode<BranchT> BranchNode;
00081 typedef OctreeLeafNode<LeafT> LeafNode;
00082
00083
00084 typedef OctreeDepthFirstIterator<DataT, OctreeT> Iterator;
00085 typedef const OctreeDepthFirstIterator<DataT, OctreeT> ConstIterator;
00086
00087 typedef OctreeLeafNodeIterator<DataT, OctreeT> LeafNodeIterator;
00088 typedef const OctreeLeafNodeIterator<DataT, OctreeT> ConstLeafNodeIterator;
00089
00090 typedef OctreeDepthFirstIterator<DataT, OctreeT> DepthFirstIterator;
00091 typedef const OctreeDepthFirstIterator<DataT, OctreeT> ConstDepthFirstIterator;
00092 typedef OctreeBreadthFirstIterator<DataT, OctreeT> BreadthFirstIterator;
00093 typedef const OctreeBreadthFirstIterator<DataT, OctreeT> ConstBreadthFirstIterator;
00094
00095
00097 OctreeBase ();
00098
00100 virtual
00101 ~OctreeBase ();
00102
00104 OctreeBase (const OctreeBase& source) :
00105 leafCount_ (source.leafCount_),
00106 branchCount_ (source.branchCount_),
00107 objectCount_ (source.objectCount_),
00108 rootNode_ (new (BranchNode) (*(source.rootNode_))),
00109 depthMask_ (source.depthMask_),
00110 octreeDepth_ (source.octreeDepth_),
00111 maxKey_ (source.maxKey_),
00112 branchNodePool_ (),
00113 leafNodePool_ ()
00114 {
00115 }
00116
00118 inline OctreeBase&
00119 operator = (const OctreeBase &source)
00120 {
00121 leafCount_ = source.leafCount_;
00122 branchCount_ = source.branchCount_;
00123 objectCount_ = source.objectCount_;
00124 rootNode_ = new (BranchNode) (*(source.rootNode_));
00125 depthMask_ = source.depthMask_;
00126 maxKey_ = source.maxKey_;
00127 octreeDepth_ = source.octreeDepth_;
00128 return (*this);
00129 }
00130
00134 void
00135 setMaxVoxelIndex (unsigned int maxVoxelIndex_arg);
00136
00140 void
00141 setTreeDepth (unsigned int depth_arg);
00142
00146 inline unsigned int
00147 getTreeDepth () const
00148 {
00149 return this->octreeDepth_;
00150 }
00151
00156 inline void
00157 enableDynamicDepth ( size_t maxObjsPerLeaf )
00158 {
00159 assert(leafCount_==0);
00160 maxObjsPerLeaf_ = maxObjsPerLeaf;
00161 }
00162
00169 void
00170 addData (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg,
00171 const DataT& data_arg);
00172
00180 bool
00181 getData (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg, DataT& data_arg) const ;
00182
00189 bool
00190 existLeaf (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg) const ;
00191
00197 void
00198 removeLeaf (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg);
00199
00203 inline std::size_t
00204 getLeafCount () const
00205 {
00206 return leafCount_;
00207 }
00208
00212 inline std::size_t
00213 getBranchCount () const
00214 {
00215 return branchCount_;
00216 }
00217
00221 void
00222 deleteTree ( bool freeMemory_arg = true );
00223
00227 void
00228 serializeTree (std::vector<char>& binaryTreeOut_arg);
00229
00234 void
00235 serializeTree (std::vector<char>& binaryTreeOut_arg, std::vector<DataT>& dataVector_arg);
00236
00240 void
00241 serializeLeafs (std::vector<DataT>& dataVector_arg);
00242
00246 void
00247 deserializeTree (std::vector<char>& binaryTreeIn_arg);
00248
00253 void
00254 deserializeTree (std::vector<char>& binaryTreeIn_arg, std::vector<DataT>& dataVector_arg);
00255
00256 protected:
00257
00259
00261
00267 virtual bool
00268 genOctreeKeyForDataT (const DataT &, OctreeKey &) const
00269 {
00270
00271 return (false);
00272 }
00273
00279 virtual bool
00280 genDataTByOctreeKey (const OctreeKey &, DataT &) const
00281 {
00282
00283 return (false);
00284 }
00285
00290 inline void
00291 addData (const OctreeKey& key_arg, const DataT& data_arg)
00292 {
00293 addDataToLeafRecursive (key_arg, depthMask_,data_arg, rootNode_);
00294 }
00295
00300 inline LeafNode*
00301 findLeaf (const OctreeKey& key_arg) const
00302 {
00303 LeafNode* result = 0;
00304 findLeafRecursive (key_arg, depthMask_, rootNode_, result);
00305 return result;
00306 }
00307
00312 inline bool
00313 existLeaf (const OctreeKey& key_arg) const
00314 {
00315 LeafNode* leafNode = 0;
00316 if (key_arg <= maxKey_)
00317 findLeafRecursive (key_arg, depthMask_, rootNode_, leafNode);
00318 return (leafNode != 0);
00319 }
00320
00324 inline void
00325 removeLeaf (const OctreeKey& key_arg)
00326 {
00327 if (key_arg <= maxKey_)
00328 deleteLeafRecursive (key_arg, depthMask_, rootNode_);
00329 }
00330
00332
00334
00336 OctreeNode*
00337 getRootNode () const
00338 {
00339 return this->rootNode_;
00340 }
00341
00347 inline bool
00348 branchHasChild (const BranchNode& branch_arg, unsigned char childIdx_arg) const
00349 {
00350
00351 return (branch_arg.getChildPtr(childIdx_arg) != 0);
00352 }
00353
00359 inline OctreeNode*
00360 getBranchChildPtr (const BranchNode& branch_arg,
00361 unsigned char childIdx_arg) const
00362 {
00363 return branch_arg.getChildPtr(childIdx_arg);
00364 }
00365
00371 inline void setBranchChildPtr (BranchNode& branch_arg,
00372 unsigned char childIdx_arg, OctreeNode* newChild_arg)
00373 {
00374 branch_arg[childIdx_arg] = newChild_arg;
00375 }
00376
00381 inline void getDataFromOctreeNode (const OctreeNode* node_arg,
00382 DataT& data_arg)
00383 {
00384 if (node_arg->getNodeType () == LEAF_NODE)
00385 {
00386 const LeafT* leafContainer = dynamic_cast<const LeafT*> (node_arg);
00387 leafContainer->getData (data_arg);
00388 }
00389 else
00390 {
00391 const BranchT* branchContainer =
00392 dynamic_cast<const BranchT*> (node_arg);
00393 branchContainer->getData (data_arg);
00394 }
00395 }
00396
00401 inline void getDataFromOctreeNode (const OctreeNode* node_arg,
00402 std::vector<DataT>& data_arg)
00403 {
00404 if (node_arg->getNodeType () == LEAF_NODE)
00405 {
00406 const LeafT* leafContainer = dynamic_cast<const LeafT*> (node_arg);
00407 leafContainer->getData (data_arg);
00408 }
00409 else
00410 {
00411 const BranchT* branchContainer =
00412 dynamic_cast<const BranchT*> (node_arg);
00413 branchContainer->getData (data_arg);
00414 }
00415 }
00416
00421 inline size_t getDataSizeFromOctreeNode (const OctreeNode* node_arg)
00422 {
00423 size_t nodeSize;
00424 if (node_arg->getNodeType () == LEAF_NODE)
00425 {
00426 const LeafT* leafContainer = dynamic_cast<const LeafT*> (node_arg);
00427 nodeSize = leafContainer->getSize ();
00428 }
00429 else
00430 {
00431 const BranchT* branchContainer =
00432 dynamic_cast<const BranchT*> (node_arg);
00433 nodeSize = branchContainer->getSize ();
00434 }
00435 return nodeSize;
00436 }
00441 inline char
00442 getBranchBitPattern (const BranchNode& branch_arg) const
00443 {
00444 unsigned char i;
00445 char nodeBits;
00446
00447
00448 nodeBits = 0;
00449 for (i = 0; i < 8; i++) {
00450 const OctreeNode* child = branch_arg.getChildPtr(i);
00451 nodeBits |= static_cast<char> ((!!child) << i);
00452 }
00453
00454 return (nodeBits);
00455 }
00456
00461 inline void
00462 deleteBranchChild (BranchNode& branch_arg, unsigned char childIdx_arg)
00463 {
00464 if (branch_arg.hasChild(childIdx_arg))
00465 {
00466 OctreeNode* branchChild = branch_arg[childIdx_arg];
00467
00468 switch (branchChild->getNodeType ())
00469 {
00470 case BRANCH_NODE:
00471 {
00472
00473 deleteBranch (*static_cast<BranchNode*> (branchChild));
00474
00475 branchNodePool_.pushNode (
00476 static_cast<BranchNode*> (branchChild));
00477 }
00478 break;
00479
00480 case LEAF_NODE:
00481 {
00482
00483 leafNodePool_.pushNode (static_cast<LeafNode*> (branchChild));
00484 break;
00485 }
00486 default:
00487 break;
00488 }
00489
00490
00491 branch_arg[childIdx_arg] = 0;
00492 }
00493 }
00494
00498 inline void
00499 deleteBranch (BranchNode& branch_arg)
00500 {
00501 char i;
00502
00503
00504 for (i = 0; i < 8; i++)
00505 deleteBranchChild (branch_arg, i);
00506 }
00507
00513 inline void createBranchChild (BranchNode& branch_arg,
00514 unsigned char childIdx_arg, BranchNode*& newBranchChild_arg)
00515 {
00516 newBranchChild_arg = branchNodePool_.popNode ();
00517 branch_arg[childIdx_arg] =
00518 static_cast<OctreeNode*> (newBranchChild_arg);
00519 }
00520
00526 inline void
00527 createLeafChild (BranchNode& branch_arg, unsigned char childIdx_arg, LeafNode*& newLeafChild_arg)
00528 {
00529 newLeafChild_arg = leafNodePool_.popNode();
00530
00531 branch_arg[childIdx_arg] = static_cast<OctreeNode*> (newLeafChild_arg);
00532 }
00533
00537 inline void
00538 branchReset (BranchNode& branch_arg)
00539 {
00540 branch_arg.reset ();
00541 }
00542
00545 inline void
00546 poolCleanUp ()
00547 {
00548 branchNodePool_.deletePool();
00549 leafNodePool_.deletePool();
00550 }
00551
00553
00555
00562 void
00563 addDataToLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg, const DataT& data_arg, BranchNode* branch_arg);
00564
00572 void
00573 findLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg, BranchNode* branch_arg, LeafNode*& result_arg) const;
00574
00581 bool
00582 deleteLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg, BranchNode* branch_arg);
00583
00590 void
00591 serializeTreeRecursive (const BranchNode* branch_arg, OctreeKey& key_arg,
00592 std::vector<char>* binaryTreeOut_arg,
00593 typename std::vector<DataT>* dataVector_arg) const;
00594
00604 void
00605 deserializeTreeRecursive (BranchNode* branch_arg,
00606 unsigned int depthMask_arg, OctreeKey& key_arg,
00607 typename std::vector<char>::const_iterator& binaryTreeIT_arg,
00608 typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
00609 typename std::vector<DataT>::const_iterator* dataVectorIterator_arg,
00610 typename std::vector<DataT>::const_iterator* dataVectorEndIterator_arg);
00611
00612
00614
00616
00619 virtual void serializeTreeCallback (const LeafNode &,
00620 const OctreeKey &) const
00621 {
00622
00623 }
00624
00627 virtual void deserializeTreeCallback (LeafNode&, const OctreeKey&)
00628 {
00629
00630 }
00631
00633
00635
00640 inline double
00641 Log2 (double n_arg)
00642 {
00643 return log( n_arg ) / log( 2.0 );
00644 }
00645
00649 inline bool
00650 octreeCanResize ()
00651 {
00652 return (true);
00653 }
00654
00656
00658
00660 std::size_t leafCount_;
00661
00663 std::size_t branchCount_;
00664
00666 std::size_t objectCount_;
00667
00669 BranchNode* rootNode_;
00670
00674 std::size_t maxObjsPerLeaf_;
00675
00677 unsigned int depthMask_;
00678
00680 unsigned int octreeDepth_;
00681
00683 OctreeKey maxKey_;
00684
00686 OctreeNodePool<BranchNode> branchNodePool_;
00687
00689 OctreeNodePool<LeafNode> leafNodePool_;
00690 };
00691 }
00692 }
00693
00694
00695
00696 #endif
00697