Point Cloud Library (PCL)
1.3.1
|
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