Point Cloud Library (PCL)  1.3.1
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-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_base.h 3017 2011-11-01 03:24:04Z rusu $
00037  */
00038 
00039 #ifndef OCTREE_TREE_BASE_H
00040 #define OCTREE_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 OctreeBase
00065       {
00066 
00067         friend class OctreeNodeIterator<DataT, LeafT, OctreeBase> ;
00068         friend class OctreeLeafNodeIterator<DataT, LeafT, OctreeBase> ;
00069 
00070       public:
00071 
00072         // Octree iterators
00073         typedef OctreeNodeIterator<DataT, LeafT, OctreeBase> Iterator;
00074         typedef const OctreeNodeIterator<DataT, LeafT, OctreeBase> ConstIterator;
00075 
00076         // Octree iterators
00077         typedef OctreeLeafNodeIterator<DataT, LeafT, OctreeBase> LeafNodeIterator;
00078         typedef const OctreeLeafNodeIterator<DataT, LeafT, OctreeBase> ConstLeafNodeIterator;
00079 
00081         OctreeBase ();
00082 
00084         virtual
00085         ~OctreeBase ();
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 () const
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 
00166         void
00167         deleteTree ( bool freeMemory_arg = false );
00168 
00172         void
00173         serializeTree (std::vector<char>& binaryTreeOut_arg);
00174 
00179         void
00180         serializeTree (std::vector<char>& binaryTreeOut_arg, std::vector<DataT>& dataVector_arg);
00181 
00185         void
00186         serializeLeafs (std::vector<DataT>& dataVector_arg);
00187 
00191         void
00192         deserializeTree (std::vector<char>& binaryTreeIn_arg);
00193 
00198         void
00199         deserializeTree (std::vector<char>& binaryTreeIn_arg, std::vector<DataT>& dataVector_arg);
00200 
00205         void
00206         deserializeTreeAndOutputLeafData (std::vector<char>& binaryTreeIn_arg, std::vector<DataT>& dataVector_arg);
00207 
00208       protected:
00209 
00211 
00215 
00216         class OctreeKey
00217         {
00218         public:
00219 
00223           bool
00224           operator == (const OctreeKey& b) const
00225           {
00226             return ((b.x == this->x) && (b.y == this->y) && (b.z == this->z));
00227           }
00228 
00229           // Indices addressing a voxel at (X, Y, Z)
00230           unsigned int x;unsigned int y;unsigned int z;
00231         };
00232 
00234 
00238 
00239         class OctreeBranch : public OctreeNode
00240         {
00241           // OctreeBase is a friend!
00242           friend class OctreeBase;
00243 
00244         public:
00245 
00247           OctreeBranch ()
00248           {
00249             memset (this->subNodes_, 0, sizeof(this->subNodes_));
00250           }
00251 
00253           virtual
00254           ~OctreeBranch ()
00255           {
00256           }
00257 
00261           virtual node_type_t
00262           getNodeType () const
00263           {
00264             return BRANCH_NODE;
00265           }
00266 
00267         private:
00268 
00270           const OctreeNode * subNodes_[8];
00271 
00272         };
00273 
00274         typedef LeafT OctreeLeaf;
00275 
00277         // Protected octree methods based on octree keys
00279 
00280 
00282         const OctreeNode*
00283         getRootNode () const
00284         {
00285           return this->rootNode_;
00286         }
00287 
00293         virtual bool
00294         genOctreeKeyForDataT (const DataT& data_arg, OctreeKey & key_arg) const
00295         {
00296           // this class cannot relate DataT objects to octree keys
00297           return false;
00298         }
00299 
00305         virtual bool
00306         genDataTByOctreeKey (const OctreeKey & key_arg, DataT& data_arg) const
00307         {
00308           // this class cannot relate DataT objects to octree keys
00309           return false;
00310         }
00311 
00318         inline void
00319         genOctreeKeyByIntIdx (const unsigned int idxX_arg, const unsigned int idxY_arg, const unsigned int idxZ_arg,
00320                               OctreeKey & key_arg) const
00321         {
00322           // copy data to octree key class
00323           key_arg.x = idxX_arg;
00324           key_arg.y = idxY_arg;
00325           key_arg.z = idxZ_arg;
00326         }
00327 
00332         inline void
00333         add (const OctreeKey& key_arg, const DataT& data_arg)
00334         {
00335           // request a (new) leaf from tree
00336           LeafT* leaf = getLeaf (key_arg);
00337 
00338           // assign data to leaf
00339           if (leaf)
00340           {
00341             leaf->setData (data_arg);
00342             objectCount_++;
00343           }
00344         }
00345 
00350         void
00351         add (const std::vector<OctreeKey>& key_vector_arg, const std::vector<DataT>& data_vector_arg);
00352 
00357         inline LeafT*
00358         findLeaf (const OctreeKey& key_arg) const
00359         {
00360           return findLeafRecursive (key_arg, depthMask_, rootNode_);
00361         }
00362 
00368         inline LeafT*
00369         getLeaf (const OctreeKey& key_arg)
00370         {
00371           return getLeafRecursive (key_arg, depthMask_, rootNode_);
00372         }
00373 
00378         inline bool
00379         existLeaf (const OctreeKey& key_arg) const
00380         {
00381           return (findLeafRecursive (key_arg, depthMask_, rootNode_) != 0);
00382         }
00383 
00387         inline void
00388         removeLeaf (const OctreeKey& key_arg)
00389         {
00390           deleteLeafRecursive (key_arg, depthMask_, rootNode_);
00391         }
00392 
00394         // Branch node accessor inline functions
00396 
00402         inline const OctreeNode*
00403         getBranchChild (const OctreeBranch& branch_arg, const unsigned char childIdx_arg) const
00404         {
00405           return branch_arg.subNodes_[childIdx_arg];
00406         }
00407 
00413         inline bool
00414         branchHasChild (const OctreeBranch& branch_arg, const unsigned char childIdx_arg) const
00415         {
00416           return (branch_arg.subNodes_[childIdx_arg] != 0);
00417         }
00418 
00423         inline char
00424         getBranchBitPattern (const OctreeBranch& branch_arg) const
00425         {
00426           unsigned char i;
00427           char nodeBits;
00428 
00429           // create bit pattern
00430           nodeBits = 0;
00431           for (i = 0; i < 8; i++)
00432           {
00433             nodeBits |= (!!branch_arg.subNodes_[i]) << i;
00434           }
00435 
00436           return nodeBits;
00437         }
00438 
00444         inline void
00445         setBranchChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg, const OctreeNode * newChild_arg)
00446         {
00447           branch_arg.subNodes_[childIdx_arg] = newChild_arg;
00448         }
00449 
00454         inline void
00455         deleteBranchChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg)
00456         {
00457           if (branchHasChild (branch_arg, childIdx_arg))
00458           {
00459             const OctreeNode* branchChild;
00460             branchChild = getBranchChild (branch_arg, childIdx_arg);
00461 
00462             switch (branchChild->getNodeType ())
00463             {
00464               case BRANCH_NODE:
00465                 // free child branch recursively
00466                 deleteBranch (*(OctreeBranch*)branchChild);
00467 
00468                 // push unused branch to branch pool
00469                 unusedBranchesPool_.push_back ((OctreeBranch*)branchChild);
00470                 break;
00471 
00472               case LEAF_NODE:
00473 
00474                 // push unused leaf to branch pool
00475                 unusedLeafsPool_.push_back ((OctreeLeaf*)branchChild);
00476                 break;
00477             }
00478 
00479             // set branch child pointer to 0
00480             setBranchChild (branch_arg, childIdx_arg, 0);
00481           }
00482         }
00483 
00487         inline void
00488         deleteBranch (OctreeBranch& branch_arg)
00489         {
00490           char i;
00491 
00492           // delete all branch node children
00493           for (i = 0; i < 8; i++)
00494           {
00495             deleteBranchChild (branch_arg, i);
00496           }
00497         }
00498 
00502         inline void
00503         createBranch (OctreeBranch*& newBranchChild_arg)
00504         {
00505           if (!unusedBranchesPool_.size ())
00506           {
00507             // branch pool is empty
00508             // we need to create a new octree branch class
00509             newBranchChild_arg = (OctreeBranch*)new OctreeBranch ();
00510           }
00511           else
00512           {
00513             // reuse branch from branch pool
00514             newBranchChild_arg = unusedBranchesPool_.back ();
00515             unusedBranchesPool_.pop_back ();
00516             branchReset (*newBranchChild_arg);
00517           }
00518         }
00519 
00525         inline void
00526         createBranchChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg,
00527                            OctreeBranch*& newBranchChild_arg)
00528         {
00529           createBranch ( newBranchChild_arg );
00530           setBranchChild (branch_arg, childIdx_arg, (OctreeNode*)newBranchChild_arg);
00531         }
00532 
00533 
00539         inline void
00540         createLeafChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg, OctreeLeaf*& newLeafChild_arg)
00541         {
00542 
00543           if (!unusedLeafsPool_.size ())
00544           {
00545             // leaf pool is empty
00546             // we need to create a new octree leaf class
00547             newLeafChild_arg = (OctreeLeaf*)new OctreeLeaf ();
00548           }
00549           else
00550           {
00551             // reuse leaf node from branch pool
00552             newLeafChild_arg = unusedLeafsPool_.back ();
00553             unusedLeafsPool_.pop_back ();
00554           }
00555 
00556           newLeafChild_arg->reset ();
00557 
00558           setBranchChild (branch_arg, childIdx_arg, (OctreeNode*)newLeafChild_arg);
00559 
00560         }
00561 
00565         inline void
00566         branchReset (OctreeBranch& branch_arg)
00567         {
00568           memset (branch_arg.subNodes_, 0, sizeof(branch_arg.subNodes_));
00569         }
00570 
00573         inline void
00574         poolCleanUp ()
00575         {
00576           // delete all branch instances from branch pool
00577           while (!unusedBranchesPool_.empty ())
00578           {
00579             delete (unusedBranchesPool_.back ());
00580             unusedBranchesPool_.pop_back ();
00581           }
00582 
00583           // delete all leaf instances from leaf pool
00584           while (!unusedLeafsPool_.empty ())
00585           {
00586             delete (unusedLeafsPool_.back ());
00587             unusedLeafsPool_.pop_back ();
00588           }
00589         }
00590 
00592         // Recursive octree methods
00594 
00595 
00602         LeafT*
00603         getLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg, OctreeBranch* branch_arg);
00604 
00612         LeafT*
00613         findLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg, OctreeBranch* branch_arg) const;
00614 
00621         bool
00622         deleteLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg, OctreeBranch* branch_arg);
00623 
00629         void
00630         serializeTreeRecursive (std::vector<char>& binaryTreeOut_arg, const OctreeBranch* branch_arg, const OctreeKey& key_arg);
00631 
00638         void
00639         serializeTreeRecursive (std::vector<char>& binaryTreeOut_arg, const OctreeBranch* branch_arg,
00640                                 const OctreeKey& key_arg, typename std::vector<DataT>& dataVector_arg);
00641 
00647         void
00648         serializeLeafsRecursive (const OctreeBranch* branch_arg, const OctreeKey& key_arg,
00649                                  typename std::vector<DataT>& dataVector_arg);
00650 
00657         void
00658         deserializeTreeRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg,
00659                                   OctreeBranch* branch_arg, const unsigned int depthMask_arg, const OctreeKey& key_arg);
00660 
00669         void
00670         deserializeTreeRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg,
00671                                   OctreeBranch* branch_arg, const unsigned int depthMask_arg, const OctreeKey& key_arg,
00672                                   typename std::vector<DataT>::const_iterator& dataVectorIterator_arg,
00673                                   typename std::vector<DataT>::const_iterator& dataVectorEndIterator_arg);
00674 
00682         void
00683         deserializeTreeAndOutputLeafDataRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg,
00684                                                    OctreeBranch* branch_arg, const unsigned int depthMask_arg,
00685                                                    const OctreeKey& key_arg,
00686                                                    typename std::vector<DataT>& dataVector_arg);
00687 
00689         // Serialization callbacks
00691 
00696         virtual void
00697         serializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg);
00698 
00704         virtual void
00705         serializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg, std::vector<DataT>& dataVector_arg);
00706 
00711         virtual void
00712         deserializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg);
00713 
00720         virtual void
00721         deserializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg,
00722                                  typename std::vector<DataT>::const_iterator& dataVectorIterator_arg,
00723                                  typename std::vector<DataT>::const_iterator& dataVectorEndIterator_arg);
00724 
00730         virtual void
00731         deserializeTreeAndSerializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey & key_arg,
00732                                                  std::vector<DataT>& dataVector_arg);
00733 
00735         // Helpers
00737 
00742         inline double
00743         Log2 (double n_arg)
00744         {
00745            return log( n_arg ) / log( 2.0 );
00746         }
00747 
00751         inline bool
00752         octreeCanResize ()
00753         {
00754           return (true);
00755         }
00756 
00758         // Globals
00760 
00762         unsigned int leafCount_;
00763 
00765         unsigned int branchCount_;
00766 
00768         unsigned int objectCount_;
00769 
00771         OctreeBranch* rootNode_;
00772 
00774         unsigned int depthMask_;
00775 
00777         unsigned int octreeDepth_;
00778 
00780         std::vector<OctreeBranch*> unusedBranchesPool_;
00781 
00783         std::vector<LeafT*> unusedLeafsPool_;
00784 
00785       };
00786   }
00787 }
00788 
00789 //#include "impl/octree_base.hpp"
00790 
00791 #endif
00792 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines