Point Cloud Library (PCL)  1.3.1
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-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: octree2buf_base.hpp 3017 2011-11-01 03:24:04Z rusu $
00037  */
00038 
00039 #ifndef OCTREE_2BUF_BASE_HPP
00040 #define OCTREE_2BUF_BASE_HPP
00041 
00042 namespace pcl
00043 {
00044   namespace octree
00045   {
00046 
00047     using namespace std;
00048 
00050     template<typename DataT, typename LeafT>
00051       Octree2BufBase<DataT, LeafT>::Octree2BufBase ()
00052       {
00053 
00054         // Initialization of globals
00055         rootNode_ = new OctreeBranch ();
00056         leafCount_ = 0;
00057         branchCount_ = 1;
00058         objectCount_ = 0;
00059         depthMask_ = 0;
00060         octreeDepth_ = 0;
00061         bufferSelector_ = 0;
00062         resetTree_ = false;
00063         treeDirtyFlag_ = false;
00064       }
00065 
00067     template<typename DataT, typename LeafT>
00068       Octree2BufBase<DataT, LeafT>::~Octree2BufBase ()
00069       {
00070 
00071         // deallocate tree structure
00072         deleteTree ();
00073         delete (rootNode_);
00074         poolCleanUp ();
00075       }
00076 
00078     template<typename DataT, typename LeafT>
00079       void
00080       Octree2BufBase<DataT, LeafT>::setMaxVoxelIndex (unsigned int maxVoxelIndex_arg)
00081       {
00082         unsigned int treeDepth;
00083 
00084         assert (maxVoxelIndex_arg > 0);
00085 
00086         // tree depth == amount of bits of maxVoxels
00087         treeDepth = max ((min ((unsigned int)OCT_MAXTREEDEPTH, (unsigned int)ceil (Log2 (maxVoxelIndex_arg)))),
00088                          (unsigned int)0);
00089 
00090         // define depthMask_ by setting a single bit to 1 at bit position == tree depth
00091         depthMask_ = (1 << (treeDepth - 1));
00092 
00093       }
00094 
00096     template<typename DataT, typename LeafT>
00097       void
00098       Octree2BufBase<DataT, LeafT>::setTreeDepth (unsigned int depth_arg)
00099       {
00100 
00101         assert (depth_arg > 0);
00102 
00103         // set octree depth
00104         octreeDepth_ = depth_arg;
00105 
00106         // define depthMask_ by setting a single bit to 1 at bit position == tree depth
00107         depthMask_ = (1 << (depth_arg - 1));
00108       }
00109 
00111     template<typename DataT, typename LeafT>
00112       void
00113       Octree2BufBase<DataT, LeafT>::add (const unsigned int idxX_arg, const unsigned int idxY_arg,
00114                                          const unsigned int idxZ_arg, const DataT& data_arg)
00115       {
00116         OctreeKey key;
00117 
00118         // generate key
00119         genOctreeKeyByIntIdx (idxX_arg, idxY_arg, idxZ_arg, key);
00120 
00121         // add data_arg to octree
00122         add (key, data_arg);
00123 
00124         objectCount_++;
00125 
00126       }
00127 
00129     template<typename DataT, typename LeafT>
00130       bool
00131       Octree2BufBase<DataT, LeafT>::get (const unsigned int idxX_arg, const unsigned int idxY_arg,
00132                                          const unsigned int idxZ_arg, DataT& data_arg) const
00133       {
00134         OctreeKey key;
00135 
00136         // generate key
00137         genOctreeKeyByIntIdx (idxX_arg, idxY_arg, idxZ_arg, key);
00138 
00139         // search for leaf at key
00140         LeafT* leaf = findLeaf (key);
00141         if (leaf)
00142         {
00143           const DataT * dataPtr;
00144           // if successful, decode data to data_arg
00145           leaf->getData (dataPtr);
00146           if (dataPtr)
00147             data_arg = *dataPtr;
00148         }
00149         // returns true on success
00150         return (leaf != 0);
00151       }
00152 
00154     template<typename DataT, typename LeafT>
00155       bool
00156       Octree2BufBase<DataT, LeafT>::existLeaf (const unsigned int idxX_arg, const unsigned int idxY_arg,
00157                                                const unsigned int idxZ_arg) const
00158       {
00159         OctreeKey key;
00160 
00161         // generate key
00162         this->genOctreeKeyByIntIdx (idxX_arg, idxY_arg, idxZ_arg, key);
00163 
00164         // check if key exist in octree
00165         return existLeaf (key);
00166       }
00167 
00169     template<typename DataT, typename LeafT>
00170       void
00171       Octree2BufBase<DataT, LeafT>::removeLeaf (const unsigned int idxX_arg, const unsigned int idxY_arg,
00172                                                 const unsigned int idxZ_arg)
00173       {
00174         OctreeKey key;
00175 
00176         // generate key
00177         this->genOctreeKeyByIntIdx (idxX_arg, idxY_arg, idxZ_arg, key);
00178 
00179         // free voxel at key
00180         return this->removeLeaf (key);
00181       }
00182 
00184     template<typename DataT, typename LeafT>
00185       void
00186       Octree2BufBase<DataT, LeafT>::deleteTree ( bool freeMemory_arg )
00187       {
00188 
00189         if (rootNode_)
00190         {
00191           // reset octree
00192           deleteBranch (*rootNode_);
00193           leafCount_ = 0;
00194           branchCount_ = 1;
00195           objectCount_ = 0;
00196 
00197           resetTree_ = false;
00198           treeDirtyFlag_ = false;
00199           depthMask_ = 0;
00200           octreeDepth_ = 0;
00201         }
00202 
00203         // delete node pool
00204         if (freeMemory_arg)
00205           poolCleanUp ();
00206 
00207       }
00208 
00210     template<typename DataT, typename LeafT>
00211       void
00212       Octree2BufBase<DataT, LeafT>::switchBuffers ()
00213       {
00214         if (treeDirtyFlag_)
00215         {
00216           // make sure that all unused branch nodes from previous buffer are deleted
00217           treeCleanUpRecursive (rootNode_);
00218         }
00219 
00220         // switch butter selector
00221         bufferSelector_ = !bufferSelector_;
00222 
00223         // reset flags
00224         treeDirtyFlag_ = true;
00225         leafCount_ = 0;
00226         objectCount_ = 0;
00227         branchCount_ = 1;
00228         resetTree_ = true;
00229       }
00230 
00232     template<typename DataT, typename LeafT>
00233       void
00234       Octree2BufBase<DataT, LeafT>::serializeTree (std::vector<char>& binaryTreeOut_arg, bool doXOREncoding_arg)
00235       {
00236         OctreeKey newKey;
00237         newKey.x = newKey.y = newKey.z = 0;
00238 
00239         // clear binary vector
00240         binaryTreeOut_arg.clear ();
00241         binaryTreeOut_arg.reserve (this->branchCount_);
00242 
00243         serializeTreeRecursive (binaryTreeOut_arg, rootNode_, newKey, doXOREncoding_arg);
00244 
00245         // serializeTreeRecursive cleans-up unused octree nodes in previous octree
00246         treeDirtyFlag_ = false;
00247 
00248       }
00249 
00251     template<typename DataT, typename LeafT>
00252       void
00253       Octree2BufBase<DataT, LeafT>::serializeTree (std::vector<char>& binaryTreeOut_arg,
00254                                                    std::vector<DataT>& dataVector_arg, bool doXOREncoding_arg)
00255       {
00256         OctreeKey newKey;
00257         newKey.x = newKey.y = newKey.z = 0;
00258 
00259         // clear output vectors
00260         binaryTreeOut_arg.clear ();
00261         dataVector_arg.clear ();
00262 
00263         dataVector_arg.reserve (objectCount_);
00264         binaryTreeOut_arg.reserve (this->branchCount_);
00265 
00266 
00267         Octree2BufBase<DataT, LeafT>::serializeTreeRecursive (binaryTreeOut_arg, rootNode_, newKey,
00268                                                               dataVector_arg, doXOREncoding_arg);
00269 
00270         // serializeTreeRecursive cleans-up unused octree nodes in previous octree
00271         treeDirtyFlag_ = false;
00272       }
00273 
00275     template<typename DataT, typename LeafT>
00276       void
00277       Octree2BufBase<DataT, LeafT>::serializeLeafs (std::vector<DataT>& dataVector_arg)
00278       {
00279         OctreeKey newKey;
00280         newKey.x = newKey.y = newKey.z = 0;
00281 
00282         // clear output vector
00283         dataVector_arg.clear ();
00284 
00285         dataVector_arg.reserve (objectCount_);
00286 
00287         serializeLeafsRecursive (rootNode_, newKey, dataVector_arg);
00288 
00289         // serializeLeafsRecursive cleans-up unused octree nodes in previous octree
00290         treeDirtyFlag_ = false;
00291       }
00292 
00294     template<typename DataT, typename LeafT>
00295       void
00296       Octree2BufBase<DataT, LeafT>::deserializeTree (std::vector<char>& binaryTreeIn_arg, bool doXORDecoding_arg)
00297       {
00298         OctreeKey newKey;
00299         newKey.x = newKey.y = newKey.z = 0;
00300 
00301         // we will rebuild an octree -> reset leafCount
00302         leafCount_ = 0;
00303 
00304         // iterator for binary tree structure vector
00305         vector<char>::const_iterator binaryTreeVectorIterator = binaryTreeIn_arg.begin ();
00306 
00307         deserializeTreeRecursive (binaryTreeVectorIterator, rootNode_, depthMask_, newKey, resetTree_, doXORDecoding_arg);
00308 
00309         // we modified the octree structure -> clean-up/tree-reset might be required
00310         resetTree_ = false;
00311         treeDirtyFlag_ = true;
00312 
00313         objectCount_ = this->leafCount_;
00314 
00315       }
00316 
00318     template<typename DataT, typename LeafT>
00319       void
00320       Octree2BufBase<DataT, LeafT>::deserializeTree (std::vector<char>& binaryTreeIn_arg,
00321                                                      std::vector<DataT>& dataVector_arg, bool doXORDecoding_arg)
00322       {
00323         OctreeKey newKey;
00324         newKey.x = newKey.y = newKey.z = 0;
00325 
00326         // set data iterator to first element
00327         typename std::vector<DataT>::const_iterator dataVectorIterator = dataVector_arg.begin ();
00328 
00329         // set data iterator to last element
00330         typename std::vector<DataT>::const_iterator dataVectorEndIterator = dataVector_arg.end ();
00331 
00332         // we will rebuild an octree -> reset leafCount
00333         leafCount_ = 0;
00334 
00335         // iterator for binary tree structure vector
00336         vector<char>::const_iterator binaryTreeVectorIterator = binaryTreeIn_arg.begin ();
00337 
00338         deserializeTreeRecursive (binaryTreeVectorIterator, rootNode_, depthMask_, newKey, dataVectorIterator,
00339                                   dataVectorEndIterator, resetTree_, doXORDecoding_arg);
00340 
00341         // we modified the octree structure -> clean-up/tree-reset might be required
00342         resetTree_ = false;
00343         treeDirtyFlag_ = true;
00344 
00345         objectCount_ = dataVector_arg.size();
00346       }
00347 
00349     template<typename DataT, typename LeafT>
00350       void
00351       Octree2BufBase<DataT, LeafT>::deserializeTreeAndOutputLeafData (std::vector<char>& binaryTreeIn_arg,
00352                                                                       std::vector<DataT>& dataVector_arg,
00353                                                                       bool doXORDecoding_arg)
00354       {
00355 
00356         OctreeKey newKey;
00357         newKey.x = newKey.y = newKey.z = 0;
00358 
00359         // free existing tree before tree rebuild
00360         deleteTree ();
00361 
00362         // iterator for binary tree structure vector
00363         vector<char>::const_iterator binaryTreeVectorIterator = binaryTreeIn_arg.begin ();
00364 
00365         deserializeTreeAndOutputLeafDataRecursive (binaryTreeVectorIterator, rootNode_, depthMask_, newKey,
00366                                                    dataVector_arg, resetTree_, doXORDecoding_arg);
00367 
00368         // we modified the octree structure -> clean-up/tree-reset might be required
00369         resetTree_ = false;
00370         treeDirtyFlag_ = true;
00371 
00372         objectCount_ = dataVector_arg.size();
00373       }
00374 
00376     template<typename DataT, typename LeafT>
00377       void
00378       Octree2BufBase<DataT, LeafT>::serializeNewLeafs (std::vector<DataT>& dataVector_arg,
00379                                                        const int minPointsPerLeaf_arg)
00380       {
00381         OctreeKey newKey;
00382         newKey.x = newKey.y = newKey.z = 0;
00383 
00384         // clear output vector
00385         dataVector_arg.clear ();
00386 
00387         dataVector_arg.reserve (leafCount_);
00388 
00389         serializeNewLeafsRecursive (rootNode_, newKey, dataVector_arg, minPointsPerLeaf_arg);
00390 
00391         // serializeLeafsRecursive cleans-up unused octree nodes in previous octree
00392         treeDirtyFlag_ = false;
00393       }
00394 
00396     template<typename DataT, typename LeafT>
00397       LeafT*
00398       Octree2BufBase<DataT, LeafT>::getLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg,
00399                                                       OctreeBranch* branch_arg, bool branchReset_arg)
00400       {
00401 
00402         // index to branch child
00403         unsigned char childIdx;
00404         LeafT* result = 0;
00405 
00406         // branch reset -> this branch has been taken from previous buffer
00407         if (branchReset_arg)
00408         {
00409           // we can safely remove children references
00410           for (childIdx = 0; childIdx < 8; childIdx++)
00411           {
00412             setBranchChild (*branch_arg, childIdx, 0);
00413           }
00414 
00415         }
00416 
00417         // find branch child from key
00418         childIdx = ((!!(key_arg.x & depthMask_arg)) << 2) | ((!!(key_arg.y & depthMask_arg)) << 1) | (!!(key_arg.z
00419             & depthMask_arg));
00420 
00421         if (depthMask_arg > 1)
00422         {
00423           // we have not reached maximum tree depth
00424 
00425           OctreeBranch* childBranch;
00426           bool doNodeReset;
00427 
00428           doNodeReset = false;
00429 
00430           // if required branch does not exist
00431           if (!branchHasChild (*branch_arg, childIdx))
00432           {
00433 
00434             // check if we find a branch node reference in previous buffer
00435             if (branchHasChild (*branch_arg, !bufferSelector_, childIdx))
00436             {
00437 
00438               // take child branch from previous buffer
00439               childBranch = (OctreeBranch*)getBranchChild (*branch_arg, !bufferSelector_, childIdx);
00440               setBranchChild (*branch_arg, bufferSelector_, childIdx, (OctreeNode*)childBranch);
00441 
00442               doNodeReset = true; // reset the branch pointer array of stolen child node
00443 
00444             }
00445             else
00446             {
00447               // if required branch does not exist -> create it
00448               createBranchChild (*branch_arg, childIdx, childBranch);
00449             }
00450 
00451             branchCount_++;
00452 
00453           }
00454           else
00455           {
00456             // required branch node already exists - use it
00457             childBranch = (OctreeBranch*)getBranchChild (*branch_arg, childIdx);
00458           }
00459 
00460           // recursively proceed with indexed child branch
00461           result = getLeafRecursive (key_arg, depthMask_arg / 2, childBranch, doNodeReset);
00462 
00463         }
00464         else
00465         {
00466           // branch childs are leaf nodes
00467 
00468           OctreeLeaf* childLeaf;
00469           if (!branchHasChild (*branch_arg, childIdx))
00470           {
00471             // leaf node at childIdx does not exist
00472 
00473             // check if we can take copy a reference from previous buffer
00474             if (branchHasChild (*branch_arg, !bufferSelector_, childIdx))
00475             {
00476 
00477               // take child leaf node from previous buffer
00478               childLeaf = (OctreeLeaf*)getBranchChild (*branch_arg, !bufferSelector_, childIdx);
00479               setBranchChild (*branch_arg, bufferSelector_, childIdx, (OctreeNode*)childLeaf);
00480 
00481               childLeaf->reset ();
00482 
00483               leafCount_++;
00484 
00485             }
00486             else
00487             {
00488 
00489               // if required leaf does not exist -> create it
00490               createLeafChild (*branch_arg, childIdx, childLeaf);
00491               leafCount_++;
00492             }
00493 
00494             // return leaf node
00495             result = childLeaf;
00496 
00497           }
00498           else
00499           {
00500 
00501             // leaf node already exist
00502             childLeaf = (OctreeLeaf*)getBranchChild (*branch_arg, childIdx);
00503 
00504             // return leaf node
00505             result = childLeaf;
00506 
00507           }
00508 
00509         }
00510 
00511         return result;
00512 
00513       }
00514 
00516     template<typename DataT, typename LeafT>
00517       LeafT*
00518       Octree2BufBase<DataT, LeafT>::findLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg,
00519                                                        OctreeBranch* branch_arg) const
00520       {
00521         // return leaf node
00522         unsigned char childIdx;
00523         LeafT* result = 0;
00524 
00525         // find branch child from key
00526         childIdx = ((!!(key_arg.x & depthMask_arg)) << 2) | ((!!(key_arg.y & depthMask_arg)) << 1) | (!!(key_arg.z
00527             & depthMask_arg));
00528 
00529         if (depthMask_arg > 1)
00530         {
00531           // we have not reached maximum tree depth
00532           OctreeBranch* childBranch;
00533           childBranch = (OctreeBranch*)getBranchChild (*branch_arg, childIdx);
00534 
00535           if (childBranch)
00536             // recursively proceed with indexed child branch
00537             result = findLeafRecursive (key_arg, depthMask_arg / 2, childBranch);
00538 
00539         }
00540         else
00541         {
00542           // we reached leaf node level
00543           if (branchHasChild (*branch_arg, childIdx))
00544           {
00545 
00546             // return existing leaf node
00547             OctreeLeaf* childLeaf = (OctreeLeaf*)getBranchChild (*branch_arg, childIdx);
00548             result = childLeaf;
00549           }
00550 
00551         }
00552 
00553         return result;
00554 
00555       }
00556 
00558     template<typename DataT, typename LeafT>
00559       bool
00560       Octree2BufBase<DataT, LeafT>::deleteLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg,
00561                                                          OctreeBranch* branch_arg)
00562       {
00563         // index to branch child
00564         unsigned char childIdx;
00565         // indicates if branch is empty and can be safely removed
00566         bool bNoChilds;
00567 
00568         // find branch child from key
00569         childIdx = ((!!(key_arg.x & depthMask_arg)) << 2) | ((!!(key_arg.y & depthMask_arg)) << 1) | (!!(key_arg.z
00570             & depthMask_arg));
00571 
00572         if (depthMask_arg > 1)
00573         {
00574           // we have not reached maximum tree depth
00575 
00576           OctreeBranch* childBranch;
00577           bool bBranchOccupied;
00578 
00579           // next branch child on our path through the tree
00580           childBranch = (OctreeBranch*)getBranchChild (*branch_arg, childIdx);
00581 
00582           if (childBranch)
00583           {
00584             // recursively explore the indexed child branch
00585             bBranchOccupied = deleteLeafRecursive (key_arg, depthMask_arg / 2, childBranch);
00586 
00587             if (!bBranchOccupied)
00588             {
00589               // child branch does not own any sub-child nodes anymore -> delete child branch
00590               deleteBranchChild (*branch_arg, childIdx);
00591               branchCount_--;
00592             }
00593           }
00594 
00595         }
00596         else
00597         {
00598 
00599           // our child is a leaf node -> delete it
00600           deleteBranchChild (*branch_arg, childIdx);
00601           leafCount_--;
00602         }
00603 
00604         // check if current branch still owns childs
00605         bNoChilds = false;
00606         for (childIdx = 0; childIdx < 8; childIdx++)
00607         {
00608           bNoChilds = branchHasChild (*branch_arg, childIdx);
00609           if (bNoChilds)
00610             break;
00611         }
00612 
00613         // return true if current branch can be deleted
00614         return bNoChilds;
00615 
00616       }
00617 
00619     template<typename DataT, typename LeafT>
00620       void
00621       Octree2BufBase<DataT, LeafT>::serializeTreeRecursive (std::vector<char>& binaryTreeOut_arg,
00622                                                             OctreeBranch* branch_arg, const OctreeKey& key_arg,
00623                                                             bool doXOREncoding_arg)
00624       {
00625 
00626         // child iterator
00627         unsigned char childIdx;
00628 
00629         // bit pattern
00630         char nodeBitPatternCurrentBuffer;
00631         char nodeBitPatternLastBuffer;
00632         char nodeXORBitPattern;
00633         char unusedBranchesBits;
00634 
00635         // occupancy bit patterns of branch node  (current and previous octree buffer)
00636         nodeBitPatternCurrentBuffer = getBranchBitPattern (*branch_arg, bufferSelector_);
00637         nodeBitPatternLastBuffer = getBranchBitPattern (*branch_arg, !bufferSelector_);
00638 
00639         // XOR of current and previous occupancy bit patterns
00640         nodeXORBitPattern = nodeBitPatternCurrentBuffer ^ nodeBitPatternLastBuffer;
00641 
00642         // bit pattern indicating unused octree nodes in previous branch
00643         unusedBranchesBits = nodeXORBitPattern & nodeBitPatternLastBuffer;
00644 
00645         if (doXOREncoding_arg)
00646         {
00647           // write XOR bit pattern to output vector
00648           binaryTreeOut_arg.push_back (nodeXORBitPattern);
00649         }
00650         else
00651         {
00652           // write bit pattern of current buffer to output vector
00653           binaryTreeOut_arg.push_back (nodeBitPatternCurrentBuffer);
00654         }
00655 
00656         // iterate over all children
00657         for (childIdx = 0; childIdx < 8; childIdx++)
00658         {
00659 
00660           if (branchHasChild (*branch_arg, childIdx))
00661           {
00662             const OctreeNode * childNode;
00663             childNode = getBranchChild (*branch_arg, childIdx);
00664 
00665             // generate new key for current branch voxel
00666             OctreeKey newKey;
00667             newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2)));
00668             newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1)));
00669             newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0)));
00670 
00671             switch (childNode->getNodeType ())
00672             {
00673               case BRANCH_NODE:
00674 
00675                 // recursively proceed with indexed child branch
00676                 serializeTreeRecursive (binaryTreeOut_arg, (OctreeBranch*)childNode, newKey, doXOREncoding_arg);
00677                 break;
00678 
00679               case LEAF_NODE:
00680                 OctreeLeaf* childLeaf = (OctreeLeaf*)childNode;
00681 
00682                 // we reached a leaf node -> execute serialization callback
00683                 serializeLeafCallback (*childLeaf, newKey);
00684                 break;
00685             }
00686 
00687           }
00688 
00689           // check for unused branches in previous buffer
00690           if (unusedBranchesBits & (1 << childIdx))
00691           {
00692             // delete branch, free memory
00693             deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
00694           }
00695 
00696         }
00697 
00698       }
00699 
00701     template<typename DataT, typename LeafT>
00702       void
00703       Octree2BufBase<DataT, LeafT>::serializeTreeRecursive (std::vector<char>& binaryTreeOut_arg,
00704                                                             OctreeBranch* branch_arg, const OctreeKey& key_arg,
00705                                                             typename std::vector<DataT>& dataVector_arg,
00706                                                             bool doXOREncoding_arg)
00707       {
00708 
00709         // child iterator
00710         unsigned char childIdx;
00711 
00712         // bit pattern
00713         char nodeBitPatternCurrentBuffer;
00714         char nodeBitPatternLastBuffer;
00715         char nodeXORBitPattern;
00716         char unusedBranchesBits;
00717 
00718         // occupancy bit patterns of branch node  (current and previous octree buffer)
00719         nodeBitPatternCurrentBuffer = getBranchBitPattern (*branch_arg, bufferSelector_);
00720         nodeBitPatternLastBuffer = getBranchBitPattern (*branch_arg, !bufferSelector_);
00721 
00722         // XOR of current and previous occupancy bit patterns
00723         nodeXORBitPattern = nodeBitPatternCurrentBuffer ^ nodeBitPatternLastBuffer;
00724 
00725         // bit pattern indicating unused octree nodes in previous branch
00726         unusedBranchesBits = nodeXORBitPattern & nodeBitPatternLastBuffer;
00727 
00728         if (doXOREncoding_arg)
00729         {
00730           // write XOR bit pattern to output vector
00731           binaryTreeOut_arg.push_back (nodeXORBitPattern);
00732         }
00733         else
00734         {
00735           // write bit pattern of current buffer to output vector
00736           binaryTreeOut_arg.push_back (nodeBitPatternCurrentBuffer);
00737         }
00738 
00739         // iterate over all children
00740         for (childIdx = 0; childIdx < 8; childIdx++)
00741         {
00742           if (branchHasChild (*branch_arg, childIdx))
00743           {
00744 
00745             // generate new key for current branch voxel
00746             OctreeKey newKey;
00747             newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2)));
00748             newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1)));
00749             newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0)));
00750 
00751             const OctreeNode * childNode;
00752             childNode = getBranchChild (*branch_arg, childIdx);
00753 
00754             switch (childNode->getNodeType ())
00755             {
00756               case BRANCH_NODE:
00757                 // recursively proceed with indexed child branch
00758                 serializeTreeRecursive (binaryTreeOut_arg, (OctreeBranch*)childNode, newKey, dataVector_arg,
00759                                         doXOREncoding_arg);
00760                 break;
00761 
00762               case LEAF_NODE:
00763                 OctreeLeaf* childLeaf = (OctreeLeaf*)childNode;
00764 
00765                 // we reached a leaf node -> execute serialization callback
00766                 serializeLeafCallback (*childLeaf, newKey, dataVector_arg);
00767 
00768                 break;
00769             }
00770 
00771           }
00772 
00773           // check for unused branches in previous buffer
00774           if (unusedBranchesBits & (1 << childIdx))
00775           {
00776             // delete branch, free memory
00777             deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
00778           }
00779 
00780         }
00781 
00782       }
00783 
00785     template<typename DataT, typename LeafT>
00786       void
00787       Octree2BufBase<DataT, LeafT>::serializeLeafsRecursive (OctreeBranch* branch_arg, const OctreeKey& key_arg,
00788                                                              typename std::vector<DataT>& dataVector_arg)
00789       {
00790         // child iterator
00791         unsigned char childIdx;
00792 
00793         // bit pattern
00794         char nodeBitPatternLastBuffer;
00795         char nodeXORBitPattern;
00796         char unusedBranchesBits;
00797 
00798         // occupancy bit pattern of branch node  (previous octree buffer)
00799         nodeBitPatternLastBuffer = getBranchBitPattern (*branch_arg, !bufferSelector_);
00800 
00801         // XOR of current and previous occupancy bit patterns
00802         nodeXORBitPattern = getBranchXORBitPattern (*branch_arg);
00803 
00804         // bit pattern indicating unused octree nodes in previous branch
00805         unusedBranchesBits = nodeXORBitPattern & nodeBitPatternLastBuffer;
00806 
00807         // iterate over all children
00808         for (childIdx = 0; childIdx < 8; childIdx++)
00809         {
00810           if (branchHasChild (*branch_arg, childIdx))
00811           {
00812             const OctreeNode * childNode;
00813             childNode = getBranchChild (*branch_arg, childIdx);
00814 
00815             // generate new key for current branch voxel
00816             OctreeKey newKey;
00817             newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2)));
00818             newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1)));
00819             newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0)));
00820 
00821             switch (childNode->getNodeType ())
00822             {
00823               case BRANCH_NODE:
00824 
00825                 // recursively proceed with indexed child branch
00826                 serializeLeafsRecursive ((OctreeBranch*)childNode, newKey, dataVector_arg);
00827                 break;
00828               case LEAF_NODE:
00829                 OctreeLeaf* childLeaf = (OctreeLeaf*)childNode;
00830 
00831                 // we reached a leaf node -> execute serialization callback
00832                 serializeLeafCallback (*childLeaf, newKey, dataVector_arg);
00833                 break;
00834             }
00835 
00836           }
00837 
00838           // check for unused branches in previous buffer
00839           if (unusedBranchesBits & (1 << childIdx))
00840           {
00841             // delete branch, free memory
00842             deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
00843           }
00844 
00845         }
00846       }
00847 
00849     template<typename DataT, typename LeafT>
00850       void
00851       Octree2BufBase<DataT, LeafT>::serializeNewLeafsRecursive (OctreeBranch* branch_arg, const OctreeKey& key_arg,
00852                                                                 std::vector<DataT>& dataVector_arg,
00853                                                                 const int minPointsPerLeaf_arg)
00854       {
00855         // child iterator
00856         unsigned char childIdx;
00857 
00858         // bit pattern
00859         char nodeBitPatternLastBuffer;
00860         char nodeXORBitPattern;
00861         char unusedBranchesBits;
00862 
00863         // occupancy bit pattern of branch node  (previous octree buffer)
00864         nodeBitPatternLastBuffer = getBranchBitPattern (*branch_arg, !bufferSelector_);
00865 
00866         // XOR of current and previous occupancy bit patterns
00867         nodeXORBitPattern = getBranchXORBitPattern (*branch_arg);
00868 
00869         // bit pattern indicating unused octree nodes in previous branch
00870         unusedBranchesBits = nodeXORBitPattern & nodeBitPatternLastBuffer;
00871 
00872         // iterate over all children
00873         for (childIdx = 0; childIdx < 8; childIdx++)
00874         {
00875           if (branchHasChild (*branch_arg, childIdx))
00876           {
00877             const OctreeNode * childNode;
00878             childNode = getBranchChild (*branch_arg, childIdx);
00879 
00880             // generate new key for current branch voxel
00881             OctreeKey newKey;
00882             newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2)));
00883             newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1)));
00884             newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0)));
00885 
00886             switch (childNode->getNodeType ())
00887             {
00888               case BRANCH_NODE:
00889 
00890                 // recursively proceed with indexed child branch
00891                 serializeNewLeafsRecursive ((OctreeBranch*)childNode, newKey, dataVector_arg, minPointsPerLeaf_arg);
00892                 break;
00893               case LEAF_NODE:
00894                 // check if leaf existed already in previous buffer
00895                 if (!(nodeBitPatternLastBuffer & (1 << childIdx)))
00896                 {
00897                   // we reached a leaf node
00898 
00899                   OctreeLeaf* childLeaf = (OctreeLeaf*)childNode;
00900 
00901                   serializeNewLeafCallback (*childLeaf, newKey, minPointsPerLeaf_arg, dataVector_arg);
00902                 }
00903                 break;
00904             }
00905 
00906           }
00907 
00908           // check for unused branches in previous buffer
00909           if (unusedBranchesBits & (1 << childIdx))
00910           {
00911             // delete branch, free memory
00912             deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
00913           }
00914 
00915         }
00916       }
00917 
00919     template<typename DataT, typename LeafT>
00920     void
00921     Octree2BufBase<DataT, LeafT>::deserializeTreeRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg,
00922                                                             OctreeBranch* branch_arg,
00923                                                             const unsigned int depthMask_arg,
00924                                                             const OctreeKey& key_arg, bool branchReset_arg,
00925                                                             bool doXORDecoding_arg)
00926     {
00927       // child iterator
00928       unsigned char childIdx;
00929 
00930       // node bits
00931       char nodeBits;
00932       char recoveredNodeBits;
00933 
00934       // branch reset -> this branch has been taken from previous buffer
00935       if (branchReset_arg)
00936       {
00937         // we can safely remove children references
00938         for (childIdx = 0; childIdx < 8; childIdx++)
00939         {
00940           setBranchChild (*branch_arg, childIdx, 0);
00941         }
00942 
00943       }
00944 
00945       // read branch occupancy bit pattern from vector
00946       nodeBits = (*binaryTreeIn_arg);
00947       binaryTreeIn_arg++;
00948 
00949       // recover branch occupancy bit pattern
00950       if (doXORDecoding_arg)
00951       {
00952         recoveredNodeBits = getBranchBitPattern (*branch_arg, !bufferSelector_) ^ nodeBits;
00953       }
00954       else
00955       {
00956         recoveredNodeBits = nodeBits;
00957       }
00958 
00959       // iterate over all children
00960       for (childIdx = 0; childIdx < 8; childIdx++)
00961       {
00962         // if occupancy bit for childIdx is set..
00963         if (recoveredNodeBits & (1 << childIdx))
00964         {
00965 
00966           // generate new key for current branch voxel
00967           OctreeKey newKey;
00968           newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2)));
00969           newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1)));
00970           newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0)));
00971 
00972           bool doNodeReset;
00973 
00974           doNodeReset = false;
00975 
00976           if (depthMask_arg > 1)
00977           {
00978             // we have not reached maximum tree depth
00979             OctreeBranch* childBranch;
00980 
00981             if (!branchHasChild (*branch_arg, childIdx))
00982             {
00983 
00984               // check if we find a branch node reference in previous buffer
00985               if (branchHasChild (*branch_arg, !bufferSelector_, childIdx))
00986               {
00987                 // take child branch from previous buffer
00988                 childBranch = (OctreeBranch*)getBranchChild (*branch_arg, !bufferSelector_, childIdx);
00989                 setBranchChild (*branch_arg, bufferSelector_, childIdx, (OctreeNode*)childBranch);
00990                 doNodeReset = true;
00991 
00992               }
00993               else
00994               {
00995                 // if required branch does not exist -> create it
00996                 createBranchChild (*branch_arg, childIdx, childBranch);
00997               }
00998 
00999               branchCount_++;
01000 
01001             }
01002             else
01003             {
01004               // required branch node already exists - use it
01005               childBranch = (OctreeBranch*)getBranchChild (*branch_arg, childIdx);
01006             }
01007 
01008             // recursively proceed with indexed child branch
01009             deserializeTreeRecursive (binaryTreeIn_arg, childBranch, depthMask_arg / 2, newKey, doNodeReset, doXORDecoding_arg);
01010 
01011           }
01012           else
01013           {
01014             // branch childs are leaf nodes
01015 
01016             OctreeLeaf* childLeaf;
01017 
01018             // check if we can take copy a reference from previous buffer
01019             if (branchHasChild (*branch_arg, !bufferSelector_, childIdx))
01020             {
01021               // take child leaf node from previous buffer
01022               childLeaf = (OctreeLeaf*)getBranchChild (*branch_arg, !bufferSelector_, childIdx);
01023               setBranchChild (*branch_arg, bufferSelector_, childIdx, (OctreeNode*)childLeaf);
01024 
01025               // reset child leaf
01026               childLeaf->reset ();
01027 
01028             }
01029             else
01030             {
01031               // if required leaf does not exist -> create it
01032               createLeafChild (*branch_arg, childIdx, childLeaf);
01033 
01034             }
01035 
01036             // execute deserialization callback
01037             deserializeLeafCallback (*childLeaf, newKey);
01038 
01039             leafCount_++;
01040 
01041           }
01042         }
01043         else
01044         {
01045 
01046           // remove old branch pointer information in current branch
01047           setBranchChild (*branch_arg, bufferSelector_, childIdx, 0);
01048 
01049           // remove unused branches in previous buffer
01050           deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
01051         }
01052       }
01053 
01054     }
01055 
01057     template<typename DataT, typename LeafT>
01058     void
01059     Octree2BufBase<DataT, LeafT>::deserializeTreeRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg,
01060                                                             OctreeBranch* branch_arg,
01061                                                             const unsigned int depthMask_arg,
01062                                                             const OctreeKey& key_arg,
01063                                                             typename std::vector<DataT>::const_iterator& dataVectorIterator_arg,
01064                                                             typename std::vector<DataT>::const_iterator& dataVectorEndIterator_arg,
01065                                                             bool branchReset_arg, bool doXORDecoding_arg)
01066     {
01067       // child iterator
01068       unsigned char childIdx;
01069 
01070       // node bits
01071       char nodeBits;
01072       char recoveredNodeBits;
01073 
01074       // branch reset -> this branch has been taken from previous buffer
01075       if (branchReset_arg)
01076       {
01077         // we can safely remove children references
01078         for (childIdx = 0; childIdx < 8; childIdx++)
01079         {
01080           setBranchChild (*branch_arg, childIdx, 0);
01081         }
01082 
01083       }
01084 
01085       // read branch occupancy bit pattern from vector
01086       nodeBits = (*binaryTreeIn_arg);
01087       binaryTreeIn_arg++;
01088 
01089       // recover branch occupancy bit pattern
01090       if (doXORDecoding_arg)
01091       {
01092         recoveredNodeBits = getBranchBitPattern (*branch_arg, !bufferSelector_) ^ nodeBits;
01093       }
01094       else
01095       {
01096         recoveredNodeBits = nodeBits;
01097       }
01098 
01099       // iterate over all children
01100       for (childIdx = 0; childIdx < 8; childIdx++)
01101       {
01102         // if occupancy bit for childIdx is set..
01103         if (recoveredNodeBits & (1 << childIdx))
01104         {
01105           // generate new key for current branch voxel
01106           OctreeKey newKey;
01107           newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2)));
01108           newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1)));
01109           newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0)));
01110 
01111           bool doNodeReset;
01112 
01113           doNodeReset = false;
01114 
01115           if (depthMask_arg > 1)
01116           {
01117             // we have not reached maximum tree depth
01118 
01119             OctreeBranch* childBranch;
01120 
01121             // check if we find a branch node reference in previous buffer
01122             if (!branchHasChild (*branch_arg, childIdx))
01123             {
01124 
01125               if (branchHasChild (*branch_arg, !bufferSelector_, childIdx))
01126               {
01127                 // take child branch from previous buffer
01128                 childBranch = (OctreeBranch*)getBranchChild (*branch_arg, !bufferSelector_, childIdx);
01129                 setBranchChild (*branch_arg, bufferSelector_, childIdx, (OctreeNode*)childBranch);
01130                 doNodeReset = true;
01131 
01132               }
01133               else
01134               {
01135                 // if required branch does not exist -> create it
01136                 createBranchChild (*branch_arg, childIdx, childBranch);
01137               }
01138 
01139               branchCount_++;
01140 
01141             }
01142             else
01143             {
01144               // required branch node already exists - use it
01145               childBranch = (OctreeBranch*)getBranchChild (*branch_arg, childIdx);
01146             }
01147 
01148             // recursively proceed with indexed child branch
01149             deserializeTreeRecursive (binaryTreeIn_arg, childBranch, depthMask_arg / 2, newKey,
01150                 dataVectorIterator_arg, dataVectorEndIterator_arg, doNodeReset, doXORDecoding_arg);
01151 
01152           }
01153           else
01154           {
01155             // branch childs are leaf nodes
01156 
01157             OctreeLeaf* childLeaf;
01158 
01159             // check if we can take copy a reference pointer from previous buffer
01160             if (branchHasChild (*branch_arg, !bufferSelector_, childIdx))
01161             {
01162               // take child leaf node from previous buffer
01163               childLeaf = (OctreeLeaf*)getBranchChild (*branch_arg, !bufferSelector_, childIdx);
01164               setBranchChild (*branch_arg, bufferSelector_, childIdx, (OctreeNode*)childLeaf);
01165 
01166               // reset child leaf
01167               childLeaf->reset ();
01168 
01169             }
01170             else
01171             {
01172               // if required leaf does not exist -> create it
01173               createLeafChild (*branch_arg, childIdx, childLeaf);
01174             }
01175 
01176             leafCount_++;
01177 
01178             // execute deserialization callback
01179             deserializeLeafCallback (*childLeaf, newKey, dataVectorIterator_arg, dataVectorEndIterator_arg);
01180 
01181           }
01182         }
01183         else
01184         {
01185 
01186           // remove old branch pointer information in current branch
01187           setBranchChild (*branch_arg, bufferSelector_, childIdx, 0);
01188 
01189           // remove unused branches in previous buffer
01190           deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
01191         }
01192       }
01193     }
01194 
01196     template<typename DataT, typename LeafT>
01197     void
01198     Octree2BufBase<DataT, LeafT>::deserializeTreeAndOutputLeafDataRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg,
01199                                                                              OctreeBranch* branch_arg,
01200                                                                              const unsigned int depthMask_arg,
01201                                                                              const OctreeKey& key_arg,
01202                                                                              typename std::vector<DataT>& dataVector_arg,
01203                                                                              bool branchReset_arg, bool doXORDecoding_arg)
01204     {
01205       // child iterator
01206       unsigned char childIdx;
01207 
01208       // node bits
01209       char nodeBits;
01210       char recoveredNodeBits;
01211 
01212       // branch reset -> this branch has been taken from previous buffer
01213       if (branchReset_arg)
01214       {
01215         // we can safely remove children references
01216         for (childIdx = 0; childIdx < 8; childIdx++)
01217         {
01218           setBranchChild (*branch_arg, childIdx, 0);
01219         }
01220 
01221       }
01222 
01223       // read branch occupancy bit pattern from vector
01224       nodeBits = (*binaryTreeIn_arg);
01225       binaryTreeIn_arg++;
01226 
01227       // recover branch occupancy bit pattern
01228       if (doXORDecoding_arg)
01229       {
01230         recoveredNodeBits = getBranchBitPattern (*branch_arg, !bufferSelector_) ^ nodeBits;
01231       }
01232       else
01233       {
01234         recoveredNodeBits = nodeBits;
01235       }
01236 
01237       // iterate over all children
01238       for (childIdx = 0; childIdx < 8; childIdx++)
01239       {
01240         // if occupancy bit for childIdx is set..
01241         if (recoveredNodeBits & (1 << childIdx))
01242         {
01243 
01244           // generate new key for current branch voxel
01245           OctreeKey newKey;
01246           newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2)));
01247           newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1)));
01248           newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0)));
01249 
01250           bool doNodeReset;
01251 
01252           doNodeReset = false;
01253 
01254           if (depthMask_arg > 1)
01255           {
01256             // we have not reached maximum tree depth
01257             OctreeBranch* childBranch;
01258 
01259             if (!branchHasChild (*branch_arg, childIdx))
01260             {
01261 
01262               // check if we find a branch node reference in previous buffer
01263               if (branchHasChild (*branch_arg, !bufferSelector_, childIdx))
01264               {
01265                 // take child branch from previous buffer
01266                 childBranch = (OctreeBranch*)getBranchChild (*branch_arg, !bufferSelector_, childIdx);
01267                 setBranchChild (*branch_arg, bufferSelector_, childIdx, (OctreeNode*)childBranch);
01268                 doNodeReset = true;
01269 
01270               }
01271               else
01272               {
01273                 // if required branch does not exist -> create it
01274                 createBranchChild (*branch_arg, childIdx, childBranch);
01275               }
01276 
01277               branchCount_++;
01278 
01279             }
01280             else
01281             {
01282               // required branch node already exists - use it
01283               childBranch = (OctreeBranch*)getBranchChild (*branch_arg, childIdx);
01284             }
01285 
01286             // recursively proceed with indexed child branch
01287             deserializeTreeRecursive (binaryTreeIn_arg, childBranch, depthMask_arg / 2, newKey, doNodeReset, doXORDecoding_arg);
01288 
01289           }
01290           else
01291           {
01292             // branch childs are leaf nodes
01293 
01294             OctreeLeaf* childLeaf;
01295 
01296               // check if we can take copy a reference from previous buffer
01297               if (branchHasChild (*branch_arg, !bufferSelector_, childIdx))
01298             {
01299               // take child leaf node from previous buffer
01300               childLeaf = (OctreeLeaf*)getBranchChild (*branch_arg, !bufferSelector_, childIdx);
01301               setBranchChild (*branch_arg, bufferSelector_, childIdx, (OctreeNode*)childLeaf);
01302 
01303               // reset child leaf
01304               childLeaf->reset ();
01305 
01306             }
01307             else
01308             {
01309               // if required leaf does not exist -> create it
01310               createLeafChild (*branch_arg, childIdx, childLeaf);
01311 
01312             }
01313 
01314             // execute deserialization callback
01315             deserializeTreeAndSerializeLeafCallback (*childLeaf, newKey, dataVector_arg);
01316 
01317             leafCount_++;
01318 
01319           }
01320         }
01321         else
01322         {
01323 
01324           // remove old branch pointer information in current branch
01325           setBranchChild (*branch_arg, bufferSelector_, childIdx, 0);
01326 
01327           // remove unused branches in previous buffer
01328           deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
01329         }
01330       }
01331 
01332     }
01333 
01335     template<typename DataT, typename LeafT>
01336       void
01337       Octree2BufBase<DataT, LeafT>::serializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg)
01338       {
01339         // nothing to do
01340       }
01341 
01343     template<typename DataT, typename LeafT>
01344       void
01345       Octree2BufBase<DataT, LeafT>::serializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg,
01346                                                            std::vector<DataT>& dataVector_arg)
01347       {
01348         leaf_arg.getData (dataVector_arg);
01349       }
01350 
01352     template<typename DataT, typename LeafT>
01353       void
01354       Octree2BufBase<DataT, LeafT>::serializeNewLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg,
01355                                                               const int minPointsPerLeaf_arg,
01356                                                               std::vector<DataT>& dataVector_arg)
01357       {
01358         // we reached a leaf node
01359         std::vector<int> newPointIdx;
01360 
01361         if (minPointsPerLeaf_arg != 0)
01362         {
01363           // push to newPointIdx
01364           leaf_arg.getData (newPointIdx);
01365 
01366           // check for minimum amount of leaf point indices
01367           if (newPointIdx.size () >= (size_t)minPointsPerLeaf_arg)
01368           {
01369             dataVector_arg.insert (dataVector_arg.end (), newPointIdx.begin (), newPointIdx.end ());
01370           }
01371         }
01372         else
01373         {
01374           // push leaf data to dataVector_arg
01375           leaf_arg.getData (dataVector_arg);
01376         }
01377       }
01378 
01380     template<typename DataT, typename LeafT>
01381       void
01382       Octree2BufBase<DataT, LeafT>::deserializeLeafCallback (
01383                                                              OctreeLeaf& leaf_arg,
01384                                                              const OctreeKey& key_arg,
01385                                                              typename std::vector<DataT>::const_iterator& dataVectorIterator_arg,
01386                                                              typename std::vector<DataT>::const_iterator& dataVectorEndIterator_arg)
01387       {
01388         OctreeKey dataKey;
01389         bool bKeyBasedEncoding = false;
01390 
01391         // add DataT objects to octree leaf as long as their key fit to voxel
01392         while ((dataVectorIterator_arg != dataVectorEndIterator_arg)
01393             && (this->genOctreeKeyForDataT (*dataVectorIterator_arg, dataKey) && (dataKey == key_arg)))
01394         {
01395           leaf_arg.setData (*dataVectorIterator_arg);
01396           dataVectorIterator_arg++;
01397           bKeyBasedEncoding = true;
01398         }
01399 
01400         // add single DataT object to octree if key-based encoding is disabled
01401         if (!bKeyBasedEncoding)
01402         {
01403           leaf_arg.setData (*dataVectorIterator_arg);
01404           dataVectorIterator_arg++;
01405         }
01406       }
01407 
01409     template<typename DataT, typename LeafT>
01410       void
01411       Octree2BufBase<DataT, LeafT>::deserializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg)
01412       {
01413 
01414         DataT newDataT;
01415 
01416         // initialize new leaf child
01417         if (genDataTByOctreeKey (key_arg, newDataT))
01418         {
01419           leaf_arg.setData (newDataT);
01420         }
01421 
01422       }
01423 
01425     template<typename DataT, typename LeafT>
01426       void
01427       Octree2BufBase<DataT, LeafT>::deserializeTreeAndSerializeLeafCallback (OctreeLeaf& leaf_arg,
01428                                                                              const OctreeKey& key_arg,
01429                                                                              std::vector<DataT>& dataVector_arg)
01430       {
01431 
01432         DataT newDataT;
01433 
01434         // initialize new leaf child
01435         if (genDataTByOctreeKey (key_arg, newDataT))
01436         {
01437           leaf_arg.setData (newDataT);
01438           dataVector_arg.push_back (newDataT);
01439         }
01440       }
01441 
01443     template<typename DataT, typename LeafT>
01444       void
01445       Octree2BufBase<DataT, LeafT>::treeCleanUpRecursive (OctreeBranch* branch_arg)
01446       {
01447 
01448         // child iterator
01449         unsigned char childIdx;
01450 
01451         // bit pattern
01452         char nodeBitPatternLastBuffer;
01453         char nodeXORBitPattern;
01454         char unusedBranchesBits;
01455 
01456         // occupancy bit pattern of branch node  (previous octree buffer)
01457         nodeBitPatternLastBuffer = getBranchBitPattern (*branch_arg, !bufferSelector_);
01458 
01459         // XOR of current and previous occupancy bit patterns
01460         nodeXORBitPattern = getBranchXORBitPattern (*branch_arg);
01461 
01462         // bit pattern indicating unused octree nodes in previous branch
01463         unusedBranchesBits = nodeXORBitPattern & nodeBitPatternLastBuffer;
01464 
01465         // iterate over all children
01466         for (childIdx = 0; childIdx < 8; childIdx++)
01467         {
01468           if (branchHasChild (*branch_arg, childIdx))
01469           {
01470             const OctreeNode * childNode;
01471             childNode = getBranchChild (*branch_arg, childIdx);
01472 
01473             switch (childNode->getNodeType ())
01474             {
01475               case BRANCH_NODE:
01476                 // recursively proceed with indexed child branch
01477                 treeCleanUpRecursive ((OctreeBranch*)childNode);
01478                 break;
01479               case LEAF_NODE:
01480                 // leaf level - nothing to do..
01481                 break;
01482             }
01483 
01484           }
01485 
01486           // check for unused branches in previous buffer
01487           if (unusedBranchesBits & (1 << childIdx))
01488           {
01489             // delete branch, free memory
01490             deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
01491           }
01492 
01493         }
01494       }
01495 
01496   }
01497 }
01498 
01499 #define PCL_INSTANTIATE_Octree2BufBase(T) template class PCL_EXPORTS pcl::octree::Octree2BufBase<T>;
01500 
01501 #endif
01502 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines