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