Point Cloud Library (PCL)  1.3.1
octree_lowmemory_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_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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines