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