octree2buf_base.hpp
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: octree2buf_base.hpp 6119 2012-07-03 18:50:04Z aichim $
00037  */
00038 
00039 #ifndef OCTREE_2BUF_BASE_HPP
00040 #define OCTREE_2BUF_BASE_HPP
00041 
00042 namespace pcl
00043 {
00044   namespace octree
00045   {
00047     template<typename DataT, typename LeafT, typename BranchT>
00048     Octree2BufBase<DataT, LeafT, BranchT>::Octree2BufBase () :
00049       leafCount_ (0), 
00050       branchCount_ (1),
00051       objectCount_ (0), 
00052       rootNode_ (new BranchNode ()),
00053       depthMask_ (0), 
00054       maxKey_ (),
00055       branchNodePool_ (),
00056       leafNodePool_ (),
00057       bufferSelector_ (0),
00058       treeDirtyFlag_ (false),
00059       octreeDepth_ (0)
00060     {
00061     }
00062 
00064     template<typename DataT, typename LeafT, typename BranchT>
00065     Octree2BufBase<DataT, LeafT, BranchT>::~Octree2BufBase ()
00066     {
00067       // deallocate tree structure
00068       deleteTree ();
00069       delete (rootNode_);
00070       poolCleanUp ();
00071     }
00072 
00074     template<typename DataT, typename LeafT, typename BranchT> void
00075     Octree2BufBase<DataT, LeafT, BranchT>::setMaxVoxelIndex (unsigned int maxVoxelIndex_arg)
00076     {
00077       unsigned int treeDepth;
00078 
00079       assert (maxVoxelIndex_arg > 0);
00080 
00081       // tree depth == amount of bits of maxVoxels
00082       treeDepth = std::max ((std::min (static_cast<unsigned int> (OCT_MAXTREEDEPTH), 
00083                                        static_cast<unsigned int> (std::ceil (Log2 (maxVoxelIndex_arg))))),
00084                                        static_cast<unsigned int> (0));
00085 
00086       // define depthMask_ by setting a single bit to 1 at bit position == tree depth
00087       depthMask_ = (1 << (treeDepth - 1));
00088     }
00089 
00091     template<typename DataT, typename LeafT, typename BranchT> void
00092     Octree2BufBase<DataT, LeafT, BranchT>::setTreeDepth (unsigned int depth_arg)
00093     {
00094       assert (depth_arg > 0);
00095 
00096       // set octree depth
00097       octreeDepth_ = depth_arg;
00098 
00099       // define depthMask_ by setting a single bit to 1 at bit position == tree depth
00100       depthMask_ = (1 << (depth_arg - 1));
00101 
00102       // define max. keys
00103       maxKey_.x = maxKey_.y = maxKey_.z = (1 << depth_arg) - 1;
00104     }
00105 
00107     template<typename DataT, typename LeafT, typename BranchT> void
00108     Octree2BufBase<DataT, LeafT, BranchT>::addData (unsigned int idxX_arg, unsigned int idxY_arg,
00109                                        unsigned int idxZ_arg, const DataT& data_arg)
00110     {
00111       // generate key
00112       OctreeKey key (idxX_arg, idxY_arg, idxZ_arg);
00113 
00114       // add data_arg to octree
00115       addData (key, data_arg);
00116     }
00117 
00119     template<typename DataT, typename LeafT, typename BranchT> bool
00120     Octree2BufBase<DataT, LeafT, BranchT>::getData (unsigned int idxX_arg, unsigned int idxY_arg,
00121                                        unsigned int idxZ_arg, DataT& data_arg) const
00122     {
00123       // generate key
00124       OctreeKey key (idxX_arg, idxY_arg, idxZ_arg);
00125 
00126       // search for leaf at key
00127       LeafT* leaf = findLeaf (key);
00128       if (leaf)
00129       {
00130         // if successful, decode data to data_arg
00131         leaf->getData (data_arg);
00132       }
00133       // returns true on success
00134       return (leaf != 0);
00135     }
00136 
00138     template<typename DataT, typename LeafT, typename BranchT> bool
00139     Octree2BufBase<DataT, LeafT, BranchT>::existLeaf (unsigned int idxX_arg, unsigned int idxY_arg,
00140                                              unsigned int idxZ_arg) const
00141     {
00142       // generate key
00143       OctreeKey key (idxX_arg, idxY_arg, idxZ_arg);
00144 
00145       // check if key exist in octree
00146       return existLeaf (key);
00147     }
00148 
00150     template<typename DataT, typename LeafT, typename BranchT> void
00151     Octree2BufBase<DataT, LeafT, BranchT>::removeLeaf (unsigned int idxX_arg, unsigned int idxY_arg,
00152                                               unsigned int idxZ_arg)
00153     {
00154       // generate key
00155       OctreeKey key (idxX_arg, idxY_arg, idxZ_arg);
00156 
00157       // free voxel at key
00158       return (this->removeLeaf (key));
00159     }
00160 
00162     template<typename DataT, typename LeafT, typename BranchT> void
00163     Octree2BufBase<DataT, LeafT, BranchT>::deleteTree ( bool freeMemory_arg )
00164     {
00165       if (rootNode_)
00166       {
00167         // reset octree
00168         deleteBranch (*rootNode_);
00169         leafCount_ = 0;
00170         branchCount_ = 1;
00171         objectCount_ = 0;
00172         
00173         treeDirtyFlag_ = false;
00174         depthMask_ = 0;
00175         octreeDepth_ = 0;
00176       }
00177 
00178       // delete node pool
00179       if (freeMemory_arg)
00180         poolCleanUp ();
00181     }
00182 
00184     template<typename DataT, typename LeafT, typename BranchT> void
00185     Octree2BufBase<DataT, LeafT, BranchT>::switchBuffers ()
00186     {
00187       if (treeDirtyFlag_)
00188       {
00189         // make sure that all unused branch nodes from previous buffer are deleted
00190         treeCleanUpRecursive (rootNode_);
00191       }
00192 
00193       // switch butter selector
00194       bufferSelector_ = !bufferSelector_;
00195 
00196       // reset flags
00197       treeDirtyFlag_ = true;
00198       leafCount_ = 0;
00199       objectCount_ = 0;
00200       branchCount_ = 1;
00201 
00202       unsigned char childIdx;
00203       // we can safely remove children references of root node
00204       for (childIdx = 0; childIdx < 8; childIdx++)
00205       {
00206         rootNode_->setChildPtr(bufferSelector_, childIdx, 0);
00207       }
00208     }
00209 
00211     template<typename DataT, typename LeafT, typename BranchT> void
00212     Octree2BufBase<DataT, LeafT, BranchT>::serializeTree (std::vector<char>& binaryTreeOut_arg, bool doXOREncoding_arg)
00213     {
00214       OctreeKey newKey;
00215       
00216       // clear binary vector
00217       binaryTreeOut_arg.clear ();
00218       binaryTreeOut_arg.reserve (this->branchCount_);
00219 
00220       serializeTreeRecursive (rootNode_, newKey, &binaryTreeOut_arg, 0,
00221           doXOREncoding_arg, false, 0);
00222 
00223       // serializeTreeRecursive cleans-up unused octree nodes in previous octree
00224       treeDirtyFlag_ = false;
00225     }
00226 
00228     template<typename DataT, typename LeafT, typename BranchT> void
00229     Octree2BufBase<DataT, LeafT, BranchT>::serializeTree (std::vector<char>& binaryTreeOut_arg,
00230                                                  std::vector<DataT>& dataVector_arg, bool doXOREncoding_arg)
00231     {
00232       OctreeKey newKey;
00233 
00234       // clear output vectors
00235       binaryTreeOut_arg.clear ();
00236       dataVector_arg.clear ();
00237 
00238       dataVector_arg.reserve (objectCount_);
00239       binaryTreeOut_arg.reserve (this->branchCount_);
00240 
00241       serializeTreeRecursive (rootNode_, newKey, &binaryTreeOut_arg, &dataVector_arg, doXOREncoding_arg, false, 0);
00242 
00243       // serializeTreeRecursive cleans-up unused octree nodes in previous octree
00244       treeDirtyFlag_ = false;
00245     }
00246 
00248     template<typename DataT, typename LeafT, typename BranchT> void
00249     Octree2BufBase<DataT, LeafT, BranchT>::serializeLeafs (std::vector<DataT>& dataVector_arg)
00250     {
00251       OctreeKey newKey;
00252 
00253       // clear output vector
00254       dataVector_arg.clear ();
00255 
00256       dataVector_arg.reserve (objectCount_);
00257 
00258       serializeTreeRecursive (rootNode_, newKey, 0, &dataVector_arg, false, false, 0);
00259 
00260       // serializeLeafsRecursive cleans-up unused octree nodes in previous octree
00261       treeDirtyFlag_ = false;
00262     }
00263 
00265     template<typename DataT, typename LeafT, typename BranchT> void
00266     Octree2BufBase<DataT, LeafT, BranchT>::deserializeTree (std::vector<char>& binaryTreeIn_arg, bool doXORDecoding_arg)
00267     {
00268       OctreeKey newKey;
00269 
00270       // we will rebuild an octree -> reset leafCount
00271       leafCount_ = 0;
00272 
00273       // iterator for binary tree structure vector
00274       std::vector<char>::const_iterator binaryTreeVectorIterator =
00275           binaryTreeIn_arg.begin ();
00276       std::vector<char>::const_iterator binaryTreeVectorIteratorEnd =
00277           binaryTreeIn_arg.end ();
00278 
00279       deserializeTreeRecursive (rootNode_, depthMask_, newKey,
00280           binaryTreeVectorIterator, binaryTreeVectorIteratorEnd, 0, 0, false,
00281           doXORDecoding_arg);
00282 
00283       // we modified the octree structure -> clean-up/tree-reset might be required
00284       treeDirtyFlag_ = false;
00285     }
00286 
00288     template<typename DataT, typename LeafT, typename BranchT> void
00289     Octree2BufBase<DataT, LeafT, BranchT>::deserializeTree (std::vector<char>& binaryTreeIn_arg,
00290                                                    std::vector<DataT>& dataVector_arg, bool doXORDecoding_arg)
00291     {
00292       OctreeKey newKey;
00293 
00294       // set data iterator to first element
00295       typename std::vector<DataT>::const_iterator dataVectorIterator = dataVector_arg.begin ();
00296 
00297       // set data iterator to last element
00298       typename std::vector<DataT>::const_iterator dataVectorEndIterator = dataVector_arg.end ();
00299 
00300       // we will rebuild an octree -> reset leafCount
00301       leafCount_ = 0;
00302 
00303       // iterator for binary tree structure vector
00304       std::vector<char>::const_iterator binaryTreeVectorIterator = binaryTreeIn_arg.begin ();
00305       std::vector<char>::const_iterator binaryTreeVectorIteratorEnd = binaryTreeIn_arg.end ();
00306 
00307       deserializeTreeRecursive (rootNode_, depthMask_, newKey, binaryTreeVectorIterator, binaryTreeVectorIteratorEnd, &dataVectorIterator,
00308                                 &dataVectorEndIterator, false, doXORDecoding_arg);
00309 
00310 
00311       // we modified the octree structure -> clean-up/tree-reset might be required
00312       treeDirtyFlag_ = false;
00313     }
00314 
00315 
00317     template<typename DataT, typename LeafT, typename BranchT> void
00318     Octree2BufBase<DataT, LeafT, BranchT>::serializeNewLeafs (std::vector<DataT>& dataVector_arg,
00319                                                      const int minPointsPerLeaf_arg)
00320     {
00321       OctreeKey newKey;
00322 
00323       // clear output vector
00324       dataVector_arg.clear ();
00325 
00326       dataVector_arg.reserve (leafCount_);
00327 
00328       serializeTreeRecursive (rootNode_, newKey, 0, &dataVector_arg, false, true, minPointsPerLeaf_arg);
00329 
00330       // serializeLeafsRecursive cleans-up unused octree nodes in previous octree
00331       treeDirtyFlag_ = false;
00332     }
00333 
00335     template<typename DataT, typename LeafT, typename BranchT> LeafT*
00336     Octree2BufBase<DataT, LeafT, BranchT>::createLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg,
00337                                                     BranchNode* branch_arg, bool branchReset_arg)
00338     {
00339       // index to branch child
00340       unsigned char childIdx;
00341       LeafT* result = 0;
00342 
00343       // branch reset -> this branch has been taken from previous buffer
00344       if (branchReset_arg)
00345       {
00346         // we can safely remove children references
00347         for (childIdx = 0; childIdx < 8; childIdx++)
00348         {
00349           branch_arg->setChildPtr(bufferSelector_, childIdx, 0);
00350         }
00351       }
00352 
00353       // find branch child from key
00354       childIdx = key_arg.getChildIdxWithDepthMask (depthMask_arg);
00355 
00356       if (depthMask_arg > 1)
00357       {
00358         // we have not reached maximum tree depth
00359         BranchNode* childBranch;
00360         bool doNodeReset;
00361 
00362         doNodeReset = false;
00363 
00364         // if required branch does not exist
00365         if (!branch_arg->hasChild(bufferSelector_, childIdx))
00366         {
00367           // check if we find a branch node reference in previous buffer
00368 
00369           if (branch_arg->hasChild(!bufferSelector_, childIdx))
00370           {
00371             OctreeNode* childNode = branch_arg->getChildPtr(!bufferSelector_,childIdx);
00372 
00373             if (childNode->getNodeType()==BRANCH_NODE) {
00374               childBranch = static_cast<BranchNode*> (childNode);
00375               branch_arg->setChildPtr(bufferSelector_, childIdx, childNode);
00376             } else {
00377               // depth has changed.. child in preceeding buffer is a leaf node.
00378               deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
00379               createBranchChild (*branch_arg, childIdx, childBranch);
00380             }
00381 
00382             // take child branch from previous buffer
00383             doNodeReset = true; // reset the branch pointer array of stolen child node
00384 
00385           }
00386           else
00387           {
00388             // if required branch does not exist -> create it
00389             createBranchChild (*branch_arg, childIdx, childBranch);
00390           }
00391 
00392           branchCount_++;
00393         }
00394         // required branch node already exists - use it
00395         else
00396           childBranch = static_cast<BranchNode*> (branch_arg->getChildPtr(bufferSelector_,childIdx));
00397         
00398         // recursively proceed with indexed child branch
00399         result = createLeafRecursive (key_arg, depthMask_arg / 2, childBranch, doNodeReset);
00400       }
00401       else
00402       {
00403         // branch childs are leaf nodes
00404         LeafNode* childLeaf;
00405         if (!branch_arg->hasChild(bufferSelector_, childIdx))
00406         {
00407           // leaf node at childIdx does not exist
00408           
00409           // check if we can take copy a reference from previous buffer
00410           if (branch_arg->hasChild(!bufferSelector_, childIdx))
00411           {
00412 
00413             OctreeNode * childNode = branch_arg->getChildPtr(!bufferSelector_,childIdx);
00414             if (childNode->getNodeType () == LEAF_NODE)
00415             {
00416               childLeaf = static_cast<LeafNode*> (childNode);
00417               branch_arg->setChildPtr(bufferSelector_, childIdx, childNode);
00418               childLeaf->reset ();
00419             } else {
00420               // depth has changed.. child in preceeding buffer is a leaf node.
00421               deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
00422               createLeafChild (*branch_arg, childIdx, childLeaf);
00423             }
00424             leafCount_++;  
00425           }
00426           else
00427           {
00428             // if required leaf does not exist -> create it
00429             createLeafChild (*branch_arg, childIdx, childLeaf);
00430             leafCount_++;
00431           }
00432           
00433           // return leaf node
00434           result = childLeaf;  
00435         }
00436         else
00437         {
00438           // leaf node already exist
00439           childLeaf = static_cast<LeafNode*> (branch_arg->getChildPtr(bufferSelector_,childIdx));
00440           
00441           // return leaf node
00442           result = childLeaf;
00443         }
00444       }
00445       
00446       return (result);
00447     }
00448 
00450     template<typename DataT, typename LeafT, typename BranchT> LeafT*
00451     Octree2BufBase<DataT, LeafT, BranchT>::findLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg,
00452                                                      BranchNode* branch_arg) const
00453     {
00454       // return leaf node
00455       unsigned char childIdx;
00456       LeafT* result = 0;
00457 
00458       // find branch child from key
00459       childIdx = key_arg.getChildIdxWithDepthMask (depthMask_arg);
00460 
00461       if (depthMask_arg > 1)
00462       {
00463         // we have not reached maximum tree depth
00464         BranchNode* childBranch;
00465         childBranch = static_cast<BranchNode*> (branch_arg->getChildPtr(bufferSelector_,childIdx));
00466         
00467         if (childBranch)
00468           // recursively proceed with indexed child branch
00469           result = findLeafRecursive (key_arg, depthMask_arg / 2, childBranch);
00470       }
00471       else
00472       {
00473         // we reached leaf node level
00474         if (branch_arg->hasChild(bufferSelector_, childIdx))
00475         {
00476           // return existing leaf node
00477           LeafNode* childLeaf = static_cast<LeafNode*> (branch_arg->getChildPtr(bufferSelector_,childIdx));
00478           result = childLeaf;
00479         }
00480       }    
00481       return (result);
00482     }
00483 
00485     template<typename DataT, typename LeafT, typename BranchT> bool
00486     Octree2BufBase<DataT, LeafT, BranchT>::deleteLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg,
00487                                                        BranchNode* branch_arg)
00488     {
00489       // index to branch child
00490       unsigned char childIdx;
00491       // indicates if branch is empty and can be safely removed
00492       bool bNoChilds;
00493 
00494       // find branch child from key
00495       childIdx = key_arg.getChildIdxWithDepthMask (depthMask_arg);
00496 
00497       if (depthMask_arg > 1)
00498       {
00499         // we have not reached maximum tree depth
00500         BranchNode* childBranch;
00501         bool bBranchOccupied;
00502         
00503         // next branch child on our path through the tree
00504         childBranch = static_cast<BranchNode*> (branch_arg->getChildPtr(bufferSelector_,childIdx));
00505         
00506         if (childBranch)
00507         {
00508           // recursively explore the indexed child branch
00509           bBranchOccupied = deleteLeafRecursive (key_arg, depthMask_arg / 2, childBranch);
00510           
00511           if (!bBranchOccupied)
00512           {
00513             // child branch does not own any sub-child nodes anymore -> delete child branch
00514             deleteBranchChild (*branch_arg, bufferSelector_, childIdx);
00515             branchCount_--;
00516           }
00517         }
00518       }
00519       else
00520       {
00521         // our child is a leaf node -> delete it
00522         deleteBranchChild (*branch_arg, bufferSelector_, childIdx);
00523         leafCount_--;
00524       }
00525 
00526       // check if current branch still owns childs
00527       bNoChilds = false;
00528       for (childIdx = 0; childIdx < 8; childIdx++)
00529       {
00530         bNoChilds = branch_arg->hasChild(bufferSelector_, childIdx);
00531         if (bNoChilds)
00532           break;
00533       }
00534 
00535       // return true if current branch can be deleted
00536       return (bNoChilds);
00537     }
00538 
00540     template<typename DataT, typename LeafT, typename BranchT> void Octree2BufBase<
00541         DataT, LeafT, BranchT>::serializeTreeRecursive (BranchNode* branch_arg,
00542         OctreeKey& key_arg, std::vector<char>* binaryTreeOut_arg,
00543         typename std::vector<DataT>* dataVector_arg, bool doXOREncoding_arg,
00544         bool newLeafsFilter_arg, std::size_t minPointsPerLeaf_arg)
00545     {
00546       // child iterator
00547       unsigned char childIdx;
00548 
00549       // bit pattern
00550       char branchBitPatternCurrBuffer;
00551       char branchBitPatternPrevBuffer;
00552       char nodeXORBitPattern;
00553 
00554       // occupancy bit patterns of branch node  (current and previous octree buffer)
00555       branchBitPatternCurrBuffer = getBranchBitPattern (*branch_arg, bufferSelector_);
00556       branchBitPatternPrevBuffer = getBranchBitPattern (*branch_arg, !bufferSelector_);
00557 
00558       // XOR of current and previous occupancy bit patterns
00559       nodeXORBitPattern = branchBitPatternCurrBuffer ^ branchBitPatternPrevBuffer;
00560 
00561       if (binaryTreeOut_arg)
00562       {
00563         if (doXOREncoding_arg)
00564         {
00565           // write XOR bit pattern to output vector
00566           binaryTreeOut_arg->push_back (nodeXORBitPattern);
00567         }
00568         else
00569         {
00570           // write bit pattern of current buffer to output vector
00571           binaryTreeOut_arg->push_back (branchBitPatternCurrBuffer);
00572         }
00573       }
00574 
00575       // iterate over all children
00576       for (childIdx = 0; childIdx < 8; childIdx++)
00577       {
00578         if (branch_arg->hasChild(bufferSelector_, childIdx))
00579         {
00580           // add current branch voxel to key
00581           key_arg.pushBranch(childIdx);
00582           
00583           OctreeNode *childNode = branch_arg->getChildPtr(bufferSelector_,childIdx);
00584           
00585           switch (childNode->getNodeType ())
00586           {
00587             case BRANCH_NODE:
00588             {
00589               // recursively proceed with indexed child branch
00590               serializeTreeRecursive (static_cast<BranchNode*> (childNode),
00591                   key_arg, binaryTreeOut_arg, dataVector_arg, doXOREncoding_arg,
00592                   newLeafsFilter_arg, minPointsPerLeaf_arg);
00593               break;
00594             }
00595             case LEAF_NODE:
00596             {
00597               LeafNode* childLeaf = static_cast<LeafNode*> (childNode);
00598 
00599 
00600               if (childLeaf->getSize () >= minPointsPerLeaf_arg)
00601               {
00602                 if (!newLeafsFilter_arg)
00603                 {
00604                   if (dataVector_arg)
00605                     childLeaf->getData (*dataVector_arg);
00606 
00607                   // we reached a leaf node -> execute serialization callback
00608                   serializeTreeCallback (*childLeaf, key_arg);
00609                 }
00610                 else if (!branch_arg->hasChild (!bufferSelector_, childIdx))
00611                 {
00612                   if (dataVector_arg)
00613                     childLeaf->getData (*dataVector_arg);
00614 
00615                   serializeTreeCallback (*childLeaf, key_arg);
00616                 }
00617               }
00618 
00619 
00620               break;
00621             }
00622             default:
00623               break;
00624           }
00625 
00626           // pop current branch voxel from key
00627           key_arg.popBranch();
00628         }
00629         else if (branch_arg->hasChild (!bufferSelector_, childIdx))
00630         {
00631 
00632           // delete branch, free memory
00633           deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
00634 
00635         }
00636 
00637       }
00638     }
00639 
00640 
00642     template<typename DataT, typename LeafT, typename BranchT> void
00643     Octree2BufBase<DataT, LeafT, BranchT>::deserializeTreeRecursive (BranchNode* branch_arg,
00644         unsigned int depthMask_arg, OctreeKey& key_arg,
00645         typename std::vector<char>::const_iterator& binaryTreeIT_arg,
00646         typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
00647         typename std::vector<DataT>::const_iterator* dataVectorIterator_arg,
00648         typename std::vector<DataT>::const_iterator* dataVectorEndIterator_arg,
00649         bool branchReset_arg, bool doXORDecoding_arg)
00650     {
00651       // child iterator
00652       unsigned char childIdx;
00653 
00654       // node bits
00655       char nodeBits;
00656       char recoveredNodeBits;
00657 
00658       // branch reset -> this branch has been taken from previous buffer
00659       if (branchReset_arg)
00660       {
00661         // we can safely remove children references
00662         for (childIdx = 0; childIdx < 8; childIdx++)
00663         {
00664           branch_arg->setChildPtr(bufferSelector_, childIdx, 0);
00665         }  
00666       }
00667 
00668       if (binaryTreeIT_arg!=binaryTreeIT_End_arg) {
00669         // read branch occupancy bit pattern from vector
00670         nodeBits = *binaryTreeIT_arg++;
00671 
00672 
00673         // recover branch occupancy bit pattern
00674         if (doXORDecoding_arg)
00675         {
00676           recoveredNodeBits = getBranchBitPattern (*branch_arg, !bufferSelector_) ^ nodeBits;
00677         }
00678         else
00679         {
00680           recoveredNodeBits = nodeBits;
00681         }
00682 
00683         // iterate over all children
00684         for (childIdx = 0; childIdx < 8; childIdx++)
00685         {
00686           // if occupancy bit for childIdx is set..
00687           if (recoveredNodeBits & (1 << childIdx))
00688           {
00689             // add current branch voxel to key
00690             key_arg.pushBranch(childIdx);
00691 
00692             bool doNodeReset;
00693             
00694             doNodeReset = false;
00695             
00696             if (depthMask_arg > 1)
00697             {
00698               // we have not reached maximum tree depth
00699 
00700               BranchNode* childBranch;
00701 
00702               // check if we find a branch node reference in previous buffer
00703               if (!branch_arg->hasChild(bufferSelector_, childIdx))
00704               {
00705 
00706                 if (branch_arg->hasChild(!bufferSelector_, childIdx))
00707                 {
00708                   OctreeNode* childNode = branch_arg->getChildPtr(!bufferSelector_,childIdx);
00709 
00710                   if (childNode->getNodeType()==BRANCH_NODE) {
00711                     childBranch = static_cast<BranchNode*> (childNode);
00712                     branch_arg->setChildPtr(bufferSelector_, childIdx, childNode);
00713                   } else {
00714                     // depth has changed.. child in preceeding buffer is a leaf node.
00715                     deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
00716                     createBranchChild (*branch_arg, childIdx, childBranch);
00717                   }
00718 
00719                   // take child branch from previous buffer
00720                   doNodeReset = true; // reset the branch pointer array of stolen child node
00721                 }
00722                 else
00723                 {
00724                   // if required branch does not exist -> create it
00725                   createBranchChild (*branch_arg, childIdx, childBranch);
00726                 }
00727 
00728                 branchCount_++;
00729 
00730               }
00731               else
00732               {
00733                 // required branch node already exists - use it
00734                 childBranch = static_cast<BranchNode*> (branch_arg->getChildPtr(bufferSelector_,childIdx));
00735               }
00736 
00737               // recursively proceed with indexed child branch
00738               deserializeTreeRecursive (childBranch, depthMask_arg / 2, key_arg,
00739                   binaryTreeIT_arg, binaryTreeIT_End_arg,
00740                   dataVectorIterator_arg, dataVectorEndIterator_arg,
00741                   doNodeReset, doXORDecoding_arg);
00742 
00743             }
00744             else
00745             {
00746               // branch childs are leaf nodes
00747               LeafNode* childLeaf;
00748               
00749               // check if we can take copy a reference pointer from previous buffer
00750               if (branch_arg->hasChild(!bufferSelector_, childIdx))
00751               {
00752                 // take child leaf node from previous buffer
00753                 OctreeNode* childNode = branch_arg->getChildPtr(!bufferSelector_,childIdx);
00754                 if (childNode->getNodeType()==LEAF_NODE) {
00755                   childLeaf = static_cast<LeafNode*> (childNode);
00756                   branch_arg->setChildPtr(bufferSelector_, childIdx, childNode);
00757                   childLeaf->reset ();
00758                 } else {
00759                   // depth has changed.. child in preceeding buffer is a leaf node.
00760                   deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
00761                   createLeafChild (*branch_arg, childIdx, childLeaf);
00762                 }
00763               }
00764               else
00765               {
00766                 // if required leaf does not exist -> create it
00767                 createLeafChild (*branch_arg, childIdx, childLeaf);
00768               }
00769               
00770               leafCount_++;
00771               
00772               OctreeKey dataKey;
00773               bool bKeyBasedEncoding = false;
00774 
00775               if (dataVectorIterator_arg && dataVectorEndIterator_arg)
00776               {
00777                 // add DataT objects to octree leaf as long as their key fit to voxel
00778                 while ( (*dataVectorIterator_arg != *dataVectorEndIterator_arg)
00779                     && (this->genOctreeKeyForDataT (**dataVectorIterator_arg,
00780                         dataKey) && (dataKey == key_arg)))
00781                 {
00782                   childLeaf->setData (**dataVectorIterator_arg);
00783                   (*dataVectorIterator_arg)++;
00784                   bKeyBasedEncoding = true;
00785                   objectCount_++;
00786                 }
00787 
00788                 // add single DataT object to octree if key-based encoding is disabled
00789                 if (!bKeyBasedEncoding)
00790                 {
00791                   childLeaf->setData (**dataVectorIterator_arg);
00792                   (*dataVectorIterator_arg)++;
00793                   objectCount_++;
00794                 }
00795               }
00796 
00797               // execute deserialization callback
00798               this->deserializeTreeCallback (*childLeaf, key_arg);
00799             }
00800 
00801             // pop current branch voxel from key
00802             key_arg.popBranch();
00803           }
00804           else if (branch_arg->hasChild (!bufferSelector_, childIdx))
00805           {
00806             // remove old branch pointer information in current branch
00807             branch_arg->setChildPtr(bufferSelector_, childIdx, 0);
00808             
00809             // remove unused branches in previous buffer
00810             deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
00811           }
00812         }
00813       }
00814 
00815     }
00816     
00818     template<typename DataT, typename LeafT, typename BranchT> void
00819     Octree2BufBase<DataT, LeafT, BranchT>::treeCleanUpRecursive (BranchNode* branch_arg)
00820     {
00821       // child iterator
00822       unsigned char childIdx;
00823 
00824       // bit pattern
00825       char nodeBitPatternLastBuffer;
00826       char nodeXORBitPattern;
00827       char unusedBranchesBits;
00828 
00829       // occupancy bit pattern of branch node  (previous octree buffer)
00830       nodeBitPatternLastBuffer = getBranchBitPattern (*branch_arg, !bufferSelector_);
00831 
00832       // XOR of current and previous occupancy bit patterns
00833       nodeXORBitPattern = getBranchXORBitPattern (*branch_arg);
00834 
00835       // bit pattern indicating unused octree nodes in previous branch
00836       unusedBranchesBits = nodeXORBitPattern & nodeBitPatternLastBuffer;
00837 
00838       // iterate over all children
00839       for (childIdx = 0; childIdx < 8; childIdx++)
00840       {
00841         if (branch_arg->hasChild(bufferSelector_, childIdx))
00842         {
00843           OctreeNode *childNode = branch_arg->getChildPtr(bufferSelector_,childIdx);
00844           
00845           switch (childNode->getNodeType ())
00846           {
00847             case BRANCH_NODE:
00848             {
00849               // recursively proceed with indexed child branch
00850               treeCleanUpRecursive (static_cast<BranchNode*> (childNode));
00851               break;
00852             }
00853             case LEAF_NODE:
00854               // leaf level - nothing to do..
00855               break;
00856             default:
00857               break;
00858           }
00859         }
00860 
00861           // check for unused branches in previous buffer
00862         if (unusedBranchesBits & (1 << childIdx))
00863         {
00864           // delete branch, free memory
00865           deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
00866         }  
00867       }
00868     }
00869   }
00870 }
00871 
00872 #define PCL_INSTANTIATE_Octree2BufBase(T) template class PCL_EXPORTS pcl::octree::Octree2BufBase<T>;
00873 
00874 #endif
00875 


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