Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2012, 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$ 00037 */ 00038 00039 #ifndef PCL_OCTREE_2BUF_BASE_HPP 00040 #define PCL_OCTREE_2BUF_BASE_HPP 00041 00042 namespace pcl 00043 { 00044 namespace octree 00045 { 00046 ////////////////////////////////////////////////////////////////////////////////////////////// 00047 template<typename LeafContainerT, typename BranchContainerT> 00048 Octree2BufBase<LeafContainerT, BranchContainerT>::Octree2BufBase () : 00049 leaf_count_ (0), 00050 branch_count_ (1), 00051 root_node_ (new BranchNode ()), 00052 depth_mask_ (0), 00053 max_key_ (), 00054 buffer_selector_ (0), 00055 tree_dirty_flag_ (false), 00056 octree_depth_ (0), 00057 dynamic_depth_enabled_(false) 00058 { 00059 } 00060 00061 ////////////////////////////////////////////////////////////////////////////////////////////// 00062 template<typename LeafContainerT, typename BranchContainerT> 00063 Octree2BufBase<LeafContainerT, BranchContainerT>::~Octree2BufBase () 00064 { 00065 // deallocate tree structure 00066 deleteTree (); 00067 delete (root_node_); 00068 } 00069 00070 ////////////////////////////////////////////////////////////////////////////////////////////// 00071 template<typename LeafContainerT, typename BranchContainerT> void 00072 Octree2BufBase<LeafContainerT, BranchContainerT>::setMaxVoxelIndex (unsigned int max_voxel_index_arg) 00073 { 00074 unsigned int treeDepth; 00075 00076 assert (max_voxel_index_arg > 0); 00077 00078 // tree depth == amount of bits of maxVoxels 00079 treeDepth = std::max ((std::min (static_cast<unsigned int> (OctreeKey::maxDepth), 00080 static_cast<unsigned int> (std::ceil (Log2 (max_voxel_index_arg))))), 00081 static_cast<unsigned int> (0)); 00082 00083 // define depthMask_ by setting a single bit to 1 at bit position == tree depth 00084 depth_mask_ = (1 << (treeDepth - 1)); 00085 } 00086 00087 ////////////////////////////////////////////////////////////////////////////////////////////// 00088 template<typename LeafContainerT, typename BranchContainerT> void 00089 Octree2BufBase<LeafContainerT, BranchContainerT>::setTreeDepth (unsigned int depth_arg) 00090 { 00091 assert (depth_arg > 0); 00092 00093 // set octree depth 00094 octree_depth_ = depth_arg; 00095 00096 // define depthMask_ by setting a single bit to 1 at bit position == tree depth 00097 depth_mask_ = (1 << (depth_arg - 1)); 00098 00099 // define max. keys 00100 max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1; 00101 } 00102 00103 ////////////////////////////////////////////////////////////////////////////////////////////// 00104 template<typename LeafContainerT, typename BranchContainerT> LeafContainerT* 00105 Octree2BufBase<LeafContainerT, BranchContainerT>::findLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) 00106 { 00107 // generate key 00108 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg); 00109 00110 // check if key exist in octree 00111 return ( findLeaf (key)); 00112 } 00113 00114 ////////////////////////////////////////////////////////////////////////////////////////////// 00115 template<typename LeafContainerT, typename BranchContainerT> LeafContainerT* 00116 Octree2BufBase<LeafContainerT, BranchContainerT>::createLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) 00117 { 00118 // generate key 00119 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg); 00120 00121 // check if key exist in octree 00122 return ( createLeaf (key)); 00123 } 00124 00125 ////////////////////////////////////////////////////////////////////////////////////////////// 00126 template<typename LeafContainerT, typename BranchContainerT> bool 00127 Octree2BufBase<LeafContainerT, BranchContainerT>::existLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const 00128 { 00129 // generate key 00130 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg); 00131 00132 // check if key exist in octree 00133 return existLeaf (key); 00134 } 00135 00136 ////////////////////////////////////////////////////////////////////////////////////////////// 00137 template<typename LeafContainerT, typename BranchContainerT> void 00138 Octree2BufBase<LeafContainerT, BranchContainerT>::removeLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) 00139 { 00140 // generate key 00141 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg); 00142 00143 // free voxel at key 00144 return (this->removeLeaf (key)); 00145 } 00146 00147 ////////////////////////////////////////////////////////////////////////////////////////////// 00148 template<typename LeafContainerT, typename BranchContainerT> void 00149 Octree2BufBase<LeafContainerT, BranchContainerT>::deleteTree () 00150 { 00151 if (root_node_) 00152 { 00153 // reset octree 00154 deleteBranch (*root_node_); 00155 leaf_count_ = 0; 00156 branch_count_ = 1; 00157 00158 tree_dirty_flag_ = false; 00159 depth_mask_ = 0; 00160 octree_depth_ = 0; 00161 } 00162 } 00163 00164 ////////////////////////////////////////////////////////////////////////////////////////////// 00165 template<typename LeafContainerT, typename BranchContainerT> void 00166 Octree2BufBase<LeafContainerT, BranchContainerT>::switchBuffers () 00167 { 00168 if (tree_dirty_flag_) 00169 { 00170 // make sure that all unused branch nodes from previous buffer are deleted 00171 treeCleanUpRecursive (root_node_); 00172 } 00173 00174 // switch butter selector 00175 buffer_selector_ = !buffer_selector_; 00176 00177 // reset flags 00178 tree_dirty_flag_ = true; 00179 leaf_count_ = 0; 00180 branch_count_ = 1; 00181 00182 unsigned char child_idx; 00183 // we can safely remove children references of root node 00184 for (child_idx = 0; child_idx < 8; child_idx++) 00185 { 00186 root_node_->setChildPtr(buffer_selector_, child_idx, 0); 00187 } 00188 } 00189 00190 ////////////////////////////////////////////////////////////////////////////////////////////// 00191 template<typename LeafContainerT, typename BranchContainerT> void 00192 Octree2BufBase<LeafContainerT, BranchContainerT>::serializeTree (std::vector<char>& binary_tree_out_arg, 00193 bool do_XOR_encoding_arg) 00194 { 00195 OctreeKey new_key; 00196 00197 // clear binary vector 00198 binary_tree_out_arg.clear (); 00199 binary_tree_out_arg.reserve (this->branch_count_); 00200 00201 serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, 0, do_XOR_encoding_arg, false); 00202 00203 // serializeTreeRecursive cleans-up unused octree nodes in previous octree 00204 tree_dirty_flag_ = false; 00205 } 00206 00207 ////////////////////////////////////////////////////////////////////////////////////////////// 00208 template<typename LeafContainerT, typename BranchContainerT> void 00209 Octree2BufBase<LeafContainerT, BranchContainerT>::serializeTree (std::vector<char>& binary_tree_out_arg, 00210 std::vector<LeafContainerT*>& leaf_container_vector_arg, 00211 bool do_XOR_encoding_arg) 00212 { 00213 OctreeKey new_key; 00214 00215 // clear output vectors 00216 binary_tree_out_arg.clear (); 00217 leaf_container_vector_arg.clear (); 00218 00219 leaf_container_vector_arg.reserve (leaf_count_); 00220 binary_tree_out_arg.reserve (this->branch_count_); 00221 00222 serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg, do_XOR_encoding_arg, false); 00223 00224 // serializeTreeRecursive cleans-up unused octree nodes in previous octree 00225 tree_dirty_flag_ = false; 00226 } 00227 00228 ////////////////////////////////////////////////////////////////////////////////////////////// 00229 template<typename LeafContainerT, typename BranchContainerT> void 00230 Octree2BufBase<LeafContainerT, BranchContainerT>::serializeLeafs (std::vector<LeafContainerT*>& leaf_container_vector_arg) 00231 { 00232 OctreeKey new_key; 00233 00234 // clear output vector 00235 leaf_container_vector_arg.clear (); 00236 00237 leaf_container_vector_arg.reserve (leaf_count_); 00238 00239 serializeTreeRecursive (root_node_, new_key, 0, &leaf_container_vector_arg, false, false); 00240 00241 // serializeLeafsRecursive cleans-up unused octree nodes in previous octree 00242 tree_dirty_flag_ = false; 00243 } 00244 00245 ////////////////////////////////////////////////////////////////////////////////////////////// 00246 template<typename LeafContainerT, typename BranchContainerT> void 00247 Octree2BufBase<LeafContainerT, BranchContainerT>::deserializeTree (std::vector<char>& binary_tree_in_arg, 00248 bool do_XOR_decoding_arg) 00249 { 00250 OctreeKey new_key; 00251 00252 // we will rebuild an octree -> reset leafCount 00253 leaf_count_ = 0; 00254 00255 // iterator for binary tree structure vector 00256 std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin (); 00257 std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end (); 00258 00259 deserializeTreeRecursive (root_node_, depth_mask_, new_key, 00260 binary_tree_in_it, binary_tree_in_it_end, 0, 0, false, 00261 do_XOR_decoding_arg); 00262 00263 // we modified the octree structure -> clean-up/tree-reset might be required 00264 tree_dirty_flag_ = false; 00265 } 00266 00267 ////////////////////////////////////////////////////////////////////////////////////////////// 00268 template<typename LeafContainerT, typename BranchContainerT> void 00269 Octree2BufBase<LeafContainerT, BranchContainerT>::deserializeTree (std::vector<char>& binary_tree_in_arg, 00270 std::vector<LeafContainerT*>& leaf_container_vector_arg, 00271 bool do_XOR_decoding_arg) 00272 { 00273 OctreeKey new_key; 00274 00275 // set data iterator to first element 00276 typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it = leaf_container_vector_arg.begin (); 00277 00278 // set data iterator to last element 00279 typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it_end = leaf_container_vector_arg.end (); 00280 00281 // we will rebuild an octree -> reset leafCount 00282 leaf_count_ = 0; 00283 00284 // iterator for binary tree structure vector 00285 std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin (); 00286 std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end (); 00287 00288 deserializeTreeRecursive (root_node_, 00289 depth_mask_, 00290 new_key, 00291 binary_tree_in_it, 00292 binary_tree_in_it_end, 00293 &leaf_container_vector_it, 00294 &leaf_container_vector_it_end, 00295 false, 00296 do_XOR_decoding_arg); 00297 00298 00299 // we modified the octree structure -> clean-up/tree-reset might be required 00300 tree_dirty_flag_ = false; 00301 } 00302 00303 00304 ////////////////////////////////////////////////////////////////////////////////////////////// 00305 template<typename LeafContainerT, typename BranchContainerT> void 00306 Octree2BufBase<LeafContainerT, BranchContainerT>::serializeNewLeafs (std::vector<LeafContainerT*>& leaf_container_vector_arg) 00307 { 00308 OctreeKey new_key; 00309 00310 // clear output vector 00311 leaf_container_vector_arg.clear (); 00312 leaf_container_vector_arg.reserve (leaf_count_); 00313 00314 serializeTreeRecursive (root_node_, new_key, 0, &leaf_container_vector_arg, false, true); 00315 00316 // serializeLeafsRecursive cleans-up unused octree nodes in previous octree buffer 00317 tree_dirty_flag_ = false; 00318 } 00319 00320 ////////////////////////////////////////////////////////////////////////////////////////////// 00321 template<typename LeafContainerT, typename BranchContainerT> 00322 unsigned int 00323 Octree2BufBase<LeafContainerT, BranchContainerT>::createLeafRecursive (const OctreeKey& key_arg, 00324 unsigned int depth_mask_arg, 00325 BranchNode* branch_arg, 00326 LeafNode*& return_leaf_arg, 00327 BranchNode*& parent_of_leaf_arg, 00328 bool branch_reset_arg) 00329 { 00330 // index to branch child 00331 unsigned char child_idx; 00332 00333 // branch reset -> this branch has been taken from previous buffer 00334 if (branch_reset_arg) 00335 { 00336 // we can safely remove children references 00337 for (child_idx = 0; child_idx < 8; child_idx++) 00338 { 00339 branch_arg->setChildPtr(buffer_selector_, child_idx, 0); 00340 } 00341 } 00342 00343 // find branch child from key 00344 child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg); 00345 00346 if (depth_mask_arg > 1) 00347 { 00348 // we have not reached maximum tree depth 00349 BranchNode* child_branch; 00350 bool doNodeReset; 00351 00352 doNodeReset = false; 00353 00354 // if required branch does not exist 00355 if (!branch_arg->hasChild(buffer_selector_, child_idx)) 00356 { 00357 // check if we find a branch node reference in previous buffer 00358 00359 if (branch_arg->hasChild(!buffer_selector_, child_idx)) 00360 { 00361 OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_,child_idx); 00362 00363 if (child_node->getNodeType()==BRANCH_NODE) { 00364 child_branch = static_cast<BranchNode*> (child_node); 00365 branch_arg->setChildPtr(buffer_selector_, child_idx, child_node); 00366 } else { 00367 // depth has changed.. child in preceeding buffer is a leaf node. 00368 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); 00369 child_branch = createBranchChild (*branch_arg, child_idx); 00370 } 00371 00372 // take child branch from previous buffer 00373 doNodeReset = true; // reset the branch pointer array of stolen child node 00374 00375 } 00376 else 00377 { 00378 // if required branch does not exist -> create it 00379 child_branch = createBranchChild (*branch_arg, child_idx); 00380 } 00381 00382 branch_count_++; 00383 } 00384 // required branch node already exists - use it 00385 else 00386 child_branch = static_cast<BranchNode*> (branch_arg->getChildPtr(buffer_selector_,child_idx)); 00387 00388 // recursively proceed with indexed child branch 00389 return createLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, return_leaf_arg, parent_of_leaf_arg, doNodeReset); 00390 } 00391 else 00392 { 00393 // branch childs are leaf nodes 00394 LeafNode* child_leaf; 00395 if (!branch_arg->hasChild(buffer_selector_, child_idx)) 00396 { 00397 // leaf node at child_idx does not exist 00398 00399 // check if we can take copy a reference from previous buffer 00400 if (branch_arg->hasChild(!buffer_selector_, child_idx)) 00401 { 00402 00403 OctreeNode * child_node = branch_arg->getChildPtr(!buffer_selector_,child_idx); 00404 if (child_node->getNodeType () == LEAF_NODE) 00405 { 00406 child_leaf = static_cast<LeafNode*> (child_node); 00407 branch_arg->setChildPtr(buffer_selector_, child_idx, child_node); 00408 } else { 00409 // depth has changed.. child in preceeding buffer is a leaf node. 00410 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); 00411 child_leaf = createLeafChild (*branch_arg, child_idx); 00412 } 00413 leaf_count_++; 00414 } 00415 else 00416 { 00417 // if required leaf does not exist -> create it 00418 child_leaf = createLeafChild (*branch_arg, child_idx); 00419 leaf_count_++; 00420 } 00421 00422 // return leaf node 00423 return_leaf_arg = child_leaf; 00424 parent_of_leaf_arg = branch_arg; 00425 } 00426 else 00427 { 00428 // leaf node already exist 00429 return_leaf_arg = static_cast<LeafNode*> (branch_arg->getChildPtr(buffer_selector_,child_idx));; 00430 parent_of_leaf_arg = branch_arg; 00431 } 00432 } 00433 00434 return depth_mask_arg; 00435 } 00436 00437 ////////////////////////////////////////////////////////////////////////////////////////////// 00438 template<typename LeafContainerT, typename BranchContainerT> void 00439 Octree2BufBase<LeafContainerT, BranchContainerT>::findLeafRecursive (const OctreeKey& key_arg, 00440 unsigned int depth_mask_arg, 00441 BranchNode* branch_arg, 00442 LeafContainerT*& result_arg) const 00443 { 00444 // return leaf node 00445 unsigned char child_idx; 00446 00447 // find branch child from key 00448 child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg); 00449 00450 if (depth_mask_arg > 1) 00451 { 00452 // we have not reached maximum tree depth 00453 BranchNode* child_branch; 00454 child_branch = static_cast<BranchNode*> (branch_arg->getChildPtr(buffer_selector_,child_idx)); 00455 00456 if (child_branch) 00457 // recursively proceed with indexed child branch 00458 findLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, result_arg); 00459 } 00460 else 00461 { 00462 // we reached leaf node level 00463 if (branch_arg->hasChild(buffer_selector_, child_idx)) 00464 { 00465 // return existing leaf node 00466 LeafNode* leaf_node = static_cast<LeafNode*> (branch_arg->getChildPtr(buffer_selector_,child_idx)); 00467 result_arg = leaf_node->getContainerPtr();; 00468 } 00469 } 00470 } 00471 00472 ////////////////////////////////////////////////////////////////////////////////////////////// 00473 template<typename LeafContainerT, typename BranchContainerT> bool 00474 Octree2BufBase<LeafContainerT, BranchContainerT>::deleteLeafRecursive (const OctreeKey& key_arg, 00475 unsigned int depth_mask_arg, 00476 BranchNode* branch_arg) 00477 { 00478 // index to branch child 00479 unsigned char child_idx; 00480 // indicates if branch is empty and can be safely removed 00481 bool bNoChilds; 00482 00483 // find branch child from key 00484 child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg); 00485 00486 if (depth_mask_arg > 1) 00487 { 00488 // we have not reached maximum tree depth 00489 BranchNode* child_branch; 00490 bool bBranchOccupied; 00491 00492 // next branch child on our path through the tree 00493 child_branch = static_cast<BranchNode*> (branch_arg->getChildPtr(buffer_selector_,child_idx)); 00494 00495 if (child_branch) 00496 { 00497 // recursively explore the indexed child branch 00498 bBranchOccupied = deleteLeafRecursive (key_arg, depth_mask_arg / 2, child_branch); 00499 00500 if (!bBranchOccupied) 00501 { 00502 // child branch does not own any sub-child nodes anymore -> delete child branch 00503 deleteBranchChild (*branch_arg, buffer_selector_, child_idx); 00504 branch_count_--; 00505 } 00506 } 00507 } 00508 else 00509 { 00510 // our child is a leaf node -> delete it 00511 deleteBranchChild (*branch_arg, buffer_selector_, child_idx); 00512 leaf_count_--; 00513 } 00514 00515 // check if current branch still owns childs 00516 bNoChilds = false; 00517 for (child_idx = 0; child_idx < 8; child_idx++) 00518 { 00519 bNoChilds = branch_arg->hasChild(buffer_selector_, child_idx); 00520 if (bNoChilds) 00521 break; 00522 } 00523 00524 // return true if current branch can be deleted 00525 return (bNoChilds); 00526 } 00527 00528 ////////////////////////////////////////////////////////////////////////////////////////////// 00529 template<typename LeafContainerT, typename BranchContainerT> void Octree2BufBase< 00530 LeafContainerT, BranchContainerT>::serializeTreeRecursive (BranchNode* branch_arg, 00531 OctreeKey& key_arg, 00532 std::vector<char>* binary_tree_out_arg, 00533 typename std::vector<LeafContainerT*>* leaf_container_vector_arg, 00534 bool do_XOR_encoding_arg, 00535 bool new_leafs_filter_arg) 00536 { 00537 // child iterator 00538 unsigned char child_idx; 00539 00540 // bit pattern 00541 char branch_bit_pattern_curr_buffer; 00542 char branch_bit_pattern_prev_buffer; 00543 char node_XOR_bit_pattern; 00544 00545 // occupancy bit patterns of branch node (current and previous octree buffer) 00546 branch_bit_pattern_curr_buffer = getBranchBitPattern (*branch_arg, buffer_selector_); 00547 branch_bit_pattern_prev_buffer = getBranchBitPattern (*branch_arg, !buffer_selector_); 00548 00549 // XOR of current and previous occupancy bit patterns 00550 node_XOR_bit_pattern = branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer; 00551 00552 if (binary_tree_out_arg) 00553 { 00554 if (do_XOR_encoding_arg) 00555 { 00556 // write XOR bit pattern to output vector 00557 binary_tree_out_arg->push_back (node_XOR_bit_pattern); 00558 } 00559 else 00560 { 00561 // write bit pattern of current buffer to output vector 00562 binary_tree_out_arg->push_back (branch_bit_pattern_curr_buffer); 00563 } 00564 } 00565 00566 // iterate over all children 00567 for (child_idx = 0; child_idx < 8; child_idx++) 00568 { 00569 if (branch_arg->hasChild(buffer_selector_, child_idx)) 00570 { 00571 // add current branch voxel to key 00572 key_arg.pushBranch(child_idx); 00573 00574 OctreeNode *child_node = branch_arg->getChildPtr(buffer_selector_,child_idx); 00575 00576 switch (child_node->getNodeType ()) 00577 { 00578 case BRANCH_NODE: 00579 { 00580 // recursively proceed with indexed child branch 00581 serializeTreeRecursive (static_cast<BranchNode*> (child_node), key_arg, binary_tree_out_arg, 00582 leaf_container_vector_arg, do_XOR_encoding_arg, new_leafs_filter_arg); 00583 break; 00584 } 00585 case LEAF_NODE: 00586 { 00587 LeafNode* child_leaf = static_cast<LeafNode*> (child_node); 00588 00589 if (new_leafs_filter_arg) 00590 { 00591 if (!branch_arg->hasChild (!buffer_selector_, child_idx)) 00592 { 00593 if (leaf_container_vector_arg) 00594 leaf_container_vector_arg->push_back(child_leaf->getContainerPtr()); 00595 00596 serializeTreeCallback (**child_leaf, key_arg); 00597 } 00598 } else 00599 { 00600 00601 if (leaf_container_vector_arg) 00602 leaf_container_vector_arg->push_back(child_leaf->getContainerPtr()); 00603 00604 serializeTreeCallback (**child_leaf, key_arg); 00605 } 00606 00607 break; 00608 } 00609 default: 00610 break; 00611 } 00612 00613 // pop current branch voxel from key 00614 key_arg.popBranch(); 00615 } 00616 else if (branch_arg->hasChild (!buffer_selector_, child_idx)) 00617 { 00618 // delete branch, free memory 00619 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); 00620 00621 } 00622 00623 } 00624 } 00625 00626 00627 ////////////////////////////////////////////////////////////////////////////////////////////// 00628 template<typename LeafContainerT, typename BranchContainerT> void 00629 Octree2BufBase<LeafContainerT, BranchContainerT>::deserializeTreeRecursive (BranchNode* branch_arg, 00630 unsigned int depth_mask_arg, OctreeKey& key_arg, 00631 typename std::vector<char>::const_iterator& binaryTreeIT_arg, 00632 typename std::vector<char>::const_iterator& binaryTreeIT_End_arg, 00633 typename std::vector<LeafContainerT*>::const_iterator* dataVectorIterator_arg, 00634 typename std::vector<LeafContainerT*>::const_iterator* dataVectorEndIterator_arg, 00635 bool branch_reset_arg, bool do_XOR_decoding_arg) 00636 { 00637 // child iterator 00638 unsigned char child_idx; 00639 00640 // node bits 00641 char nodeBits; 00642 char recoveredNodeBits; 00643 00644 // branch reset -> this branch has been taken from previous buffer 00645 if (branch_reset_arg) 00646 { 00647 // we can safely remove children references 00648 for (child_idx = 0; child_idx < 8; child_idx++) 00649 { 00650 branch_arg->setChildPtr(buffer_selector_, child_idx, 0); 00651 } 00652 } 00653 00654 if (binaryTreeIT_arg!=binaryTreeIT_End_arg) { 00655 // read branch occupancy bit pattern from vector 00656 nodeBits = *binaryTreeIT_arg++; 00657 00658 00659 // recover branch occupancy bit pattern 00660 if (do_XOR_decoding_arg) 00661 { 00662 recoveredNodeBits = getBranchBitPattern (*branch_arg, !buffer_selector_) ^ nodeBits; 00663 } 00664 else 00665 { 00666 recoveredNodeBits = nodeBits; 00667 } 00668 00669 // iterate over all children 00670 for (child_idx = 0; child_idx < 8; child_idx++) 00671 { 00672 // if occupancy bit for child_idx is set.. 00673 if (recoveredNodeBits & (1 << child_idx)) 00674 { 00675 // add current branch voxel to key 00676 key_arg.pushBranch(child_idx); 00677 00678 bool doNodeReset; 00679 00680 doNodeReset = false; 00681 00682 if (depth_mask_arg > 1) 00683 { 00684 // we have not reached maximum tree depth 00685 00686 BranchNode* child_branch; 00687 00688 // check if we find a branch node reference in previous buffer 00689 if (!branch_arg->hasChild(buffer_selector_, child_idx)) 00690 { 00691 00692 if (branch_arg->hasChild(!buffer_selector_, child_idx)) 00693 { 00694 OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_,child_idx); 00695 00696 if (child_node->getNodeType()==BRANCH_NODE) { 00697 child_branch = static_cast<BranchNode*> (child_node); 00698 branch_arg->setChildPtr(buffer_selector_, child_idx, child_node); 00699 } else { 00700 // depth has changed.. child in preceeding buffer is a leaf node. 00701 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); 00702 child_branch = createBranchChild (*branch_arg, child_idx); 00703 } 00704 00705 // take child branch from previous buffer 00706 doNodeReset = true; // reset the branch pointer array of stolen child node 00707 } 00708 else 00709 { 00710 // if required branch does not exist -> create it 00711 child_branch = createBranchChild (*branch_arg, child_idx); 00712 } 00713 00714 branch_count_++; 00715 00716 } 00717 else 00718 { 00719 // required branch node already exists - use it 00720 child_branch = static_cast<BranchNode*> (branch_arg->getChildPtr(buffer_selector_,child_idx)); 00721 } 00722 00723 // recursively proceed with indexed child branch 00724 deserializeTreeRecursive (child_branch, depth_mask_arg / 2, key_arg, 00725 binaryTreeIT_arg, binaryTreeIT_End_arg, 00726 dataVectorIterator_arg, dataVectorEndIterator_arg, 00727 doNodeReset, do_XOR_decoding_arg); 00728 00729 } 00730 else 00731 { 00732 // branch childs are leaf nodes 00733 LeafNode* child_leaf; 00734 00735 // check if we can take copy a reference pointer from previous buffer 00736 if (branch_arg->hasChild(!buffer_selector_, child_idx)) 00737 { 00738 // take child leaf node from previous buffer 00739 OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_,child_idx); 00740 if (child_node->getNodeType()==LEAF_NODE) { 00741 child_leaf = static_cast<LeafNode*> (child_node); 00742 branch_arg->setChildPtr(buffer_selector_, child_idx, child_node); 00743 } else { 00744 // depth has changed.. child in preceeding buffer is a leaf node. 00745 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); 00746 child_leaf = createLeafChild (*branch_arg, child_idx); 00747 } 00748 } 00749 else 00750 { 00751 // if required leaf does not exist -> create it 00752 child_leaf = createLeafChild (*branch_arg, child_idx); 00753 } 00754 00755 // we reached leaf node level 00756 00757 if (dataVectorIterator_arg 00758 && (*dataVectorIterator_arg != *dataVectorEndIterator_arg)) 00759 { 00760 LeafContainerT& container = **child_leaf; 00761 container = ***dataVectorIterator_arg; 00762 ++*dataVectorIterator_arg; 00763 } 00764 00765 leaf_count_++; 00766 00767 // execute deserialization callback 00768 deserializeTreeCallback (**child_leaf, key_arg); 00769 00770 } 00771 00772 // pop current branch voxel from key 00773 key_arg.popBranch(); 00774 } 00775 else if (branch_arg->hasChild (!buffer_selector_, child_idx)) 00776 { 00777 // remove old branch pointer information in current branch 00778 branch_arg->setChildPtr(buffer_selector_, child_idx, 0); 00779 00780 // remove unused branches in previous buffer 00781 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); 00782 } 00783 } 00784 } 00785 00786 } 00787 00788 ////////////////////////////////////////////////////////////////////////////////////////////// 00789 template<typename LeafContainerT, typename BranchContainerT> void 00790 Octree2BufBase<LeafContainerT, BranchContainerT>::treeCleanUpRecursive (BranchNode* branch_arg) 00791 { 00792 // child iterator 00793 unsigned char child_idx; 00794 00795 // bit pattern 00796 char occupied_children_bit_pattern_prev_buffer; 00797 char node_XOR_bit_pattern; 00798 char unused_branches_bit_pattern; 00799 00800 // occupancy bit pattern of branch node (previous octree buffer) 00801 occupied_children_bit_pattern_prev_buffer = getBranchBitPattern (*branch_arg, !buffer_selector_); 00802 00803 // XOR of current and previous occupancy bit patterns 00804 node_XOR_bit_pattern = getBranchXORBitPattern (*branch_arg); 00805 00806 // bit pattern indicating unused octree nodes in previous branch 00807 unused_branches_bit_pattern = node_XOR_bit_pattern & occupied_children_bit_pattern_prev_buffer; 00808 00809 // iterate over all children 00810 for (child_idx = 0; child_idx < 8; child_idx++) 00811 { 00812 if (branch_arg->hasChild(buffer_selector_, child_idx)) 00813 { 00814 OctreeNode *child_node = branch_arg->getChildPtr(buffer_selector_,child_idx); 00815 00816 switch (child_node->getNodeType ()) 00817 { 00818 case BRANCH_NODE: 00819 { 00820 // recursively proceed with indexed child branch 00821 treeCleanUpRecursive (static_cast<BranchNode*> (child_node)); 00822 break; 00823 } 00824 case LEAF_NODE: 00825 // leaf level - nothing to do.. 00826 break; 00827 default: 00828 break; 00829 } 00830 } 00831 00832 // check for unused branches in previous buffer 00833 if (unused_branches_bit_pattern & (1 << child_idx)) 00834 { 00835 // delete branch, free memory 00836 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx); 00837 } 00838 } 00839 } 00840 } 00841 } 00842 00843 #define PCL_INSTANTIATE_Octree2BufBase(T) template class PCL_EXPORTS pcl::octree::Octree2BufBase<T>; 00844 00845 #endif 00846