39 #ifndef PCL_OCTREE_BASE_HPP
40 #define PCL_OCTREE_BASE_HPP
44 #include <pcl/impl/instantiate.hpp>
49 template <
typename LeafContainerT,
typename BranchContainerT>
56 , dynamic_depth_enabled_(false)
60 template <
typename LeafContainerT,
typename BranchContainerT>
69 template <
typename LeafContainerT,
typename BranchContainerT>
72 unsigned int max_voxel_index_arg)
74 unsigned int tree_depth;
76 assert(max_voxel_index_arg > 0);
80 std::min(static_cast<unsigned int>(OctreeKey::maxDepth),
81 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>
92 assert(depth_arg > 0);
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>
168 deleteBranch(*root_node_);
175 template <
typename LeafContainerT,
typename BranchContainerT>
178 std::vector<char>& binary_tree_out_arg)
184 binary_tree_out_arg.clear();
185 binary_tree_out_arg.reserve(this->branch_count_);
187 serializeTreeRecursive(root_node_, new_key, &binary_tree_out_arg,
nullptr);
191 template <
typename LeafContainerT,
typename BranchContainerT>
194 std::vector<char>& binary_tree_out_arg,
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(
208 root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg);
212 template <
typename LeafContainerT,
typename BranchContainerT>
215 std::vector<LeafContainerT*>& leaf_container_vector_arg)
220 leaf_container_vector_arg.clear();
222 leaf_container_vector_arg.reserve(this->leaf_count_);
224 serializeTreeRecursive(root_node_, new_key,
nullptr, &leaf_container_vector_arg);
228 template <
typename LeafContainerT,
typename BranchContainerT>
231 std::vector<char>& binary_tree_out_arg)
239 std::vector<char>::const_iterator binary_tree_out_it = binary_tree_out_arg.begin();
240 std::vector<char>::const_iterator binary_tree_out_it_end = binary_tree_out_arg.end();
242 deserializeTreeRecursive(root_node_,
246 binary_tree_out_it_end,
252 template <
typename LeafContainerT,
typename BranchContainerT>
255 std::vector<char>& binary_tree_in_arg,
256 std::vector<LeafContainerT*>& leaf_container_vector_arg)
261 typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it =
262 leaf_container_vector_arg.begin();
265 typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it_end =
266 leaf_container_vector_arg.end();
272 std::vector<char>::const_iterator binary_tree_input_it = binary_tree_in_arg.begin();
273 std::vector<char>::const_iterator binary_tree_input_it_end = binary_tree_in_arg.end();
275 deserializeTreeRecursive(root_node_,
278 binary_tree_input_it,
279 binary_tree_input_it_end,
281 &leaf_vector_it_end);
285 template <
typename LeafContainerT,
typename BranchContainerT>
289 unsigned int depth_mask_arg,
295 unsigned char child_idx;
300 OctreeNode* child_node = (*branch_arg)[child_idx];
303 if ((!dynamic_depth_enabled_) && (depth_mask_arg > 1)) {
305 BranchNode* childBranch = createBranchChild(*branch_arg, child_idx);
310 return createLeafRecursive(key_arg,
317 LeafNode* leaf_node = createLeafChild(*branch_arg, child_idx);
318 return_leaf_arg = leaf_node;
319 parent_of_leaf_arg = branch_arg;
327 return createLeafRecursive(key_arg,
329 static_cast<BranchNode*>(child_node),
335 return_leaf_arg = static_cast<LeafNode*>(child_node);
336 parent_of_leaf_arg = branch_arg;
341 return (depth_mask_arg >> 1);
345 template <
typename LeafContainerT,
typename BranchContainerT>
349 unsigned int depth_mask_arg,
351 LeafContainerT*& result_arg)
const
354 unsigned char child_idx;
359 OctreeNode* child_node = (*branch_arg)[child_idx];
366 child_branch = static_cast<BranchNode*>(child_node);
368 findLeafRecursive(key_arg, depth_mask_arg / 2, child_branch, result_arg);
374 child_leaf = static_cast<LeafNode*>(child_node);
383 template <
typename LeafContainerT,
typename BranchContainerT>
389 unsigned char child_idx;
396 OctreeNode* child_node = (*branch_arg)[child_idx];
403 child_branch = static_cast<BranchNode*>(child_node);
406 b_no_children = deleteLeafRecursive(key_arg, depth_mask_arg / 2, child_branch);
408 if (!b_no_children) {
410 deleteBranchChild(*branch_arg, child_idx);
419 deleteBranchChild(*branch_arg, child_idx);
426 b_no_children =
false;
427 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>
440 std::vector<char>* binary_tree_out_arg,
441 typename std::vector<LeafContainerT*>* leaf_container_vector_arg)
const
443 char node_bit_pattern;
446 node_bit_pattern = getBranchBitPattern(*branch_arg);
449 if (binary_tree_out_arg)
450 binary_tree_out_arg->push_back(node_bit_pattern);
453 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
456 if (branch_arg->
hasChild(child_idx)) {
465 serializeTreeRecursive(static_cast<const BranchNode*>(childNode),
468 leaf_container_vector_arg);
472 LeafNode* child_leaf = static_cast<LeafNode*>(childNode);
474 if (leaf_container_vector_arg)
478 serializeTreeCallback(**child_leaf, key_arg);
492 template <
typename LeafContainerT,
typename BranchContainerT>
496 unsigned int depth_mask_arg,
498 typename std::vector<char>::const_iterator& binary_tree_input_it_arg,
499 typename std::vector<char>::const_iterator& binary_tree_input_it_end_arg,
500 typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_arg,
501 typename std::vector<LeafContainerT*>::const_iterator*
502 leaf_container_vector_it_end_arg)
505 if (binary_tree_input_it_arg != binary_tree_input_it_end_arg) {
507 char 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++) {
513 if (node_bits & (1 << child_idx)) {
517 if (depth_mask_arg > 1) {
519 BranchNode* newBranch = createBranchChild(*branch_arg, child_idx);
524 deserializeTreeRecursive(newBranch,
527 binary_tree_input_it_arg,
528 binary_tree_input_it_end_arg,
529 leaf_container_vector_it_arg,
530 leaf_container_vector_it_end_arg);
535 LeafNode* child_leaf = createLeafChild(*branch_arg, child_idx);
537 if (leaf_container_vector_it_arg &&
538 (*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) \
561 template class PCL_EXPORTS pcl::octree::OctreeBase<T>;