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)
63 template<
typename LeafContainerT,
typename BranchContainerT>
72 template<
typename LeafContainerT,
typename BranchContainerT>
76 unsigned int tree_depth;
78 assert(max_voxel_index_arg>0);
81 tree_depth = std::min (
static_cast<unsigned int> (OctreeKey::maxDepth),
static_cast<unsigned int> (std::ceil (std::log2 (max_voxel_index_arg))));
84 depth_mask_ = (1 << (tree_depth - 1));
88 template<
typename LeafContainerT,
typename BranchContainerT>
95 octree_depth_ = depth_arg;
98 depth_mask_ = (1 << (depth_arg - 1));
101 max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
105 template<
typename LeafContainerT,
typename BranchContainerT>
108 unsigned int idx_y_arg,
109 unsigned int idx_z_arg)
112 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
115 return (findLeaf (key));
119 template<
typename LeafContainerT,
typename BranchContainerT>
122 unsigned int idx_y_arg,
123 unsigned int idx_z_arg)
126 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
129 return (createLeaf (key));
133 template<
typename LeafContainerT,
typename BranchContainerT>
136 unsigned int idx_y_arg,
137 unsigned int idx_z_arg)
const
140 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
143 return (existLeaf (key));
147 template<
typename LeafContainerT,
typename BranchContainerT>
150 unsigned int idx_y_arg,
151 unsigned int idx_z_arg)
154 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
157 deleteLeafRecursive (key, depth_mask_, root_node_);
161 template<
typename LeafContainerT,
typename BranchContainerT>
169 deleteBranch (*root_node_);
177 template<
typename LeafContainerT,
typename BranchContainerT>
185 binary_tree_out_arg.clear ();
186 binary_tree_out_arg.reserve (this->branch_count_);
188 serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg,
nullptr);
192 template<
typename LeafContainerT,
typename BranchContainerT>
195 std::vector<LeafContainerT*>& leaf_container_vector_arg)
201 binary_tree_out_arg.clear ();
202 leaf_container_vector_arg.clear ();
204 binary_tree_out_arg.reserve (this->branch_count_);
205 leaf_container_vector_arg.reserve (this->leaf_count_);
207 serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg);
211 template<
typename LeafContainerT,
typename BranchContainerT>
218 leaf_container_vector_arg.clear ();
220 leaf_container_vector_arg.reserve (this->leaf_count_);
222 serializeTreeRecursive (root_node_, new_key,
nullptr, &leaf_container_vector_arg);
226 template<
typename LeafContainerT,
typename BranchContainerT>
236 std::vector<char>::const_iterator binary_tree_out_it = binary_tree_out_arg.begin ();
237 std::vector<char>::const_iterator binary_tree_out_it_end = binary_tree_out_arg.end ();
239 deserializeTreeRecursive (root_node_,
243 binary_tree_out_it_end,
250 template<
typename LeafContainerT,
typename BranchContainerT>
253 std::vector<LeafContainerT*>& leaf_container_vector_arg)
258 typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it = leaf_container_vector_arg.begin ();
261 typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it_end = leaf_container_vector_arg.end ();
267 std::vector<char>::const_iterator binary_tree_input_it = binary_tree_in_arg.begin ();
268 std::vector<char>::const_iterator binary_tree_input_it_end = binary_tree_in_arg.end ();
270 deserializeTreeRecursive (root_node_,
273 binary_tree_input_it,
274 binary_tree_input_it_end,
276 &leaf_vector_it_end);
281 template<
typename LeafContainerT,
typename BranchContainerT>
284 unsigned int depth_mask_arg,
290 unsigned char child_idx;
295 OctreeNode* child_node = (*branch_arg)[child_idx];
299 if ((!dynamic_depth_enabled_) && (depth_mask_arg > 1))
302 BranchNode* childBranch = createBranchChild (*branch_arg, child_idx);
307 return createLeafRecursive (key_arg, depth_mask_arg / 2, childBranch, return_leaf_arg, parent_of_leaf_arg);
311 LeafNode* leaf_node = createLeafChild (*branch_arg, child_idx);
312 return_leaf_arg = leaf_node;
313 parent_of_leaf_arg = branch_arg;
323 return createLeafRecursive (key_arg, depth_mask_arg / 2,
static_cast<BranchNode*
> (child_node),
324 return_leaf_arg, parent_of_leaf_arg);
328 return_leaf_arg =
static_cast<LeafNode*
> (child_node);;
329 parent_of_leaf_arg = branch_arg;
335 return (depth_mask_arg >> 1);
339 template<
typename LeafContainerT,
typename BranchContainerT>
342 unsigned int depth_mask_arg,
344 LeafContainerT*& result_arg)
const
347 unsigned char child_idx;
352 OctreeNode* child_node = (*branch_arg)[child_idx];
361 child_branch =
static_cast<BranchNode*
> (child_node);
363 findLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, result_arg);
369 child_leaf =
static_cast<LeafNode*
> (child_node);
378 template<
typename LeafContainerT,
typename BranchContainerT>
381 unsigned int depth_mask_arg,
385 unsigned char child_idx;
392 OctreeNode* child_node = (*branch_arg)[child_idx];
401 child_branch =
static_cast<BranchNode*
> (child_node);
404 b_no_children = deleteLeafRecursive (key_arg, depth_mask_arg / 2, child_branch);
409 deleteBranchChild (*branch_arg, child_idx);
418 deleteBranchChild (*branch_arg, child_idx);
425 b_no_children =
false;
426 for (child_idx = 0; (!b_no_children) && (child_idx < 8); child_idx++)
428 b_no_children = branch_arg->
hasChild (child_idx);
431 return (b_no_children);
435 template<
typename LeafContainerT,
typename BranchContainerT>
439 std::vector<char>* binary_tree_out_arg,
440 typename std::vector<LeafContainerT*>* leaf_container_vector_arg)
const
442 char node_bit_pattern;
445 node_bit_pattern = getBranchBitPattern (*branch_arg);
448 if (binary_tree_out_arg)
449 binary_tree_out_arg->push_back (node_bit_pattern);
452 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++)
456 if (branch_arg->
hasChild (child_idx))
468 serializeTreeRecursive (
static_cast<const BranchNode*
> (childNode), key_arg, binary_tree_out_arg,
469 leaf_container_vector_arg);
476 if (leaf_container_vector_arg)
480 serializeTreeCallback (**child_leaf, key_arg);
494 template<
typename LeafContainerT,
typename BranchContainerT>
497 typename std::vector<char>::const_iterator& binary_tree_input_it_arg,
498 typename std::vector<char>::const_iterator& binary_tree_input_it_end_arg,
499 typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_arg,
500 typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_end_arg)
504 if (binary_tree_input_it_arg != binary_tree_input_it_end_arg)
507 node_bits = (*binary_tree_input_it_arg);
508 binary_tree_input_it_arg++;
511 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++)
514 if (node_bits & (1 << child_idx))
519 if (depth_mask_arg > 1)
522 BranchNode * newBranch = createBranchChild (*branch_arg, child_idx);
527 deserializeTreeRecursive (newBranch, depth_mask_arg / 2, key_arg,
528 binary_tree_input_it_arg, binary_tree_input_it_end_arg,
529 leaf_container_vector_it_arg, leaf_container_vector_it_end_arg);
535 LeafNode* child_leaf = createLeafChild (*branch_arg, child_idx);
537 if (leaf_container_vector_it_arg && (*leaf_container_vector_it_arg != *leaf_container_vector_it_end_arg))
539 LeafContainerT& container = **child_leaf;
540 container = ***leaf_container_vector_it_arg;
541 ++*leaf_container_vector_it_arg;
547 deserializeTreeCallback (**child_leaf, key_arg);
560 #define PCL_INSTANTIATE_OctreeBase(T) template class PCL_EXPORTS pcl::octree::OctreeBase<T>;