|
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: octree_base.hpp 4702 2012-02-23 09:39:33Z gedikli $ 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, typename BranchT> 00057 OctreeBase<DataT, LeafT, BranchT>::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, typename BranchT> 00072 OctreeBase<DataT, LeafT, BranchT>::~OctreeBase () 00073 { 00074 00075 // deallocate tree structure 00076 deleteTree (); 00077 delete (rootNode_); 00078 poolCleanUp (); 00079 } 00080 00082 template<typename DataT, typename LeafT, typename BranchT> 00083 void 00084 OctreeBase<DataT, LeafT, BranchT>::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, typename BranchT> 00101 void 00102 OctreeBase<DataT, LeafT, BranchT>::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, typename BranchT> 00117 void 00118 OctreeBase<DataT, LeafT, BranchT>::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, typename BranchT> 00138 bool 00139 OctreeBase<DataT, LeafT, BranchT>::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, typename BranchT> 00164 bool 00165 OctreeBase<DataT, LeafT, BranchT>::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, typename BranchT> 00179 void 00180 OctreeBase<DataT, LeafT, BranchT>::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, typename BranchT> 00194 void 00195 OctreeBase<DataT, LeafT, BranchT>::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, typename BranchT> 00216 void 00217 OctreeBase<DataT, LeafT, BranchT>::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, typename BranchT> 00231 void 00232 OctreeBase<DataT, LeafT, BranchT>::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, BranchT>::serializeTreeRecursive (binaryTreeOut_arg, rootNode_, newKey, dataVector_arg); 00245 } 00246 00248 template<typename DataT, typename LeafT, typename BranchT> 00249 void 00250 OctreeBase<DataT, LeafT, BranchT>::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, typename BranchT> 00266 void 00267 OctreeBase<DataT, LeafT, BranchT>::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, typename BranchT> 00286 void 00287 OctreeBase<DataT, LeafT, BranchT>::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_ = (unsigned int)dataVector_arg.size(); 00309 } 00310 00312 template<typename DataT, typename LeafT, typename BranchT> 00313 void 00314 OctreeBase<DataT, LeafT, BranchT>::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_ = (unsigned int)dataVector_arg.size(); 00331 } 00332 00334 template<typename DataT, typename LeafT, typename BranchT> 00335 LeafT* 00336 OctreeBase<DataT, LeafT, BranchT>::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, typename BranchT> 00408 LeafT* 00409 OctreeBase<DataT, LeafT, BranchT>::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, typename BranchT> 00450 bool 00451 OctreeBase<DataT, LeafT, BranchT>::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, typename BranchT> 00512 void 00513 OctreeBase<DataT, LeafT, BranchT>::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 default: 00547 break; 00548 00549 case BRANCH_NODE: 00550 00551 // recursively proceed with indexed child branch 00552 serializeTreeRecursive (binaryTreeOut_arg, (OctreeBranch*)childNode, newKey); 00553 break; 00554 00555 case LEAF_NODE: 00556 OctreeLeaf* childLeaf = (OctreeLeaf*)childNode; 00557 00558 // we reached a leaf node -> execute serialization callback 00559 serializeLeafCallback (*childLeaf, newKey); 00560 break; 00561 } 00562 00563 } 00564 00565 } 00566 00567 } 00568 00570 template<typename DataT, typename LeafT, typename BranchT> 00571 void 00572 OctreeBase<DataT, LeafT, BranchT>::serializeTreeRecursive (std::vector<char>& binaryTreeOut_arg, 00573 const OctreeBranch* branch_arg, const OctreeKey& key_arg, 00574 typename std::vector<DataT>& dataVector_arg) 00575 { 00576 00577 // child iterator 00578 unsigned char childIdx; 00579 char nodeBitPattern; 00580 00581 // branch occupancy bit pattern 00582 nodeBitPattern = getBranchBitPattern (*branch_arg); 00583 00584 // write bit pattern to output vector 00585 binaryTreeOut_arg.push_back (nodeBitPattern); 00586 00587 // iterate over all children 00588 for (childIdx = 0; childIdx < 8; childIdx++) 00589 { 00590 00591 // if child exist 00592 if (branchHasChild (*branch_arg, childIdx)) 00593 { 00594 00595 // generate new key for current branch voxel 00596 OctreeKey newKey; 00597 newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2))); 00598 newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1))); 00599 newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0))); 00600 00601 const OctreeNode * childNode; 00602 childNode = getBranchChild (*branch_arg, childIdx); 00603 00604 switch (childNode->getNodeType ()) 00605 { 00606 default: 00607 break; 00608 00609 case BRANCH_NODE: 00610 00611 // recursively proceed with indexed child branch 00612 serializeTreeRecursive (binaryTreeOut_arg, (OctreeBranch*)childNode, newKey, dataVector_arg); 00613 break; 00614 00615 case LEAF_NODE: 00616 OctreeLeaf* childLeaf = (OctreeLeaf*)childNode; 00617 00618 // we reached a leaf node -> execute serialization callback 00619 serializeLeafCallback (*childLeaf, newKey, dataVector_arg); 00620 break; 00621 } 00622 00623 } 00624 00625 } 00626 00627 } 00628 00630 template<typename DataT, typename LeafT, typename BranchT> 00631 void 00632 OctreeBase<DataT, LeafT, BranchT>::serializeLeafsRecursive (const OctreeBranch* branch_arg, const OctreeKey& key_arg, 00633 std::vector<DataT>& dataVector_arg) 00634 { 00635 // child iterator 00636 unsigned char childIdx; 00637 00638 // iterate over all children 00639 for (childIdx = 0; childIdx < 8; childIdx++) 00640 { 00641 00642 // if child exist 00643 if (branchHasChild (*branch_arg, childIdx)) 00644 { 00645 const OctreeNode * childNode; 00646 childNode = getBranchChild (*branch_arg, childIdx); 00647 00648 // generate new key for current branch voxel 00649 OctreeKey newKey; 00650 newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2))); 00651 newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1))); 00652 newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0))); 00653 00654 switch (childNode->getNodeType ()) 00655 { 00656 default: 00657 break; 00658 00659 case BRANCH_NODE: 00660 00661 // recursively proceed with indexed child branch 00662 serializeLeafsRecursive ((OctreeBranch*)childNode, newKey, dataVector_arg); 00663 break; 00664 00665 case LEAF_NODE: 00666 OctreeLeaf* childLeaf = (OctreeLeaf*)childNode; 00667 00668 // we reached a leaf node -> execute serialization callback 00669 serializeLeafCallback (*childLeaf, newKey, dataVector_arg); 00670 break; 00671 } 00672 00673 } 00674 00675 } 00676 } 00677 00679 template<typename DataT, typename LeafT, typename BranchT> 00680 void 00681 OctreeBase<DataT, LeafT, BranchT>::deserializeTreeRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg, 00682 OctreeBranch* branch_arg, const unsigned int depthMask_arg, 00683 const OctreeKey& key_arg) 00684 { 00685 // child iterator 00686 unsigned char childIdx; 00687 char nodeBits; 00688 00689 // read branch occupancy bit pattern from input vector 00690 nodeBits = (*binaryTreeIn_arg); 00691 binaryTreeIn_arg++; 00692 00693 // iterate over all children 00694 for (childIdx = 0; childIdx < 8; childIdx++) 00695 { 00696 // if occupancy bit for childIdx is set.. 00697 if (nodeBits & (1 << childIdx)) 00698 { 00699 00700 // generate new key for current branch voxel 00701 OctreeKey newKey; 00702 newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2))); 00703 newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1))); 00704 newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0))); 00705 00706 if (depthMask_arg > 1) 00707 { 00708 // we have not reached maximum tree depth 00709 OctreeBranch * newBranch; 00710 00711 // create new child branch 00712 createBranchChild (*branch_arg, childIdx, newBranch); 00713 00714 branchCount_++; 00715 00716 // recursively proceed with new child branch 00717 deserializeTreeRecursive (binaryTreeIn_arg, newBranch, depthMask_arg / 2, newKey); 00718 00719 } 00720 else 00721 { 00722 // we reached leaf node level 00723 OctreeLeaf* childLeaf; 00724 00725 // create leaf node 00726 createLeafChild (*branch_arg, childIdx, childLeaf); 00727 00728 // execute deserialization callback 00729 deserializeLeafCallback (*childLeaf, newKey); 00730 00731 leafCount_++; 00732 } 00733 } 00734 } 00735 00736 } 00737 00739 template<typename DataT, typename LeafT, typename BranchT> 00740 void 00741 OctreeBase<DataT, LeafT, BranchT>::deserializeTreeRecursive ( 00742 typename std::vector<char>::const_iterator& binaryTreeIn_arg, 00743 OctreeBranch* branch_arg, 00744 const unsigned int depthMask_arg, 00745 const OctreeKey& key_arg, 00746 typename std::vector<DataT>::const_iterator& dataVectorIterator_arg, 00747 typename std::vector<DataT>::const_iterator& dataVectorEndIterator_arg) 00748 { 00749 // child iterator 00750 unsigned char childIdx; 00751 char nodeBits; 00752 00753 // read branch occupancy bit pattern from input vector 00754 nodeBits = (*binaryTreeIn_arg); 00755 binaryTreeIn_arg++; 00756 00757 // iterate over all children 00758 for (childIdx = 0; childIdx < 8; childIdx++) 00759 { 00760 // if occupancy bit for childIdx is set.. 00761 if (nodeBits & (1 << childIdx)) 00762 { 00763 // generate new key for current branch voxel 00764 OctreeKey newKey; 00765 newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2))); 00766 newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1))); 00767 newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0))); 00768 00769 if (depthMask_arg > 1) 00770 { 00771 // we have not reached maximum tree depth 00772 OctreeBranch * newBranch; 00773 00774 // create new child branch 00775 createBranchChild (*branch_arg, childIdx, newBranch); 00776 00777 branchCount_++; 00778 00779 // recursively proceed with new child branch 00780 deserializeTreeRecursive (binaryTreeIn_arg, newBranch, depthMask_arg / 2, newKey, dataVectorIterator_arg, 00781 dataVectorEndIterator_arg); 00782 00783 } 00784 else 00785 { 00786 // we reached leaf node level 00787 00788 OctreeLeaf* childLeaf; 00789 00790 // create leaf node 00791 createLeafChild (*branch_arg, childIdx, childLeaf); 00792 00793 // execute deserialization callback 00794 deserializeLeafCallback (*childLeaf, newKey, dataVectorIterator_arg, dataVectorEndIterator_arg); 00795 00796 leafCount_++; 00797 } 00798 } 00799 } 00800 } 00801 00803 template<typename DataT, typename LeafT, typename BranchT> 00804 void 00805 OctreeBase<DataT, LeafT, BranchT>::deserializeTreeAndOutputLeafDataRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg, 00806 OctreeBranch* branch_arg, 00807 const unsigned int depthMask_arg, 00808 const OctreeKey& key_arg, 00809 std::vector<DataT>& dataVector_arg) 00810 { 00811 // child iterator 00812 unsigned char childIdx; 00813 char nodeBits; 00814 00815 // read branch occupancy bit pattern from input vector 00816 nodeBits = (*binaryTreeIn_arg); 00817 binaryTreeIn_arg++; 00818 00819 // iterate over all children 00820 for (childIdx = 0; childIdx < 8; childIdx++) 00821 { 00822 // if occupancy bit for childIdx is set.. 00823 if (nodeBits & (1 << childIdx)) 00824 { 00825 00826 // generate new key for current branch voxel 00827 OctreeKey newKey; 00828 newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2))); 00829 newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1))); 00830 newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0))); 00831 00832 if (depthMask_arg > 1) 00833 { 00834 // we have not reached maximum tree depth 00835 OctreeBranch * newBranch; 00836 00837 // create new child branch 00838 createBranchChild (*branch_arg, childIdx, newBranch); 00839 00840 branchCount_++; 00841 00842 // recursively proceed with new child branch 00843 deserializeTreeAndOutputLeafDataRecursive (binaryTreeIn_arg, newBranch, depthMask_arg / 2, newKey, 00844 dataVector_arg); 00845 00846 } 00847 else 00848 { 00849 // we reached leaf node level 00850 OctreeLeaf* childLeaf; 00851 00852 // create leaf node 00853 createLeafChild (*branch_arg, childIdx, childLeaf); 00854 00855 // execute deserialization callback 00856 deserializeTreeAndSerializeLeafCallback (*childLeaf, newKey, dataVector_arg); 00857 00858 leafCount_++; 00859 } 00860 } 00861 } 00862 00863 } 00864 00866 template<typename DataT, typename LeafT, typename BranchT> 00867 void 00868 OctreeBase<DataT, LeafT, BranchT>::serializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg) 00869 { 00870 // nothing to do 00871 } 00872 00873 00875 template<typename DataT, typename LeafT, typename BranchT> 00876 void 00877 OctreeBase<DataT, LeafT, BranchT>::serializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg, 00878 std::vector<DataT>& dataVector_arg) 00879 { 00880 leaf_arg.getData (dataVector_arg); 00881 } 00882 00884 template<typename DataT, typename LeafT, typename BranchT> 00885 void 00886 OctreeBase<DataT, LeafT, BranchT>::deserializeLeafCallback ( 00887 OctreeLeaf& leaf_arg, 00888 const OctreeKey& key_arg, 00889 typename std::vector<DataT>::const_iterator& dataVectorIterator_arg, 00890 typename std::vector<DataT>::const_iterator& dataVectorEndIterator_arg) 00891 { 00892 OctreeKey dataKey; 00893 bool bKeyBasedEncoding = false; 00894 00895 // add DataT objects to octree leaf as long as their key fit to voxel 00896 while ((dataVectorIterator_arg != dataVectorEndIterator_arg) 00897 && (this->genOctreeKeyForDataT (*dataVectorIterator_arg, dataKey) && (dataKey == key_arg))) 00898 { 00899 leaf_arg.setData (*dataVectorIterator_arg); 00900 dataVectorIterator_arg++; 00901 bKeyBasedEncoding = true; 00902 } 00903 00904 // add single DataT object to octree if key-based encoding is disabled 00905 if (!bKeyBasedEncoding) 00906 { 00907 leaf_arg.setData (*dataVectorIterator_arg); 00908 dataVectorIterator_arg++; 00909 } 00910 } 00911 00913 template<typename DataT, typename LeafT, typename BranchT> 00914 void 00915 OctreeBase<DataT, LeafT, BranchT>::deserializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg) 00916 { 00917 00918 DataT newDataT; 00919 00920 // initialize new leaf child 00921 if (genDataTByOctreeKey (key_arg, newDataT)) 00922 { 00923 leaf_arg.setData (newDataT); 00924 } 00925 00926 } 00927 00929 template<typename DataT, typename LeafT, typename BranchT> 00930 void 00931 OctreeBase<DataT, LeafT, BranchT>::deserializeTreeAndSerializeLeafCallback (OctreeLeaf& leaf_arg, 00932 const OctreeKey& key_arg, 00933 std::vector<DataT>& dataVector_arg) 00934 { 00935 00936 DataT newDataT; 00937 00938 // initialize new leaf child 00939 if (genDataTByOctreeKey (key_arg, newDataT)) 00940 { 00941 leaf_arg.setData (newDataT); 00942 dataVector_arg.push_back (newDataT); 00943 } 00944 } 00945 00946 } 00947 } 00948 00949 #define PCL_INSTANTIATE_OctreeBase(T) template class PCL_EXPORTS pcl::octree::OctreeBase<T>; 00950 00951 #endif 00952
1.8.0
