|
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_lowmemory_base.hpp 4702 2012-02-23 09:39:33Z gedikli $ 00037 */ 00038 00039 #ifndef OCTREE_LOWMEM_TREE_BASE_HPP 00040 #define OCTREE_LOWMEM_TREE_BASE_HPP 00041 00042 #include <vector> 00043 00044 #include "pcl/impl/instantiate.hpp" 00045 #include "pcl/point_types.h" 00046 #include "pcl/octree/octree.h" 00047 00048 namespace pcl 00049 { 00050 namespace octree 00051 { 00052 00053 using namespace std; 00054 00056 template<typename DataT, typename LeafT> 00057 OctreeLowMemBase<DataT, LeafT>::OctreeLowMemBase () 00058 { 00059 00060 // Initialization of globals 00061 rootNode_ = new OctreeBranch (); 00062 leafCount_ = 0; 00063 depthMask_ = 0; 00064 branchCount_ = 1; 00065 objectCount_ = 0; 00066 octreeDepth_ = 0; 00067 00068 } 00069 00071 template<typename DataT, typename LeafT> 00072 OctreeLowMemBase<DataT, LeafT>::~OctreeLowMemBase () 00073 { 00074 00075 // deallocate tree structure 00076 deleteTree (); 00077 delete (rootNode_); 00078 } 00079 00081 template<typename DataT, typename LeafT> 00082 void 00083 OctreeLowMemBase<DataT, LeafT>::setMaxVoxelIndex (unsigned int maxVoxelIndex_arg) 00084 { 00085 unsigned int treeDepth; 00086 00087 assert (maxVoxelIndex_arg>0); 00088 00089 // tree depth == amount of bits of maxVoxels 00090 treeDepth = max ((min ((unsigned int)OCT_MAXTREEDEPTH, (unsigned int)ceil (Log2 (maxVoxelIndex_arg)))), 00091 (unsigned int)0); 00092 00093 // define depthMask_ by setting a single bit to 1 at bit position == tree depth 00094 depthMask_ = (1 << (treeDepth - 1)); 00095 00096 } 00097 00099 template<typename DataT, typename LeafT> 00100 void 00101 OctreeLowMemBase<DataT, LeafT>::setTreeDepth (unsigned int depth_arg) 00102 { 00103 00104 assert (depth_arg>0); 00105 00106 // set octree depth 00107 octreeDepth_ = depth_arg; 00108 00109 // define depthMask_ by setting a single bit to 1 at bit position == tree depth 00110 depthMask_ = (1 << (depth_arg - 1)); 00111 00112 } 00113 00115 template<typename DataT, typename LeafT> 00116 void 00117 OctreeLowMemBase<DataT, LeafT>::add (const unsigned int idxX_arg, const unsigned int idxY_arg, 00118 const unsigned int idxZ_arg, const DataT& data_arg) 00119 { 00120 00121 OctreeKey key; 00122 00123 // generate key 00124 genOctreeKeyByIntIdx (idxX_arg, idxY_arg, idxZ_arg, key); 00125 00126 // add data_arg to octree 00127 add (key, data_arg); 00128 00129 objectCount_++; 00130 00131 } 00132 00133 00134 00136 template<typename DataT, typename LeafT> 00137 bool 00138 OctreeLowMemBase<DataT, LeafT>::get (const unsigned int idxX_arg, const unsigned int idxY_arg, 00139 const unsigned int idxZ_arg, DataT& data_arg) const 00140 { 00141 OctreeKey key; 00142 00143 // generate key 00144 genOctreeKeyByIntIdx (idxX_arg, idxY_arg, idxZ_arg, key); 00145 00146 // search for leaf at key 00147 LeafT* leaf = findLeaf (key); 00148 if (leaf) 00149 { 00150 const DataT * dataPtr; 00151 // if successful, decode data to data_arg 00152 leaf->getData (dataPtr); 00153 if (dataPtr) 00154 data_arg = *dataPtr; 00155 } 00156 00157 // returns true on success 00158 return (leaf != 0); 00159 } 00160 00162 template<typename DataT, typename LeafT> 00163 bool 00164 OctreeLowMemBase<DataT, LeafT>::existLeaf (const unsigned int idxX_arg, const unsigned int idxY_arg, 00165 const unsigned int idxZ_arg) const 00166 { 00167 OctreeKey key; 00168 00169 // generate key 00170 this->genOctreeKeyByIntIdx (idxX_arg, idxY_arg, idxZ_arg, key); 00171 00172 // check if key exist in octree 00173 return existLeaf (key); 00174 } 00175 00177 template<typename DataT, typename LeafT> 00178 void 00179 OctreeLowMemBase<DataT, LeafT>::removeLeaf (const unsigned int idxX_arg, const unsigned int idxY_arg, 00180 const unsigned int idxZ_arg) 00181 { 00182 OctreeKey key; 00183 00184 // generate key 00185 this->genOctreeKeyByIntIdx (idxX_arg, idxY_arg, idxZ_arg, key); 00186 00187 // check if key exist in octree 00188 deleteLeafRecursive (key, depthMask_, rootNode_); 00189 } 00190 00192 template<typename DataT, typename LeafT> 00193 void 00194 OctreeLowMemBase<DataT, LeafT>::deleteTree ( ) 00195 { 00196 00197 if (rootNode_) 00198 { 00199 // reset octree 00200 deleteBranch (*rootNode_); 00201 leafCount_ = 0; 00202 branchCount_ = 1; 00203 objectCount_ = 0; 00204 00205 } 00206 00207 } 00208 00210 template<typename DataT, typename LeafT> 00211 void 00212 OctreeLowMemBase<DataT, LeafT>::serializeTree (std::vector<char>& binaryTreeOut_arg, 00213 bool doXOREncoding_arg) 00214 { 00215 OctreeKey newKey; 00216 newKey.x = newKey.y = newKey.z = 0; 00217 00218 // clear binary vector 00219 binaryTreeOut_arg.clear (); 00220 binaryTreeOut_arg.reserve (this->branchCount_); 00221 00222 serializeTreeRecursive (binaryTreeOut_arg, rootNode_, newKey); 00223 } 00224 00226 template<typename DataT, typename LeafT> 00227 void 00228 OctreeLowMemBase<DataT, LeafT>::serializeTree (std::vector<char>& binaryTreeOut_arg, 00229 std::vector<DataT>& dataVector_arg, bool doXOREncoding_arg) 00230 { 00231 OctreeKey newKey; 00232 newKey.x = newKey.y = newKey.z = 0; 00233 00234 // clear output vectors 00235 binaryTreeOut_arg.clear (); 00236 dataVector_arg.clear (); 00237 00238 dataVector_arg.reserve (this->objectCount_); 00239 binaryTreeOut_arg.reserve (this->branchCount_); 00240 00241 OctreeLowMemBase<DataT, LeafT>::serializeTreeRecursive (binaryTreeOut_arg, rootNode_, newKey, dataVector_arg); 00242 } 00243 00245 template<typename DataT, typename LeafT> 00246 void 00247 OctreeLowMemBase<DataT, LeafT>::serializeLeafs (std::vector<DataT>& dataVector_arg) 00248 { 00249 00250 OctreeKey newKey; 00251 newKey.x = newKey.y = newKey.z = 0; 00252 00253 // clear output vector 00254 dataVector_arg.clear (); 00255 00256 dataVector_arg.reserve(this->objectCount_); 00257 00258 serializeLeafsRecursive (rootNode_, newKey, dataVector_arg); 00259 } 00260 00262 template<typename DataT, typename LeafT> 00263 void 00264 OctreeLowMemBase<DataT, LeafT>::deserializeTree (std::vector<char>& binaryTreeIn_arg, 00265 bool doXORDecoding_arg) 00266 { 00267 00268 OctreeKey newKey; 00269 newKey.x = newKey.y = newKey.z = 0; 00270 00271 // free existing tree before tree rebuild 00272 deleteTree (); 00273 00274 //iterator for binary tree structure vector 00275 vector<char>::const_iterator binaryTreeVectorIterator = binaryTreeIn_arg.begin (); 00276 00277 deserializeTreeRecursive (binaryTreeVectorIterator, rootNode_, depthMask_, newKey); 00278 00279 objectCount_ = this->leafCount_; 00280 } 00281 00283 template<typename DataT, typename LeafT> 00284 void 00285 OctreeLowMemBase<DataT, LeafT>::deserializeTree (std::vector<char>& binaryTreeIn_arg, 00286 std::vector<DataT>& dataVector_arg, 00287 bool doXORDecoding_arg) 00288 { 00289 OctreeKey newKey; 00290 newKey.x = newKey.y = newKey.z = 0; 00291 00292 // set data iterator to first element 00293 typename std::vector<DataT>::const_iterator dataVectorIterator = dataVector_arg.begin (); 00294 00295 // set data iterator to last element 00296 typename std::vector<DataT>::const_iterator dataVectorEndIterator = dataVector_arg.end (); 00297 00298 // free existing tree before tree rebuild 00299 deleteTree (); 00300 00301 //iterator for binary tree structure vector 00302 vector<char>::const_iterator binaryTreeVectorIterator = binaryTreeIn_arg.begin (); 00303 00304 deserializeTreeRecursive (binaryTreeVectorIterator, rootNode_, depthMask_, newKey, dataVectorIterator, 00305 dataVectorEndIterator); 00306 00307 objectCount_ = (unsigned int)dataVector_arg.size(); 00308 } 00309 00311 template<typename DataT, typename LeafT> 00312 void 00313 OctreeLowMemBase<DataT, LeafT>::deserializeTreeAndOutputLeafData (std::vector<char>& binaryTreeIn_arg, 00314 std::vector<DataT>& dataVector_arg) 00315 { 00316 00317 OctreeKey newKey; 00318 newKey.x = newKey.y = newKey.z = 0; 00319 00320 // free existing tree before tree rebuild 00321 deleteTree (); 00322 00323 //iterator for binary tree structure vector 00324 vector<char>::const_iterator binaryTreeVectorIterator = binaryTreeIn_arg.begin (); 00325 00326 deserializeTreeAndOutputLeafDataRecursive (binaryTreeVectorIterator, rootNode_, depthMask_, newKey, 00327 dataVector_arg); 00328 00329 objectCount_ = (unsigned int)dataVector_arg.size(); 00330 } 00331 00333 template<typename DataT, typename LeafT> 00334 LeafT* 00335 OctreeLowMemBase<DataT, LeafT>::getLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg, 00336 OctreeBranch* branch_arg) 00337 { 00338 00339 // index to branch child 00340 unsigned char childIdx; 00341 LeafT* result = 0; 00342 00343 // find branch child from key 00344 childIdx = ((!!(key_arg.x & depthMask_arg)) << 2) | ((!!(key_arg.y & depthMask_arg)) << 1) | (!!(key_arg.z 00345 & depthMask_arg)); 00346 00347 if (depthMask_arg > 1) 00348 { 00349 // we have not reached maximum tree depth 00350 00351 OctreeBranch* childBranch; 00352 if (!branchHasChild (*branch_arg, childIdx)) 00353 { 00354 00355 // if required branch does not exist -> create it 00356 createBranchChild (*branch_arg, childIdx, childBranch); 00357 00358 branchCount_++; 00359 00360 } 00361 else 00362 { 00363 00364 // required branch exists 00365 childBranch = (OctreeBranch*)getBranchChild (*branch_arg, childIdx); 00366 } 00367 00368 // recursively proceed with indexed child branch 00369 result = getLeafRecursive (key_arg, depthMask_arg / 2, childBranch); 00370 00371 } 00372 else 00373 { 00374 // branch childs are leaf nodes 00375 00376 OctreeLeaf* childLeaf; 00377 if (!branchHasChild (*branch_arg, childIdx)) 00378 { 00379 00380 // if leaf node at childIdx does not exist 00381 createLeafChild (*branch_arg, childIdx, childLeaf); 00382 leafCount_++; 00383 00384 // return leaf node 00385 result = childLeaf; 00386 00387 } 00388 else 00389 { 00390 00391 // leaf node already exist 00392 childLeaf = (OctreeLeaf*)getBranchChild (*branch_arg, childIdx); 00393 00394 // return leaf node 00395 result = childLeaf; 00396 00397 } 00398 00399 } 00400 00401 return result; 00402 00403 } 00404 00406 template<typename DataT, typename LeafT> 00407 LeafT* 00408 OctreeLowMemBase<DataT, LeafT>::findLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg, 00409 OctreeBranch* branch_arg) const 00410 { 00411 // index to branch child 00412 unsigned char childIdx; 00413 LeafT* result = 0; 00414 00415 // find branch child from key 00416 childIdx = ((!!(key_arg.x & depthMask_arg)) << 2) | ((!!(key_arg.y & depthMask_arg)) << 1) | (!!(key_arg.z 00417 & depthMask_arg)); 00418 00419 if (depthMask_arg > 1) 00420 { 00421 // we have not reached maximum tree depth 00422 OctreeBranch* childBranch; 00423 childBranch = (OctreeBranch*)getBranchChild (*branch_arg, childIdx); 00424 00425 if (childBranch) 00426 // recursively proceed with indexed child branch 00427 result = findLeafRecursive (key_arg, depthMask_arg / 2, childBranch); 00428 00429 } 00430 else 00431 { 00432 // we reached leaf node level 00433 if (branchHasChild (*branch_arg, childIdx)) 00434 { 00435 00436 // return existing leaf node 00437 OctreeLeaf* childLeaf = (OctreeLeaf*)getBranchChild (*branch_arg, childIdx); 00438 result = childLeaf; 00439 } 00440 00441 } 00442 00443 return result; 00444 00445 } 00446 00448 template<typename DataT, typename LeafT> 00449 bool 00450 OctreeLowMemBase<DataT, LeafT>::deleteLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg, 00451 OctreeBranch* branch_arg) 00452 { 00453 // index to branch child 00454 unsigned char childIdx; 00455 // indicates if branch is empty and can be safely removed 00456 bool bNoChilds; 00457 00458 // find branch child from key 00459 childIdx = ((!!(key_arg.x & depthMask_arg)) << 2) | ((!!(key_arg.y & depthMask_arg)) << 1) | (!!(key_arg.z 00460 & depthMask_arg)); 00461 00462 if (depthMask_arg > 1) 00463 { 00464 // we have not reached maximum tree depth 00465 00466 OctreeBranch* childBranch; 00467 bool bBranchOccupied; 00468 00469 // next branch child on our path through the tree 00470 childBranch = (OctreeBranch*)getBranchChild (*branch_arg, childIdx); 00471 00472 if (childBranch) 00473 { 00474 // recursively explore the indexed child branch 00475 bBranchOccupied = deleteLeafRecursive (key_arg, depthMask_arg / 2, childBranch); 00476 00477 if (!bBranchOccupied) 00478 { 00479 // child branch does not own any sub-child nodes anymore -> delete child branch 00480 delete (childBranch); 00481 setBranchChild (*branch_arg, childIdx, 0); 00482 branchCount_--; 00483 } 00484 } 00485 00486 } 00487 else 00488 { 00489 00490 // our child is a leaf node -> delete it 00491 deleteBranchChild (*branch_arg, childIdx); 00492 leafCount_--; 00493 } 00494 00495 // check if current branch still owns childs 00496 bNoChilds = false; 00497 for (childIdx = 0; childIdx < 8; childIdx++) 00498 { 00499 bNoChilds = branchHasChild (*branch_arg, childIdx); 00500 if (bNoChilds) 00501 break; 00502 } 00503 00504 // return true if current branch can be deleted 00505 return bNoChilds; 00506 00507 } 00508 00510 template<typename DataT, typename LeafT> 00511 void 00512 OctreeLowMemBase<DataT, LeafT>::serializeTreeRecursive (std::vector<char>& binaryTreeOut_arg, 00513 const OctreeBranch* branch_arg, const OctreeKey& key_arg) 00514 { 00515 00516 // child iterator 00517 unsigned char childIdx; 00518 char nodeBitPattern; 00519 00520 // branch occupancy bit pattern 00521 nodeBitPattern = getBranchBitPattern (*branch_arg); 00522 00523 // write bit pattern to output vector 00524 binaryTreeOut_arg.push_back (nodeBitPattern); 00525 00526 // iterate over all children 00527 for (childIdx = 0; childIdx < 8; childIdx++) 00528 { 00529 00530 // if child exist 00531 if (branchHasChild (*branch_arg, childIdx)) 00532 { 00533 00534 // generate new key for current branch voxel 00535 OctreeKey newKey; 00536 newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2))); 00537 newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1))); 00538 newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0))); 00539 00540 const OctreeNode * childNode; 00541 childNode = getBranchChild (*branch_arg, childIdx); 00542 00543 switch (childNode->getNodeType ()) 00544 { 00545 case BRANCH_NODE: 00546 // recursively proceed with indexed child branch 00547 serializeTreeRecursive (binaryTreeOut_arg, (OctreeBranch*)childNode, newKey); 00548 break; 00549 case LEAF_NODE: 00550 { 00551 OctreeLeaf* childLeaf = (OctreeLeaf*)childNode; 00552 00553 // we reached a leaf node -> execute serialization callback 00554 serializeLeafCallback (*childLeaf, newKey); 00555 } 00556 break; 00557 default: 00558 break; 00559 00560 } 00561 } 00562 } 00563 } 00564 00566 template<typename DataT, typename LeafT> 00567 void 00568 OctreeLowMemBase<DataT, LeafT>::serializeTreeRecursive (std::vector<char>& binaryTreeOut_arg, 00569 const OctreeBranch* branch_arg, const OctreeKey& key_arg, 00570 typename std::vector<DataT>& dataVector_arg) 00571 { 00572 00573 // child iterator 00574 unsigned char childIdx; 00575 char nodeBitPattern; 00576 00577 // branch occupancy bit pattern 00578 nodeBitPattern = getBranchBitPattern (*branch_arg); 00579 00580 // write bit pattern to output vector 00581 binaryTreeOut_arg.push_back (nodeBitPattern); 00582 00583 // iterate over all children 00584 for (childIdx = 0; childIdx < 8; childIdx++) 00585 { 00586 00587 // if child exist 00588 if (branchHasChild (*branch_arg, childIdx)) 00589 { 00590 00591 // generate new key for current branch voxel 00592 OctreeKey newKey; 00593 newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2))); 00594 newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1))); 00595 newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0))); 00596 00597 const OctreeNode * childNode; 00598 childNode = getBranchChild (*branch_arg, childIdx); 00599 00600 switch (childNode->getNodeType ()) 00601 { 00602 case BRANCH_NODE: 00603 00604 // recursively proceed with indexed child branch 00605 serializeTreeRecursive (binaryTreeOut_arg, (OctreeBranch*)childNode, newKey, dataVector_arg); 00606 break; 00607 00608 case LEAF_NODE: 00609 { 00610 OctreeLeaf* childLeaf = (OctreeLeaf*)childNode; 00611 00612 // we reached a leaf node -> execute serialization callback 00613 serializeLeafCallback (*childLeaf, newKey, dataVector_arg); 00614 } 00615 break; 00616 default: 00617 break; 00618 00619 } 00620 } 00621 } 00622 } 00623 00625 template<typename DataT, typename LeafT> 00626 void 00627 OctreeLowMemBase<DataT, LeafT>::serializeLeafsRecursive (const OctreeBranch* branch_arg, const OctreeKey& key_arg, 00628 std::vector<DataT>& dataVector_arg) 00629 { 00630 // child iterator 00631 unsigned char childIdx; 00632 00633 // iterate over all children 00634 for (childIdx = 0; childIdx < 8; childIdx++) 00635 { 00636 00637 // if child exist 00638 if (branchHasChild (*branch_arg, childIdx)) 00639 { 00640 const OctreeNode * childNode; 00641 childNode = getBranchChild (*branch_arg, childIdx); 00642 00643 // generate new key for current branch voxel 00644 OctreeKey newKey; 00645 newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2))); 00646 newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1))); 00647 newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0))); 00648 00649 switch (childNode->getNodeType ()) 00650 { 00651 case BRANCH_NODE: 00652 00653 // recursively proceed with indexed child branch 00654 serializeLeafsRecursive ((OctreeBranch*)childNode, newKey, dataVector_arg); 00655 break; 00656 00657 case LEAF_NODE: 00658 { 00659 OctreeLeaf* childLeaf = (OctreeLeaf*)childNode; 00660 00661 // we reached a leaf node -> execute serialization callback 00662 serializeLeafCallback (*childLeaf, newKey, dataVector_arg); 00663 } 00664 break; 00665 default: 00666 break; 00667 00668 } 00669 } 00670 } 00671 } 00672 00674 template<typename DataT, typename LeafT> 00675 void 00676 OctreeLowMemBase<DataT, LeafT>::deserializeTreeRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg, 00677 OctreeBranch* branch_arg, const unsigned int depthMask_arg, 00678 const OctreeKey& key_arg) 00679 { 00680 // child iterator 00681 unsigned char childIdx; 00682 char nodeBits; 00683 00684 // read branch occupancy bit pattern from input vector 00685 nodeBits = (*binaryTreeIn_arg); 00686 binaryTreeIn_arg++; 00687 00688 // iterate over all children 00689 for (childIdx = 0; childIdx < 8; childIdx++) 00690 { 00691 // if occupancy bit for childIdx is set.. 00692 if (nodeBits & (1 << childIdx)) 00693 { 00694 00695 // generate new key for current branch voxel 00696 OctreeKey newKey; 00697 newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2))); 00698 newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1))); 00699 newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0))); 00700 00701 if (depthMask_arg > 1) 00702 { 00703 // we have not reached maximum tree depth 00704 OctreeBranch * newBranch; 00705 00706 // create new child branch 00707 createBranchChild (*branch_arg, childIdx, newBranch); 00708 00709 branchCount_++; 00710 00711 // recursively proceed with new child branch 00712 deserializeTreeRecursive (binaryTreeIn_arg, newBranch, depthMask_arg / 2, newKey); 00713 00714 } 00715 else 00716 { 00717 // we reached leaf node level 00718 OctreeLeaf* childLeaf; 00719 00720 // create leaf node 00721 createLeafChild (*branch_arg, childIdx, childLeaf); 00722 00723 // execute deserialization callback 00724 deserializeLeafCallback (*childLeaf, newKey); 00725 00726 leafCount_++; 00727 } 00728 } 00729 } 00730 00731 } 00732 00734 template<typename DataT, typename LeafT> 00735 void 00736 OctreeLowMemBase<DataT, LeafT>::deserializeTreeRecursive ( 00737 typename std::vector<char>::const_iterator& binaryTreeIn_arg, 00738 OctreeBranch* branch_arg, 00739 const unsigned int depthMask_arg, 00740 const OctreeKey& key_arg, 00741 typename std::vector<DataT>::const_iterator& dataVectorIterator_arg, 00742 typename std::vector<DataT>::const_iterator& dataVectorEndIterator_arg) 00743 { 00744 // child iterator 00745 unsigned char childIdx; 00746 char nodeBits; 00747 00748 // read branch occupancy bit pattern from input vector 00749 nodeBits = (*binaryTreeIn_arg); 00750 binaryTreeIn_arg++; 00751 00752 // iterate over all children 00753 for (childIdx = 0; childIdx < 8; childIdx++) 00754 { 00755 // if occupancy bit for childIdx is set.. 00756 if (nodeBits & (1 << childIdx)) 00757 { 00758 // generate new key for current branch voxel 00759 OctreeKey newKey; 00760 newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2))); 00761 newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1))); 00762 newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0))); 00763 00764 if (depthMask_arg > 1) 00765 { 00766 // we have not reached maximum tree depth 00767 OctreeBranch * newBranch; 00768 00769 // create new child branch 00770 createBranchChild (*branch_arg, childIdx, newBranch); 00771 00772 branchCount_++; 00773 00774 // recursively proceed with new child branch 00775 deserializeTreeRecursive (binaryTreeIn_arg, newBranch, depthMask_arg / 2, newKey, dataVectorIterator_arg, 00776 dataVectorEndIterator_arg); 00777 00778 } 00779 else 00780 { 00781 // we reached leaf node level 00782 00783 OctreeLeaf* childLeaf; 00784 00785 // create leaf node 00786 createLeafChild (*branch_arg, childIdx, childLeaf); 00787 00788 // execute deserialization callback 00789 deserializeLeafCallback (*childLeaf, newKey, dataVectorIterator_arg, dataVectorEndIterator_arg); 00790 00791 leafCount_++; 00792 } 00793 } 00794 } 00795 } 00796 00798 template<typename DataT, typename LeafT> 00799 void 00800 OctreeLowMemBase<DataT, LeafT>::deserializeTreeAndOutputLeafDataRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg, 00801 OctreeBranch* branch_arg, 00802 const unsigned int depthMask_arg, 00803 const OctreeKey& key_arg, 00804 std::vector<DataT>& dataVector_arg) 00805 { 00806 // child iterator 00807 unsigned char childIdx; 00808 char nodeBits; 00809 00810 // read branch occupancy bit pattern from input vector 00811 nodeBits = (*binaryTreeIn_arg); 00812 binaryTreeIn_arg++; 00813 00814 // iterate over all children 00815 for (childIdx = 0; childIdx < 8; childIdx++) 00816 { 00817 // if occupancy bit for childIdx is set.. 00818 if (nodeBits & (1 << childIdx)) 00819 { 00820 00821 // generate new key for current branch voxel 00822 OctreeKey newKey; 00823 newKey.x = (key_arg.x << 1) | (!!(childIdx & (1 << 2))); 00824 newKey.y = (key_arg.y << 1) | (!!(childIdx & (1 << 1))); 00825 newKey.z = (key_arg.z << 1) | (!!(childIdx & (1 << 0))); 00826 00827 if (depthMask_arg > 1) 00828 { 00829 // we have not reached maximum tree depth 00830 OctreeBranch * newBranch; 00831 00832 // create new child branch 00833 createBranchChild (*branch_arg, childIdx, newBranch); 00834 00835 branchCount_++; 00836 00837 // recursively proceed with new child branch 00838 deserializeTreeAndOutputLeafDataRecursive (binaryTreeIn_arg, newBranch, depthMask_arg / 2, newKey, 00839 dataVector_arg); 00840 00841 } 00842 else 00843 { 00844 // we reached leaf node level 00845 OctreeLeaf* childLeaf; 00846 00847 // create leaf node 00848 createLeafChild (*branch_arg, childIdx, childLeaf); 00849 00850 // execute deserialization callback 00851 deserializeTreeAndSerializeLeafCallback (*childLeaf, newKey, dataVector_arg); 00852 00853 leafCount_++; 00854 } 00855 } 00856 } 00857 00858 } 00859 00861 template<typename DataT, typename LeafT> 00862 void 00863 OctreeLowMemBase<DataT, LeafT>::serializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg) 00864 { 00865 // nothing to do 00866 } 00867 00868 00870 template<typename DataT, typename LeafT> 00871 void 00872 OctreeLowMemBase<DataT, LeafT>::serializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg, 00873 std::vector<DataT>& dataVector_arg) 00874 { 00875 leaf_arg.getData (dataVector_arg); 00876 } 00877 00879 template<typename DataT, typename LeafT> 00880 void 00881 OctreeLowMemBase<DataT, LeafT>::deserializeLeafCallback ( 00882 OctreeLeaf& leaf_arg, 00883 const OctreeKey& key_arg, 00884 typename std::vector<DataT>::const_iterator& dataVectorIterator_arg, 00885 typename std::vector<DataT>::const_iterator& dataVectorEndIterator_arg) 00886 { 00887 OctreeKey dataKey; 00888 bool bKeyBasedEncoding = false; 00889 00890 // add DataT objects to octree leaf as long as their key fit to voxel 00891 while ((dataVectorIterator_arg != dataVectorEndIterator_arg) 00892 && (this->genOctreeKeyForDataT (*dataVectorIterator_arg, dataKey) && (dataKey == key_arg))) 00893 { 00894 leaf_arg.setData (*dataVectorIterator_arg); 00895 dataVectorIterator_arg++; 00896 bKeyBasedEncoding = true; 00897 } 00898 00899 // add single DataT object to octree if key-based encoding is disabled 00900 if (!bKeyBasedEncoding) 00901 { 00902 leaf_arg.setData (*dataVectorIterator_arg); 00903 dataVectorIterator_arg++; 00904 } 00905 } 00906 00908 template<typename DataT, typename LeafT> 00909 void 00910 OctreeLowMemBase<DataT, LeafT>::deserializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg) 00911 { 00912 00913 DataT newDataT; 00914 00915 // initialize new leaf child 00916 if (genDataTByOctreeKey (key_arg, newDataT)) 00917 { 00918 leaf_arg.setData (newDataT); 00919 } 00920 00921 } 00922 00924 template<typename DataT, typename LeafT> 00925 void 00926 OctreeLowMemBase<DataT, LeafT>::deserializeTreeAndSerializeLeafCallback (OctreeLeaf& leaf_arg, 00927 const OctreeKey& key_arg, 00928 std::vector<DataT>& dataVector_arg) 00929 { 00930 00931 DataT newDataT; 00932 00933 // initialize new leaf child 00934 if (genDataTByOctreeKey (key_arg, newDataT)) 00935 { 00936 leaf_arg.setData (newDataT); 00937 dataVector_arg.push_back (newDataT); 00938 } 00939 } 00940 00941 } 00942 } 00943 00944 #define PCL_INSTANTIATE_OctreeLowMemBase(T) template class PCL_EXPORTS pcl::octree::OctreeLowMemBase<T>; 00945 00946 #endif 00947
1.8.0
