39 #ifndef PCL_OCTREE_BASE_HPP
40 #define PCL_OCTREE_BASE_HPP
44 #include <pcl/impl/instantiate.hpp>
51 template<
typename LeafContainerT,
typename BranchContainerT>
58 dynamic_depth_enabled_ (false),
64 template<
typename LeafContainerT,
typename BranchContainerT>
73 template<
typename LeafContainerT,
typename BranchContainerT>
77 unsigned int tree_depth;
79 assert(max_voxel_index_arg>0);
82 tree_depth = std::min (
static_cast<unsigned int> (OctreeKey::maxDepth),
static_cast<unsigned int> (std::ceil (Log2 (max_voxel_index_arg))));
85 depth_mask_ = (1 << (tree_depth - 1));
89 template<
typename LeafContainerT,
typename BranchContainerT>
96 octree_depth_ = depth_arg;
99 depth_mask_ = (1 << (depth_arg - 1));
102 max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
106 template<
typename LeafContainerT,
typename BranchContainerT>
109 unsigned int idx_y_arg,
110 unsigned int idx_z_arg)
113 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
116 return (findLeaf (key));
120 template<
typename LeafContainerT,
typename BranchContainerT>
123 unsigned int idx_y_arg,
124 unsigned int idx_z_arg)
127 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
130 return (createLeaf (key));
134 template<
typename LeafContainerT,
typename BranchContainerT>
137 unsigned int idx_y_arg,
138 unsigned int idx_z_arg)
const
141 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
144 return (existLeaf (key));
148 template<
typename LeafContainerT,
typename BranchContainerT>
151 unsigned int idx_y_arg,
152 unsigned int idx_z_arg)
155 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
158 deleteLeafRecursive (key, depth_mask_, root_node_);
162 template<
typename LeafContainerT,
typename BranchContainerT>
170 deleteBranch (*root_node_);
178 template<
typename LeafContainerT,
typename BranchContainerT>
186 binary_tree_out_arg.clear ();
187 binary_tree_out_arg.reserve (this->branch_count_);
189 serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, 0);
193 template<
typename LeafContainerT,
typename BranchContainerT>
196 std::vector<LeafContainerT*>& leaf_container_vector_arg)
202 binary_tree_out_arg.clear ();
203 leaf_container_vector_arg.clear ();
205 binary_tree_out_arg.reserve (this->branch_count_);
206 leaf_container_vector_arg.reserve (this->leaf_count_);
208 serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg);
212 template<
typename LeafContainerT,
typename BranchContainerT>
219 leaf_container_vector_arg.clear ();
221 leaf_container_vector_arg.reserve (this->leaf_count_);
223 serializeTreeRecursive (root_node_, new_key, 0, &leaf_container_vector_arg);
227 template<
typename LeafContainerT,
typename BranchContainerT>
237 std::vector<char>::const_iterator binary_tree_out_it = binary_tree_out_arg.begin ();
238 std::vector<char>::const_iterator binary_tree_out_it_end = binary_tree_out_arg.end ();
240 deserializeTreeRecursive (root_node_,
244 binary_tree_out_it_end,
251 template<
typename LeafContainerT,
typename BranchContainerT>
254 std::vector<LeafContainerT*>& leaf_container_vector_arg)
259 typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it = leaf_container_vector_arg.begin ();
262 typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it_end = leaf_container_vector_arg.end ();
268 std::vector<char>::const_iterator binary_tree_input_it = binary_tree_in_arg.begin ();
269 std::vector<char>::const_iterator binary_tree_input_it_end = binary_tree_in_arg.end ();
271 deserializeTreeRecursive (root_node_,
274 binary_tree_input_it,
275 binary_tree_input_it_end,
277 &leaf_vector_it_end);
282 template<
typename LeafContainerT,
typename BranchContainerT>
285 unsigned int depth_mask_arg,
291 unsigned char child_idx;
296 OctreeNode* child_node = (*branch_arg)[child_idx];
300 if ((!dynamic_depth_enabled_) && (depth_mask_arg > 1))
303 BranchNode* childBranch = createBranchChild (*branch_arg, child_idx);
308 return createLeafRecursive (key_arg, depth_mask_arg / 2, childBranch, return_leaf_arg, parent_of_leaf_arg);
314 LeafNode* leaf_node = createLeafChild (*branch_arg, child_idx);
315 return_leaf_arg = leaf_node;
316 parent_of_leaf_arg = branch_arg;
327 return createLeafRecursive (key_arg, depth_mask_arg / 2,
static_cast<BranchNode*
> (child_node),
328 return_leaf_arg, parent_of_leaf_arg);
332 return_leaf_arg =
static_cast<LeafNode*
> (child_node);;
333 parent_of_leaf_arg = branch_arg;
339 return (depth_mask_arg >> 1);
343 template<
typename LeafContainerT,
typename BranchContainerT>
346 unsigned int depth_mask_arg,
348 LeafContainerT*& result_arg)
const
351 unsigned char child_idx;
356 OctreeNode* child_node = (*branch_arg)[child_idx];
365 child_branch =
static_cast<BranchNode*
> (child_node);
367 findLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, result_arg);
373 child_leaf =
static_cast<LeafNode*
> (child_node);
382 template<
typename LeafContainerT,
typename BranchContainerT>
385 unsigned int depth_mask_arg,
389 unsigned char child_idx;
396 OctreeNode* child_node = (*branch_arg)[child_idx];
405 child_branch =
static_cast<BranchNode*
> (child_node);
408 b_no_children = deleteLeafRecursive (key_arg, depth_mask_arg / 2, child_branch);
413 deleteBranchChild (*branch_arg, child_idx);
422 deleteBranchChild (*branch_arg, child_idx);
429 b_no_children =
false;
430 for (child_idx = 0; (!b_no_children) && (child_idx < 8); child_idx++)
432 b_no_children = branch_arg->
hasChild (child_idx);
435 return (b_no_children);
439 template<
typename LeafContainerT,
typename BranchContainerT>
443 std::vector<char>* binary_tree_out_arg,
444 typename std::vector<LeafContainerT*>* leaf_container_vector_arg)
const
448 unsigned char child_idx;
449 char node_bit_pattern;
452 node_bit_pattern = getBranchBitPattern (*branch_arg);
455 if (binary_tree_out_arg)
456 binary_tree_out_arg->push_back (node_bit_pattern);
459 for (child_idx = 0; child_idx < 8; child_idx++)
463 if (branch_arg->
hasChild (child_idx))
475 serializeTreeRecursive (
static_cast<const BranchNode*
> (childNode), key_arg, binary_tree_out_arg,
476 leaf_container_vector_arg);
483 if (leaf_container_vector_arg)
487 serializeTreeCallback (**child_leaf, key_arg);
501 template<
typename LeafContainerT,
typename BranchContainerT>
504 typename std::vector<char>::const_iterator& binary_tree_input_it_arg,
505 typename std::vector<char>::const_iterator& binary_tree_input_it_end_arg,
506 typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_arg,
507 typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_end_arg)
510 unsigned char child_idx;
513 if (binary_tree_input_it_arg != binary_tree_input_it_end_arg)
516 node_bits = (*binary_tree_input_it_arg);
517 binary_tree_input_it_arg++;
520 for (child_idx = 0; child_idx < 8; child_idx++)
523 if (node_bits & (1 << child_idx))
528 if (depth_mask_arg > 1)
531 BranchNode * newBranch = createBranchChild (*branch_arg, child_idx);
536 deserializeTreeRecursive (newBranch, depth_mask_arg / 2, key_arg,
537 binary_tree_input_it_arg, binary_tree_input_it_end_arg,
538 leaf_container_vector_it_arg, leaf_container_vector_it_end_arg);
544 LeafNode* child_leaf = createLeafChild (*branch_arg, child_idx);
546 if (leaf_container_vector_it_arg && (*leaf_container_vector_it_arg != *leaf_container_vector_it_end_arg))
548 LeafContainerT& container = **child_leaf;
549 container = ***leaf_container_vector_it_arg;
550 ++*leaf_container_vector_it_arg;
556 deserializeTreeCallback (**child_leaf, key_arg);
569 #define PCL_INSTANTIATE_OctreeBase(T) template class PCL_EXPORTS pcl::octree::OctreeBase<T>;