octree_base.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2012, 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 6201 2012-07-06 11:53:17Z jkammerl $
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         // iterators are friends
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         // Octree iterators
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         // Protected octree methods based on octree keys
00261 
00267         virtual bool
00268         genOctreeKeyForDataT (const DataT &, OctreeKey &) const
00269         {
00270           // this class cannot relate DataT objects to octree keys
00271           return (false);
00272         }
00273 
00279         virtual bool
00280         genDataTByOctreeKey (const OctreeKey &, DataT &) const
00281         {
00282           // this class cannot relate DataT objects to octree keys
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         // Branch node accessor inline functions
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           // test occupancyByte for child existence
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           // create bit pattern
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                 // free child branch recursively
00473                 deleteBranch (*static_cast<BranchNode*> (branchChild));
00474                 // push unused branch to branch pool
00475                 branchNodePool_.pushNode (
00476                     static_cast<BranchNode*> (branchChild));
00477               }
00478                 break;
00479 
00480               case LEAF_NODE:
00481               {
00482                 // push unused leaf to branch pool
00483                 leafNodePool_.pushNode (static_cast<LeafNode*> (branchChild));
00484                 break;
00485               }
00486               default:
00487                 break;
00488             }
00489 
00490             // set branch child pointer to 0
00491             branch_arg[childIdx_arg] = 0;
00492           }
00493         }
00494 
00498         inline void
00499         deleteBranch (BranchNode& branch_arg)
00500         {
00501           char i;
00502 
00503           // delete all branch node children
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         // Recursive octree methods
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         // Serialization callbacks
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         // Helpers
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         // Globals
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 //#include "impl/octree_base.hpp"
00695 
00696 #endif
00697 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:56