Point Cloud Library (PCL)  1.3.1
octree_lowmemory_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: octree_lowmemory_base.hpp 3017 2011-11-01 03:24:04Z rusu $
00037  */
00038 
00039 #ifndef OCTREE_LOWMEM_TREE_BASE_HPP
00040 #define OCTREE_LOWMEM_TREE_BASE_HPP
00041 
00042 #include <vector>
00043 
00044 #include "pcl/impl/instantiate.hpp"
00045 #include "pcl/point_types.h"
00046 #include "pcl/octree/octree.h"
00047 
00048 namespace pcl
00049 {
00050   namespace octree
00051   {
00052 
00053     using namespace std;
00054 
00056     template<typename DataT, typename LeafT>
00057       OctreeLowMemBase<DataT, LeafT>::OctreeLowMemBase ()
00058       {
00059 
00060         // Initialization of globals
00061         rootNode_ = new OctreeBranch ();
00062         leafCount_ = 0;
00063         depthMask_ = 0;
00064         branchCount_ = 1;
00065         objectCount_ = 0;
00066         octreeDepth_ = 0;
00067 
00068       }
00069 
00071     template<typename DataT, typename LeafT>
00072       OctreeLowMemBase<DataT, LeafT>::~OctreeLowMemBase ()
00073       {
00074 
00075         // deallocate tree structure
00076         deleteTree ();
00077         delete (rootNode_);
00078       }
00079 
00081     template<typename DataT, typename LeafT>
00082       void
00083       OctreeLowMemBase<DataT, LeafT>::setMaxVoxelIndex (unsigned int maxVoxelIndex_arg)
00084       {
00085         unsigned int treeDepth;
00086 
00087         assert (maxVoxelIndex_arg>0);
00088 
00089         // tree depth == amount of bits of maxVoxels
00090         treeDepth = max ((min ((unsigned int)OCT_MAXTREEDEPTH, (unsigned int)ceil (Log2 (maxVoxelIndex_arg)))),
00091                          (unsigned int)0);
00092 
00093         // define depthMask_ by setting a single bit to 1 at bit position == tree depth
00094         depthMask_ = (1 << (treeDepth - 1));
00095 
00096       }
00097 
00099     template<typename DataT, typename LeafT>
00100       void
00101       OctreeLowMemBase<DataT, LeafT>::setTreeDepth (unsigned int depth_arg)
00102       {
00103 
00104         assert (depth_arg>0);
00105 
00106         // set octree depth
00107         octreeDepth_ = depth_arg;
00108 
00109         // define depthMask_ by setting a single bit to 1 at bit position == tree depth
00110         depthMask_ = (1 << (depth_arg - 1));
00111 
00112       }
00113 
00115     template<typename DataT, typename LeafT>
00116       void
00117       OctreeLowMemBase<DataT, LeafT>::add (const unsigned int idxX_arg, const unsigned int idxY_arg,
00118                                      const unsigned int idxZ_arg, const DataT& data_arg)
00119       {
00120 
00121         OctreeKey key;
00122 
00123         // generate key
00124         genOctreeKeyByIntIdx (idxX_arg, idxY_arg, idxZ_arg, key);
00125 
00126         // add data_arg to octree
00127         add (key, data_arg);
00128 
00129         objectCount_++;
00130 
00131       }
00132 
00133 
00134 
00136     template<typename DataT, typename LeafT>
00137       bool
00138       OctreeLowMemBase<DataT, LeafT>::get (const unsigned int idxX_arg, const unsigned int idxY_arg,
00139                                      const unsigned int idxZ_arg, DataT& data_arg) const
00140       {
00141         OctreeKey key;
00142 
00143         // generate key
00144         genOctreeKeyByIntIdx (idxX_arg, idxY_arg, idxZ_arg, key);
00145 
00146         // search for leaf at key
00147         LeafT* leaf = findLeaf (key);
00148         if (leaf)
00149         {
00150           const DataT * dataPtr;
00151           // if successful, decode data to data_arg
00152           leaf->getData (dataPtr);
00153           if (dataPtr)
00154             data_arg = *dataPtr;
00155         }
00156 
00157         // returns true on success
00158         return (leaf != 0);
00159       }
00160 
00162     template<typename DataT, typename LeafT>
00163       bool
00164       OctreeLowMemBase<DataT, LeafT>::existLeaf (const unsigned int idxX_arg, const unsigned int idxY_arg,
00165                                            const unsigned int idxZ_arg) const
00166       {
00167         OctreeKey key;
00168 
00169         // generate key
00170         this->genOctreeKeyByIntIdx (idxX_arg, idxY_arg, idxZ_arg, key);
00171 
00172         // check if key exist in octree
00173         return existLeaf (key);
00174       }
00175 
00177     template<typename DataT, typename LeafT>
00178       void
00179       OctreeLowMemBase<DataT, LeafT>::removeLeaf (const unsigned int idxX_arg, const unsigned int idxY_arg,
00180                                             const unsigned int idxZ_arg)
00181       {
00182         OctreeKey key;
00183 
00184         // generate key
00185         this->genOctreeKeyByIntIdx (idxX_arg, idxY_arg, idxZ_arg, key);
00186 
00187         // check if key exist in octree
00188         deleteLeafRecursive (key, depthMask_, rootNode_);
00189       }
00190 
00192     template<typename DataT, typename LeafT>
00193       void
00194       OctreeLowMemBase<DataT, LeafT>::deleteTree ( )
00195       {
00196 
00197         if (rootNode_)
00198         {
00199           // reset octree
00200           deleteBranch (*rootNode_);
00201           leafCount_ = 0;
00202           branchCount_ = 1;
00203           objectCount_ = 0;
00204 
00205         }
00206 
00207       }
00208 
00210     template<typename DataT, typename LeafT>
00211       void
00212       OctreeLowMemBase<DataT, LeafT>::serializeTree (std::vector<char>& binaryTreeOut_arg,
00213                                                      bool doXOREncoding_arg)
00214       {
00215         OctreeKey newKey;
00216         newKey.x = newKey.y = newKey.z = 0;
00217 
00218         // clear binary vector
00219         binaryTreeOut_arg.clear ();
00220         binaryTreeOut_arg.reserve (this->branchCount_);
00221 
00222         serializeTreeRecursive (binaryTreeOut_arg, rootNode_, newKey);
00223       }
00224 
00226     template<typename DataT, typename LeafT>
00227       void
00228       OctreeLowMemBase<DataT, LeafT>::serializeTree (std::vector<char>& binaryTreeOut_arg,
00229                                                      std::vector<DataT>& dataVector_arg, bool doXOREncoding_arg)
00230       {
00231         OctreeKey newKey;
00232         newKey.x = newKey.y = newKey.z = 0;
00233 
00234         // clear output vectors
00235         binaryTreeOut_arg.clear ();
00236         dataVector_arg.clear ();
00237 
00238         dataVector_arg.reserve (this->objectCount_);
00239         binaryTreeOut_arg.reserve (this->branchCount_);
00240 
00241         OctreeLowMemBase<DataT, LeafT>::serializeTreeRecursive (binaryTreeOut_arg, rootNode_, newKey, dataVector_arg);
00242       }
00243 
00245     template<typename DataT, typename LeafT>
00246       void
00247       OctreeLowMemBase<DataT, LeafT>::serializeLeafs (std::vector<DataT>& dataVector_arg)
00248       {
00249 
00250         OctreeKey newKey;
00251         newKey.x = newKey.y = newKey.z = 0;
00252 
00253         // clear output vector
00254         dataVector_arg.clear ();
00255 
00256         dataVector_arg.reserve(this->objectCount_);
00257 
00258         serializeLeafsRecursive (rootNode_, newKey, dataVector_arg);
00259       }
00260 
00262     template<typename DataT, typename LeafT>
00263       void
00264       OctreeLowMemBase<DataT, LeafT>::deserializeTree (std::vector<char>& binaryTreeIn_arg,
00265                                                        bool doXORDecoding_arg)
00266       {
00267 
00268         OctreeKey newKey;
00269         newKey.x = newKey.y = newKey.z = 0;
00270 
00271         // free existing tree before tree rebuild
00272         deleteTree ();
00273 
00274         //iterator for binary tree structure vector
00275         vector<char>::const_iterator binaryTreeVectorIterator = binaryTreeIn_arg.begin ();
00276 
00277         deserializeTreeRecursive (binaryTreeVectorIterator, rootNode_, depthMask_, newKey);
00278 
00279         objectCount_ = this->leafCount_;
00280       }
00281 
00283     template<typename DataT, typename LeafT>
00284       void
00285       OctreeLowMemBase<DataT, LeafT>::deserializeTree (std::vector<char>& binaryTreeIn_arg,
00286                                                        std::vector<DataT>& dataVector_arg,
00287                                                        bool doXORDecoding_arg)
00288       {
00289         OctreeKey newKey;
00290         newKey.x = newKey.y = newKey.z = 0;
00291 
00292         // set data iterator to first element
00293         typename std::vector<DataT>::const_iterator dataVectorIterator = dataVector_arg.begin ();
00294 
00295         // set data iterator to last element
00296         typename std::vector<DataT>::const_iterator dataVectorEndIterator = dataVector_arg.end ();
00297 
00298         // free existing tree before tree rebuild
00299         deleteTree ();
00300 
00301         //iterator for binary tree structure vector
00302         vector<char>::const_iterator binaryTreeVectorIterator = binaryTreeIn_arg.begin ();
00303 
00304         deserializeTreeRecursive (binaryTreeVectorIterator, rootNode_, depthMask_, newKey, dataVectorIterator,
00305                                   dataVectorEndIterator);
00306 
00307         objectCount_ = dataVector_arg.size();
00308       }
00309 
00311     template<typename DataT, typename LeafT>
00312       void
00313       OctreeLowMemBase<DataT, LeafT>::deserializeTreeAndOutputLeafData (std::vector<char>& binaryTreeIn_arg,
00314                                                                   std::vector<DataT>& dataVector_arg)
00315       {
00316 
00317         OctreeKey newKey;
00318         newKey.x = newKey.y = newKey.z = 0;
00319 
00320         // free existing tree before tree rebuild
00321         deleteTree ();
00322 
00323         //iterator for binary tree structure vector
00324         vector<char>::const_iterator binaryTreeVectorIterator = binaryTreeIn_arg.begin ();
00325 
00326         deserializeTreeAndOutputLeafDataRecursive (binaryTreeVectorIterator, rootNode_, depthMask_, newKey,
00327                                                    dataVector_arg);
00328 
00329         objectCount_ = dataVector_arg.size();
00330       }
00331 
00333     template<typename DataT, typename LeafT>
00334       LeafT*
00335       OctreeLowMemBase<DataT, LeafT>::getLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg,
00336                                                   OctreeBranch* branch_arg)
00337       {
00338 
00339         // index to branch child
00340         unsigned char childIdx;
00341         LeafT* result = 0;
00342 
00343         // find branch child from key
00344         childIdx = ((!!(key_arg.x & depthMask_arg)) << 2) | ((!!(key_arg.y & depthMask_arg)) << 1) | (!!(key_arg.z
00345             & depthMask_arg));
00346 
00347         if (depthMask_arg > 1)
00348         {
00349           // we have not reached maximum tree depth
00350 
00351           OctreeBranch* childBranch;
00352           if (!branchHasChild (*branch_arg, childIdx))
00353           {
00354 
00355             // if required branch does not exist -> create it
00356             createBranchChild (*branch_arg, childIdx, childBranch);
00357 
00358             branchCount_++;
00359 
00360           }
00361           else
00362           {
00363 
00364             // required branch exists
00365             childBranch = (OctreeBranch*)getBranchChild (*branch_arg, childIdx);
00366           }
00367 
00368           // recursively proceed with indexed child branch
00369           result = getLeafRecursive (key_arg, depthMask_arg / 2, childBranch);
00370 
00371         }
00372         else
00373         {
00374           // branch childs are leaf nodes
00375 
00376           OctreeLeaf* childLeaf;
00377           if (!branchHasChild (*branch_arg, childIdx))
00378           {
00379 
00380             // if leaf node at childIdx does not exist
00381             createLeafChild (*branch_arg, childIdx, childLeaf);
00382             leafCount_++;
00383 
00384             // return leaf node
00385             result = childLeaf;
00386 
00387           }
00388           else
00389           {
00390 
00391             // leaf node already exist
00392             childLeaf = (OctreeLeaf*)getBranchChild (*branch_arg, childIdx);
00393 
00394             // return leaf node
00395             result = childLeaf;
00396 
00397           }
00398 
00399         }
00400 
00401         return result;
00402 
00403       }
00404 
00406     template<typename DataT, typename LeafT>
00407       LeafT*
00408       OctreeLowMemBase<DataT, LeafT>::findLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg,
00409                                                    OctreeBranch* branch_arg) const
00410       {
00411         // index to branch child
00412         unsigned char childIdx;
00413         LeafT* result = 0;
00414 
00415         // find branch child from key
00416         childIdx = ((!!(key_arg.x & depthMask_arg)) << 2) | ((!!(key_arg.y & depthMask_arg)) << 1) | (!!(key_arg.z
00417             & depthMask_arg));
00418 
00419         if (depthMask_arg > 1)
00420         {
00421           // we have not reached maximum tree depth
00422           OctreeBranch* childBranch;
00423           childBranch = (OctreeBranch*)getBranchChild (*branch_arg, childIdx);
00424 
00425           if (childBranch)
00426             // recursively proceed with indexed child branch
00427             result = findLeafRecursive (key_arg, depthMask_arg / 2, childBranch);
00428 
00429         }
00430         else
00431         {
00432           // we reached leaf node level
00433           if (branchHasChild (*branch_arg, childIdx))
00434           {
00435 
00436             // return existing leaf node
00437             OctreeLeaf* childLeaf = (OctreeLeaf*)getBranchChild (*branch_arg, childIdx);
00438             result = childLeaf;
00439           }
00440 
00441         }
00442 
00443         return result;
00444 
00445       }
00446 
00448     template<typename DataT, typename LeafT>
00449       bool
00450       OctreeLowMemBase<DataT, LeafT>::deleteLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg,
00451                                                      OctreeBranch* branch_arg)
00452       {
00453         // index to branch child
00454         unsigned char childIdx;
00455         // indicates if branch is empty and can be safely removed
00456         bool bNoChilds;
00457 
00458         // find branch child from key
00459         childIdx = ((!!(key_arg.x & depthMask_arg)) << 2) | ((!!(key_arg.y & depthMask_arg)) << 1) | (!!(key_arg.z
00460             & depthMask_arg));
00461 
00462         if (depthMask_arg > 1)
00463         {
00464           // we have not reached maximum tree depth
00465 
00466           OctreeBranch* childBranch;
00467           bool bBranchOccupied;
00468 
00469           // next branch child on our path through the tree
00470           childBranch = (OctreeBranch*)getBranchChild (*branch_arg, childIdx);
00471 
00472           if (childBranch)
00473           {
00474             // recursively explore the indexed child branch
00475             bBranchOccupied = deleteLeafRecursive (key_arg, depthMask_arg / 2, childBranch);
00476 
00477             if (!bBranchOccupied)
00478             {
00479               // child branch does not own any sub-child nodes anymore -> delete child branch
00480               delete (childBranch);
00481               setBranchChild (*branch_arg, childIdx, 0);
00482               branchCount_--;
00483             }
00484           }
00485 
00486         }
00487         else
00488         {
00489 
00490           // our child is a leaf node -> delete it
00491           deleteBranchChild (*branch_arg, childIdx);
00492           leafCount_--;
00493         }
00494 
00495         // check if current branch still owns childs
00496         bNoChilds = false;
00497         for (childIdx = 0; childIdx < 8; childIdx++)
00498         {
00499           bNoChilds = branchHasChild (*branch_arg, childIdx);
00500           if (bNoChilds)
00501             break;
00502         }
00503 
00504         // return true if current branch can be deleted
00505         return bNoChilds;
00506 
00507       }
00508 
00510     template<typename DataT, typename LeafT>
00511       void
00512       OctreeLowMemBase<DataT, LeafT>::serializeTreeRecursive (std::vector<char>& binaryTreeOut_arg,
00513                                                         const OctreeBranch* branch_arg, const OctreeKey& key_arg)
00514       {
00515 
00516         // child iterator
00517         unsigned char childIdx;
00518         char nodeBitPattern;
00519 
00520         // branch occupancy bit pattern
00521         nodeBitPattern = getBranchBitPattern (*branch_arg);
00522 
00523         // write bit pattern to output vector
00524         binaryTreeOut_arg.push_back (nodeBitPattern);
00525 
00526         // iterate over all children
00527         for (childIdx = 0; childIdx < 8; childIdx++)
00528         {
00529 
00530           // if child exist
00531           if (branchHasChild (*branch_arg, childIdx))
00532           {
00533 
00534             // generate new key for current branch voxel
00535             OctreeKey newKey;
00536             newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2)));
00537             newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1)));
00538             newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0)));
00539 
00540             const OctreeNode * childNode;
00541             childNode = getBranchChild (*branch_arg, childIdx);
00542 
00543             switch (childNode->getNodeType ())
00544             {
00545               case BRANCH_NODE:
00546 
00547                 // recursively proceed with indexed child branch
00548                 serializeTreeRecursive (binaryTreeOut_arg, (OctreeBranch*)childNode, newKey);
00549                 break;
00550 
00551               case LEAF_NODE:
00552                 OctreeLeaf* childLeaf = (OctreeLeaf*)childNode;
00553 
00554                 // we reached a leaf node -> execute serialization callback
00555                 serializeLeafCallback (*childLeaf, newKey);
00556                 break;
00557             }
00558 
00559           }
00560 
00561         }
00562 
00563       }
00564 
00566     template<typename DataT, typename LeafT>
00567       void
00568       OctreeLowMemBase<DataT, LeafT>::serializeTreeRecursive (std::vector<char>& binaryTreeOut_arg,
00569                                                         const OctreeBranch* branch_arg, const OctreeKey& key_arg,
00570                                                         typename std::vector<DataT>& dataVector_arg)
00571       {
00572 
00573         // child iterator
00574         unsigned char childIdx;
00575         char nodeBitPattern;
00576 
00577         // branch occupancy bit pattern
00578         nodeBitPattern = getBranchBitPattern (*branch_arg);
00579 
00580         // write bit pattern to output vector
00581         binaryTreeOut_arg.push_back (nodeBitPattern);
00582 
00583         // iterate over all children
00584         for (childIdx = 0; childIdx < 8; childIdx++)
00585         {
00586 
00587           // if child exist
00588           if (branchHasChild (*branch_arg, childIdx))
00589           {
00590 
00591             // generate new key for current branch voxel
00592             OctreeKey newKey;
00593             newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2)));
00594             newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1)));
00595             newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0)));
00596 
00597             const OctreeNode * childNode;
00598             childNode = getBranchChild (*branch_arg, childIdx);
00599 
00600             switch (childNode->getNodeType ())
00601             {
00602               case BRANCH_NODE:
00603 
00604                 // recursively proceed with indexed child branch
00605                 serializeTreeRecursive (binaryTreeOut_arg, (OctreeBranch*)childNode, newKey, dataVector_arg);
00606                 break;
00607 
00608               case LEAF_NODE:
00609                 OctreeLeaf* childLeaf = (OctreeLeaf*)childNode;
00610 
00611                 // we reached a leaf node -> execute serialization callback
00612                 serializeLeafCallback (*childLeaf, newKey, dataVector_arg);
00613                 break;
00614             }
00615 
00616           }
00617 
00618         }
00619 
00620       }
00621 
00623     template<typename DataT, typename LeafT>
00624       void
00625       OctreeLowMemBase<DataT, LeafT>::serializeLeafsRecursive (const OctreeBranch* branch_arg, const OctreeKey& key_arg,
00626                                                          std::vector<DataT>& dataVector_arg)
00627       {
00628         // child iterator
00629         unsigned char childIdx;
00630 
00631         // iterate over all children
00632         for (childIdx = 0; childIdx < 8; childIdx++)
00633         {
00634 
00635           // if child exist
00636           if (branchHasChild (*branch_arg, childIdx))
00637           {
00638             const OctreeNode * childNode;
00639             childNode = getBranchChild (*branch_arg, childIdx);
00640 
00641             // generate new key for current branch voxel
00642             OctreeKey newKey;
00643             newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2)));
00644             newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1)));
00645             newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0)));
00646 
00647             switch (childNode->getNodeType ())
00648             {
00649               case BRANCH_NODE:
00650 
00651                 // recursively proceed with indexed child branch
00652                 serializeLeafsRecursive ((OctreeBranch*)childNode, newKey, dataVector_arg);
00653                 break;
00654 
00655               case LEAF_NODE:
00656                 OctreeLeaf* childLeaf = (OctreeLeaf*)childNode;
00657 
00658                 // we reached a leaf node -> execute serialization callback
00659                 serializeLeafCallback (*childLeaf, newKey, dataVector_arg);
00660                 break;
00661             }
00662 
00663           }
00664 
00665         }
00666       }
00667 
00669     template<typename DataT, typename LeafT>
00670       void
00671       OctreeLowMemBase<DataT, LeafT>::deserializeTreeRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg,
00672                                                           OctreeBranch* branch_arg, const unsigned int depthMask_arg,
00673                                                           const OctreeKey& key_arg)
00674       {
00675         // child iterator
00676         unsigned char childIdx;
00677         char nodeBits;
00678 
00679         // read branch occupancy bit pattern from input vector
00680         nodeBits = (*binaryTreeIn_arg);
00681         binaryTreeIn_arg++;
00682 
00683         // iterate over all children
00684         for (childIdx = 0; childIdx < 8; childIdx++)
00685         {
00686           // if occupancy bit for childIdx is set..
00687           if (nodeBits & (1 << childIdx))
00688           {
00689 
00690             // generate new key for current branch voxel
00691             OctreeKey newKey;
00692             newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2)));
00693             newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1)));
00694             newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0)));
00695 
00696             if (depthMask_arg > 1)
00697             {
00698               // we have not reached maximum tree depth
00699               OctreeBranch * newBranch;
00700 
00701               // create new child branch
00702               createBranchChild (*branch_arg, childIdx, newBranch);
00703 
00704               branchCount_++;
00705 
00706               // recursively proceed with new child branch
00707               deserializeTreeRecursive (binaryTreeIn_arg, newBranch, depthMask_arg / 2, newKey);
00708 
00709             }
00710             else
00711             {
00712               // we reached leaf node level
00713               OctreeLeaf* childLeaf;
00714 
00715               // create leaf node
00716               createLeafChild (*branch_arg, childIdx, childLeaf);
00717 
00718               // execute deserialization callback
00719               deserializeLeafCallback (*childLeaf, newKey);
00720 
00721               leafCount_++;
00722             }
00723           }
00724         }
00725 
00726       }
00727 
00729     template<typename DataT, typename LeafT>
00730       void
00731       OctreeLowMemBase<DataT, LeafT>::deserializeTreeRecursive (
00732                                                           typename std::vector<char>::const_iterator& binaryTreeIn_arg,
00733                                                           OctreeBranch* branch_arg,
00734                                                           const unsigned int depthMask_arg,
00735                                                           const OctreeKey& key_arg,
00736                                                           typename std::vector<DataT>::const_iterator& dataVectorIterator_arg,
00737                                                           typename std::vector<DataT>::const_iterator& dataVectorEndIterator_arg)
00738       {
00739         // child iterator
00740         unsigned char childIdx;
00741         char nodeBits;
00742 
00743         // read branch occupancy bit pattern from input vector
00744         nodeBits = (*binaryTreeIn_arg);
00745         binaryTreeIn_arg++;
00746 
00747         // iterate over all children
00748         for (childIdx = 0; childIdx < 8; childIdx++)
00749         {
00750           // if occupancy bit for childIdx is set..
00751           if (nodeBits & (1 << childIdx))
00752           {
00753             // generate new key for current branch voxel
00754             OctreeKey newKey;
00755             newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2)));
00756             newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1)));
00757             newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0)));
00758 
00759             if (depthMask_arg > 1)
00760             {
00761               // we have not reached maximum tree depth
00762               OctreeBranch * newBranch;
00763 
00764               // create new child branch
00765               createBranchChild (*branch_arg, childIdx, newBranch);
00766 
00767               branchCount_++;
00768 
00769               // recursively proceed with new child branch
00770               deserializeTreeRecursive (binaryTreeIn_arg, newBranch, depthMask_arg / 2, newKey, dataVectorIterator_arg,
00771                                         dataVectorEndIterator_arg);
00772 
00773             }
00774             else
00775             {
00776               // we reached leaf node level
00777 
00778               OctreeLeaf* childLeaf;
00779 
00780               // create leaf node
00781               createLeafChild (*branch_arg, childIdx, childLeaf);
00782 
00783               // execute deserialization callback
00784               deserializeLeafCallback (*childLeaf, newKey, dataVectorIterator_arg, dataVectorEndIterator_arg);
00785 
00786               leafCount_++;
00787             }
00788           }
00789         }
00790       }
00791 
00793     template<typename DataT, typename LeafT>
00794       void
00795       OctreeLowMemBase<DataT, LeafT>::deserializeTreeAndOutputLeafDataRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg,
00796                                                                            OctreeBranch* branch_arg,
00797                                                                            const unsigned int depthMask_arg,
00798                                                                            const OctreeKey& key_arg,
00799                                                                            std::vector<DataT>& dataVector_arg)
00800       {
00801         // child iterator
00802         unsigned char childIdx;
00803         char nodeBits;
00804 
00805         // read branch occupancy bit pattern from input vector
00806         nodeBits = (*binaryTreeIn_arg);
00807         binaryTreeIn_arg++;
00808 
00809         // iterate over all children
00810         for (childIdx = 0; childIdx < 8; childIdx++)
00811         {
00812           // if occupancy bit for childIdx is set..
00813           if (nodeBits & (1 << childIdx))
00814           {
00815 
00816             // generate new key for current branch voxel
00817             OctreeKey newKey;
00818             newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2)));
00819             newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1)));
00820             newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0)));
00821 
00822             if (depthMask_arg > 1)
00823             {
00824               // we have not reached maximum tree depth
00825               OctreeBranch * newBranch;
00826 
00827               // create new child branch
00828               createBranchChild (*branch_arg, childIdx, newBranch);
00829 
00830               branchCount_++;
00831 
00832               // recursively proceed with new child branch
00833               deserializeTreeAndOutputLeafDataRecursive (binaryTreeIn_arg, newBranch, depthMask_arg / 2, newKey,
00834                                                          dataVector_arg);
00835 
00836             }
00837             else
00838             {
00839               // we reached leaf node level
00840               OctreeLeaf* childLeaf;
00841 
00842               // create leaf node
00843               createLeafChild (*branch_arg, childIdx, childLeaf);
00844 
00845               // execute deserialization callback
00846               deserializeTreeAndSerializeLeafCallback (*childLeaf, newKey, dataVector_arg);
00847 
00848               leafCount_++;
00849             }
00850           }
00851         }
00852 
00853       }
00854 
00856     template<typename DataT, typename LeafT>
00857       void
00858       OctreeLowMemBase<DataT, LeafT>::serializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg)
00859       {
00860         // nothing to do
00861       }
00862 
00863 
00865     template<typename DataT, typename LeafT>
00866       void
00867       OctreeLowMemBase<DataT, LeafT>::serializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg,
00868                                                        std::vector<DataT>& dataVector_arg)
00869       {
00870         leaf_arg.getData (dataVector_arg);
00871       }
00872 
00874     template<typename DataT, typename LeafT>
00875       void
00876       OctreeLowMemBase<DataT, LeafT>::deserializeLeafCallback (
00877                                                          OctreeLeaf& leaf_arg,
00878                                                          const OctreeKey& key_arg,
00879                                                          typename std::vector<DataT>::const_iterator& dataVectorIterator_arg,
00880                                                          typename std::vector<DataT>::const_iterator& dataVectorEndIterator_arg)
00881       {
00882         OctreeKey dataKey;
00883         bool bKeyBasedEncoding = false;
00884 
00885         // add DataT objects to octree leaf as long as their key fit to voxel
00886         while ((dataVectorIterator_arg != dataVectorEndIterator_arg)
00887             && (this->genOctreeKeyForDataT (*dataVectorIterator_arg, dataKey) && (dataKey == key_arg)))
00888         {
00889           leaf_arg.setData (*dataVectorIterator_arg);
00890           dataVectorIterator_arg++;
00891           bKeyBasedEncoding = true;
00892         }
00893 
00894         // add single DataT object to octree if key-based encoding is disabled
00895         if (!bKeyBasedEncoding)
00896         {
00897           leaf_arg.setData (*dataVectorIterator_arg);
00898           dataVectorIterator_arg++;
00899         }
00900       }
00901 
00903     template<typename DataT, typename LeafT>
00904       void
00905       OctreeLowMemBase<DataT, LeafT>::deserializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg)
00906       {
00907 
00908         DataT newDataT;
00909 
00910         // initialize new leaf child
00911         if (genDataTByOctreeKey (key_arg, newDataT))
00912         {
00913           leaf_arg.setData (newDataT);
00914         }
00915 
00916       }
00917 
00919     template<typename DataT, typename LeafT>
00920       void
00921       OctreeLowMemBase<DataT, LeafT>::deserializeTreeAndSerializeLeafCallback (OctreeLeaf& leaf_arg,
00922                                                                          const OctreeKey& key_arg,
00923                                                                          std::vector<DataT>& dataVector_arg)
00924       {
00925 
00926         DataT newDataT;
00927 
00928         // initialize new leaf child
00929         if (genDataTByOctreeKey (key_arg, newDataT))
00930         {
00931           leaf_arg.setData (newDataT);
00932           dataVector_arg.push_back (newDataT);
00933         }
00934       }
00935 
00936   }
00937 }
00938 
00939 #define PCL_INSTANTIATE_OctreeLowMemBase(T) template class PCL_EXPORTS pcl::octree::OctreeLowMemBase<T>;
00940 
00941 #endif
00942 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines