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-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$ 00037 */ 00038 00039 #ifndef PCL_OCTREE_BASE_HPP 00040 #define PCL_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 template<typename LeafContainerT, typename BranchContainerT> 00054 OctreeBase<LeafContainerT, BranchContainerT>::OctreeBase () : 00055 leaf_count_ (0), 00056 branch_count_ (1), 00057 root_node_ (new BranchNode ()), 00058 depth_mask_ (0), 00059 octree_depth_ (0), 00060 dynamic_depth_enabled_ (false), 00061 max_key_ () 00062 { 00063 } 00064 00065 ////////////////////////////////////////////////////////////////////////////////////////////// 00066 template<typename LeafContainerT, typename BranchContainerT> 00067 OctreeBase<LeafContainerT, BranchContainerT>::~OctreeBase () 00068 { 00069 // deallocate tree structure 00070 deleteTree (); 00071 delete (root_node_); 00072 } 00073 00074 ////////////////////////////////////////////////////////////////////////////////////////////// 00075 template<typename LeafContainerT, typename BranchContainerT> 00076 void 00077 OctreeBase<LeafContainerT, BranchContainerT>::setMaxVoxelIndex (unsigned int max_voxel_index_arg) 00078 { 00079 unsigned int tree_depth; 00080 00081 assert(max_voxel_index_arg>0); 00082 00083 // tree depth == bitlength of maxVoxels 00084 tree_depth = std::max ( (std::min (static_cast<unsigned int> (OctreeKey::maxDepth), static_cast<unsigned int> (std::ceil (Log2 (max_voxel_index_arg))))), 0u); 00085 00086 // define depthMask_ by setting a single bit to 1 at bit position == tree depth 00087 depth_mask_ = (1 << (tree_depth - 1)); 00088 } 00089 00090 ////////////////////////////////////////////////////////////////////////////////////////////// 00091 template<typename LeafContainerT, typename BranchContainerT> 00092 void 00093 OctreeBase<LeafContainerT, BranchContainerT>::setTreeDepth (unsigned int depth_arg) 00094 { 00095 assert(depth_arg>0); 00096 00097 // set octree depth 00098 octree_depth_ = depth_arg; 00099 00100 // define depthMask_ by setting a single bit to 1 at bit position == tree depth 00101 depth_mask_ = (1 << (depth_arg - 1)); 00102 00103 // define max. keys 00104 max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1; 00105 } 00106 00107 ////////////////////////////////////////////////////////////////////////////////////////////// 00108 template<typename LeafContainerT, typename BranchContainerT> 00109 LeafContainerT* 00110 OctreeBase<LeafContainerT, BranchContainerT>::findLeaf (unsigned int idx_x_arg, 00111 unsigned int idx_y_arg, 00112 unsigned int idx_z_arg) 00113 { 00114 // generate key 00115 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg); 00116 00117 // check if key exist in octree 00118 return (findLeaf (key)); 00119 } 00120 00121 ////////////////////////////////////////////////////////////////////////////////////////////// 00122 template<typename LeafContainerT, typename BranchContainerT> 00123 LeafContainerT* 00124 OctreeBase<LeafContainerT, BranchContainerT>::createLeaf (unsigned int idx_x_arg, 00125 unsigned int idx_y_arg, 00126 unsigned int idx_z_arg) 00127 { 00128 // generate key 00129 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg); 00130 00131 // check if key exist in octree 00132 return (createLeaf (key)); 00133 } 00134 00135 ////////////////////////////////////////////////////////////////////////////////////////////// 00136 template<typename LeafContainerT, typename BranchContainerT> 00137 bool 00138 OctreeBase<LeafContainerT, BranchContainerT>::existLeaf (unsigned int idx_x_arg, 00139 unsigned int idx_y_arg, 00140 unsigned int idx_z_arg) const 00141 { 00142 // generate key 00143 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg); 00144 00145 // check if key exist in octree 00146 return (existLeaf (key)); 00147 } 00148 00149 ////////////////////////////////////////////////////////////////////////////////////////////// 00150 template<typename LeafContainerT, typename BranchContainerT> 00151 void 00152 OctreeBase<LeafContainerT, BranchContainerT>::removeLeaf (unsigned int idx_x_arg, 00153 unsigned int idx_y_arg, 00154 unsigned int idx_z_arg) 00155 { 00156 // generate key 00157 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg); 00158 00159 // check if key exist in octree 00160 deleteLeafRecursive (key, depth_mask_, root_node_); 00161 } 00162 00163 ////////////////////////////////////////////////////////////////////////////////////////////// 00164 template<typename LeafContainerT, typename BranchContainerT> 00165 void 00166 OctreeBase<LeafContainerT, BranchContainerT>::deleteTree () 00167 { 00168 00169 if (root_node_) 00170 { 00171 // reset octree 00172 deleteBranch (*root_node_); 00173 leaf_count_ = 0; 00174 branch_count_ = 1; 00175 } 00176 00177 } 00178 00179 ////////////////////////////////////////////////////////////////////////////////////////////// 00180 template<typename LeafContainerT, typename BranchContainerT> 00181 void 00182 OctreeBase<LeafContainerT, BranchContainerT>::serializeTree (std::vector<char>& binary_tree_out_arg) 00183 { 00184 00185 OctreeKey new_key; 00186 00187 // clear binary vector 00188 binary_tree_out_arg.clear (); 00189 binary_tree_out_arg.reserve (this->branch_count_); 00190 00191 serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, 0); 00192 } 00193 00194 ////////////////////////////////////////////////////////////////////////////////////////////// 00195 template<typename LeafContainerT, typename BranchContainerT> 00196 void 00197 OctreeBase<LeafContainerT, BranchContainerT>::serializeTree (std::vector<char>& binary_tree_out_arg, 00198 std::vector<LeafContainerT*>& leaf_container_vector_arg) 00199 { 00200 00201 OctreeKey new_key; 00202 00203 // clear output vectors 00204 binary_tree_out_arg.clear (); 00205 leaf_container_vector_arg.clear (); 00206 00207 binary_tree_out_arg.reserve (this->branch_count_); 00208 leaf_container_vector_arg.reserve (this->leaf_count_); 00209 00210 serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg); 00211 } 00212 00213 ////////////////////////////////////////////////////////////////////////////////////////////// 00214 template<typename LeafContainerT, typename BranchContainerT> 00215 void 00216 OctreeBase<LeafContainerT, BranchContainerT>::serializeLeafs (std::vector<LeafContainerT*>& leaf_container_vector_arg) 00217 { 00218 OctreeKey new_key; 00219 00220 // clear output vector 00221 leaf_container_vector_arg.clear (); 00222 00223 leaf_container_vector_arg.reserve (this->leaf_count_); 00224 00225 serializeTreeRecursive (root_node_, new_key, 0, &leaf_container_vector_arg); 00226 } 00227 00228 ////////////////////////////////////////////////////////////////////////////////////////////// 00229 template<typename LeafContainerT, typename BranchContainerT> 00230 void 00231 OctreeBase<LeafContainerT, BranchContainerT>::deserializeTree (std::vector<char>& binary_tree_out_arg) 00232 { 00233 OctreeKey new_key; 00234 00235 // free existing tree before tree rebuild 00236 deleteTree (); 00237 00238 //iterator for binary tree structure vector 00239 std::vector<char>::const_iterator binary_tree_out_it = binary_tree_out_arg.begin (); 00240 std::vector<char>::const_iterator binary_tree_out_it_end = binary_tree_out_arg.end (); 00241 00242 deserializeTreeRecursive (root_node_, 00243 depth_mask_, 00244 new_key, 00245 binary_tree_out_it, 00246 binary_tree_out_it_end, 00247 0, 00248 0); 00249 00250 } 00251 00252 ////////////////////////////////////////////////////////////////////////////////////////////// 00253 template<typename LeafContainerT, typename BranchContainerT> 00254 void 00255 OctreeBase<LeafContainerT, BranchContainerT>::deserializeTree (std::vector<char>& binary_tree_in_arg, 00256 std::vector<LeafContainerT*>& leaf_container_vector_arg) 00257 { 00258 OctreeKey new_key; 00259 00260 // set data iterator to first element 00261 typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it = leaf_container_vector_arg.begin (); 00262 00263 // set data iterator to last element 00264 typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it_end = leaf_container_vector_arg.end (); 00265 00266 // free existing tree before tree rebuild 00267 deleteTree (); 00268 00269 //iterator for binary tree structure vector 00270 std::vector<char>::const_iterator binary_tree_input_it = binary_tree_in_arg.begin (); 00271 std::vector<char>::const_iterator binary_tree_input_it_end = binary_tree_in_arg.end (); 00272 00273 deserializeTreeRecursive (root_node_, 00274 depth_mask_, 00275 new_key, 00276 binary_tree_input_it, 00277 binary_tree_input_it_end, 00278 &leaf_vector_it, 00279 &leaf_vector_it_end); 00280 00281 } 00282 00283 ////////////////////////////////////////////////////////////////////////////////////////////// 00284 template<typename LeafContainerT, typename BranchContainerT> 00285 unsigned int 00286 OctreeBase<LeafContainerT, BranchContainerT>::createLeafRecursive (const OctreeKey& key_arg, 00287 unsigned int depth_mask_arg, 00288 BranchNode* branch_arg, 00289 LeafNode*& return_leaf_arg, 00290 BranchNode*& parent_of_leaf_arg) 00291 { 00292 // index to branch child 00293 unsigned char child_idx; 00294 00295 // find branch child from key 00296 child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg); 00297 00298 OctreeNode* child_node = (*branch_arg)[child_idx]; 00299 00300 if (!child_node) 00301 { 00302 if ((!dynamic_depth_enabled_) && (depth_mask_arg > 1)) 00303 { 00304 // if required branch does not exist -> create it 00305 BranchNode* childBranch = createBranchChild (*branch_arg, child_idx); 00306 00307 branch_count_++; 00308 00309 // recursively proceed with indexed child branch 00310 return createLeafRecursive (key_arg, depth_mask_arg / 2, childBranch, return_leaf_arg, parent_of_leaf_arg); 00311 00312 } 00313 else 00314 { 00315 // if leaf node at child_idx does not exist 00316 LeafNode* leaf_node = createLeafChild (*branch_arg, child_idx); 00317 return_leaf_arg = leaf_node; 00318 parent_of_leaf_arg = branch_arg; 00319 this->leaf_count_++; 00320 } 00321 } 00322 else 00323 { 00324 // Node exists already 00325 switch (child_node->getNodeType ()) 00326 { 00327 case BRANCH_NODE: 00328 // recursively proceed with indexed child branch 00329 return createLeafRecursive (key_arg, depth_mask_arg / 2, static_cast<BranchNode*> (child_node), 00330 return_leaf_arg, parent_of_leaf_arg); 00331 break; 00332 00333 case LEAF_NODE: 00334 return_leaf_arg = static_cast<LeafNode*> (child_node);; 00335 parent_of_leaf_arg = branch_arg; 00336 break; 00337 } 00338 00339 } 00340 00341 return (depth_mask_arg >> 1); 00342 } 00343 00344 ////////////////////////////////////////////////////////////////////////////////////////////// 00345 template<typename LeafContainerT, typename BranchContainerT> 00346 void 00347 OctreeBase<LeafContainerT, BranchContainerT>::findLeafRecursive (const OctreeKey& key_arg, 00348 unsigned int depth_mask_arg, 00349 BranchNode* branch_arg, 00350 LeafContainerT*& result_arg) const 00351 { 00352 // index to branch child 00353 unsigned char child_idx; 00354 00355 // find branch child from key 00356 child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg); 00357 00358 OctreeNode* child_node = (*branch_arg)[child_idx]; 00359 00360 if (child_node) 00361 { 00362 switch (child_node->getNodeType ()) 00363 { 00364 case BRANCH_NODE: 00365 // we have not reached maximum tree depth 00366 BranchNode* child_branch; 00367 child_branch = static_cast<BranchNode*> (child_node); 00368 00369 findLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, result_arg); 00370 break; 00371 00372 case LEAF_NODE: 00373 // return existing leaf node 00374 LeafNode* child_leaf; 00375 child_leaf = static_cast<LeafNode*> (child_node); 00376 00377 result_arg = child_leaf->getContainerPtr (); 00378 break; 00379 } 00380 } 00381 } 00382 00383 ////////////////////////////////////////////////////////////////////////////////////////////// 00384 template<typename LeafContainerT, typename BranchContainerT> 00385 bool 00386 OctreeBase<LeafContainerT, BranchContainerT>::deleteLeafRecursive (const OctreeKey& key_arg, 00387 unsigned int depth_mask_arg, 00388 BranchNode* branch_arg) 00389 { 00390 // index to branch child 00391 unsigned char child_idx; 00392 // indicates if branch is empty and can be safely removed 00393 bool b_no_children; 00394 00395 // find branch child from key 00396 child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg); 00397 00398 OctreeNode* child_node = (*branch_arg)[child_idx]; 00399 00400 if (child_node) 00401 { 00402 switch (child_node->getNodeType ()) 00403 { 00404 00405 case BRANCH_NODE: 00406 BranchNode* child_branch; 00407 child_branch = static_cast<BranchNode*> (child_node); 00408 00409 // recursively explore the indexed child branch 00410 b_no_children = deleteLeafRecursive (key_arg, depth_mask_arg / 2, child_branch); 00411 00412 if (!b_no_children) 00413 { 00414 // child branch does not own any sub-child nodes anymore -> delete child branch 00415 deleteBranchChild (*branch_arg, child_idx); 00416 branch_count_--; 00417 } 00418 break; 00419 00420 case LEAF_NODE: 00421 // return existing leaf node 00422 00423 // our child is a leaf node -> delete it 00424 deleteBranchChild (*branch_arg, child_idx); 00425 this->leaf_count_--; 00426 break; 00427 } 00428 } 00429 00430 // check if current branch still owns children 00431 b_no_children = false; 00432 for (child_idx = 0; (!b_no_children) && (child_idx < 8); child_idx++) 00433 { 00434 b_no_children = branch_arg->hasChild (child_idx); 00435 } 00436 // return true if current branch can be deleted 00437 return (b_no_children); 00438 } 00439 00440 ////////////////////////////////////////////////////////////////////////////////////////////// 00441 template<typename LeafContainerT, typename BranchContainerT> 00442 void 00443 OctreeBase<LeafContainerT, BranchContainerT>::serializeTreeRecursive (const BranchNode* branch_arg, 00444 OctreeKey& key_arg, 00445 std::vector<char>* binary_tree_out_arg, 00446 typename std::vector<LeafContainerT*>* leaf_container_vector_arg) const 00447 { 00448 00449 // child iterator 00450 unsigned char child_idx; 00451 char node_bit_pattern; 00452 00453 // branch occupancy bit pattern 00454 node_bit_pattern = getBranchBitPattern (*branch_arg); 00455 00456 // write bit pattern to output vector 00457 if (binary_tree_out_arg) 00458 binary_tree_out_arg->push_back (node_bit_pattern); 00459 00460 // iterate over all children 00461 for (child_idx = 0; child_idx < 8; child_idx++) 00462 { 00463 00464 // if child exist 00465 if (branch_arg->hasChild (child_idx)) 00466 { 00467 // add current branch voxel to key 00468 key_arg.pushBranch (child_idx); 00469 00470 OctreeNode *childNode = branch_arg->getChildPtr (child_idx); 00471 00472 switch (childNode->getNodeType ()) 00473 { 00474 case BRANCH_NODE: 00475 { 00476 // recursively proceed with indexed child branch 00477 serializeTreeRecursive (static_cast<const BranchNode*> (childNode), key_arg, binary_tree_out_arg, 00478 leaf_container_vector_arg); 00479 break; 00480 } 00481 case LEAF_NODE: 00482 { 00483 LeafNode* child_leaf = static_cast<LeafNode*> (childNode); 00484 00485 if (leaf_container_vector_arg) 00486 leaf_container_vector_arg->push_back (child_leaf->getContainerPtr ()); 00487 00488 // we reached a leaf node -> execute serialization callback 00489 serializeTreeCallback (**child_leaf, key_arg); 00490 break; 00491 } 00492 default: 00493 break; 00494 } 00495 00496 // pop current branch voxel from key 00497 key_arg.popBranch (); 00498 } 00499 } 00500 } 00501 00502 ////////////////////////////////////////////////////////////////////////////////////////////// 00503 template<typename LeafContainerT, typename BranchContainerT> 00504 void 00505 OctreeBase<LeafContainerT, BranchContainerT>::deserializeTreeRecursive (BranchNode* branch_arg, unsigned int depth_mask_arg, OctreeKey& key_arg, 00506 typename std::vector<char>::const_iterator& binary_tree_input_it_arg, 00507 typename std::vector<char>::const_iterator& binary_tree_input_it_end_arg, 00508 typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_arg, 00509 typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_end_arg) 00510 { 00511 // child iterator 00512 unsigned char child_idx; 00513 char node_bits; 00514 00515 if (binary_tree_input_it_arg != binary_tree_input_it_end_arg) 00516 { 00517 // read branch occupancy bit pattern from input vector 00518 node_bits = (*binary_tree_input_it_arg); 00519 binary_tree_input_it_arg++; 00520 00521 // iterate over all children 00522 for (child_idx = 0; child_idx < 8; child_idx++) 00523 { 00524 // if occupancy bit for child_idx is set.. 00525 if (node_bits & (1 << child_idx)) 00526 { 00527 // add current branch voxel to key 00528 key_arg.pushBranch (child_idx); 00529 00530 if (depth_mask_arg > 1) 00531 { 00532 // we have not reached maximum tree depth 00533 BranchNode * newBranch = createBranchChild (*branch_arg, child_idx); 00534 00535 branch_count_++; 00536 00537 // recursively proceed with new child branch 00538 deserializeTreeRecursive (newBranch, depth_mask_arg / 2, key_arg, 00539 binary_tree_input_it_arg, binary_tree_input_it_end_arg, 00540 leaf_container_vector_it_arg, leaf_container_vector_it_end_arg); 00541 } 00542 else 00543 { 00544 // we reached leaf node level 00545 00546 LeafNode* child_leaf = createLeafChild (*branch_arg, child_idx); 00547 00548 if (leaf_container_vector_it_arg && (*leaf_container_vector_it_arg != *leaf_container_vector_it_end_arg)) 00549 { 00550 LeafContainerT& container = **child_leaf; 00551 container = ***leaf_container_vector_it_arg; 00552 ++*leaf_container_vector_it_arg; 00553 } 00554 00555 leaf_count_++; 00556 00557 // execute deserialization callback 00558 deserializeTreeCallback (**child_leaf, key_arg); 00559 } 00560 00561 // pop current branch voxel from key 00562 key_arg.popBranch (); 00563 } 00564 } 00565 } 00566 } 00567 00568 } 00569 } 00570 00571 #define PCL_INSTANTIATE_OctreeBase(T) template class PCL_EXPORTS pcl::octree::OctreeBase<T>; 00572 00573 #endif 00574