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