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_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