39 #ifndef PCL_OCTREE_2BUF_BASE_HPP
40 #define PCL_OCTREE_2BUF_BASE_HPP
47 template<
typename LeafContainerT,
typename BranchContainerT>
54 tree_dirty_flag_ (false),
56 dynamic_depth_enabled_(false)
61 template<
typename LeafContainerT,
typename BranchContainerT>
70 template<
typename LeafContainerT,
typename BranchContainerT>
void
73 unsigned int treeDepth;
75 assert (max_voxel_index_arg > 0);
78 treeDepth = std::max ((std::min (
static_cast<unsigned int> (OctreeKey::maxDepth),
79 static_cast<unsigned int> (std::ceil (std::log2 (max_voxel_index_arg))))),
80 static_cast<unsigned int> (0));
83 depth_mask_ = (1 << (treeDepth - 1));
87 template<
typename LeafContainerT,
typename BranchContainerT>
void
90 assert (depth_arg > 0);
93 octree_depth_ = depth_arg;
96 depth_mask_ = (1 << (depth_arg - 1));
99 max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
103 template<
typename LeafContainerT,
typename BranchContainerT> LeafContainerT*
107 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
110 return ( findLeaf (key));
114 template<
typename LeafContainerT,
typename BranchContainerT> LeafContainerT*
118 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
121 return ( createLeaf (key));
125 template<
typename LeafContainerT,
typename BranchContainerT>
bool
129 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
132 return existLeaf (key);
136 template<
typename LeafContainerT,
typename BranchContainerT>
void
140 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
143 return (this->removeLeaf (key));
147 template<
typename LeafContainerT,
typename BranchContainerT>
void
153 deleteBranch (*root_node_);
157 tree_dirty_flag_ =
false;
164 template<
typename LeafContainerT,
typename BranchContainerT>
void
167 if (tree_dirty_flag_)
170 treeCleanUpRecursive (root_node_);
174 buffer_selector_ = !buffer_selector_;
177 tree_dirty_flag_ =
true;
182 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++)
184 root_node_->setChildPtr(buffer_selector_, child_idx,
nullptr);
189 template<
typename LeafContainerT,
typename BranchContainerT>
void
191 bool do_XOR_encoding_arg)
196 binary_tree_out_arg.clear ();
197 binary_tree_out_arg.reserve (this->branch_count_);
199 serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg,
nullptr, do_XOR_encoding_arg,
false);
202 tree_dirty_flag_ =
false;
206 template<
typename LeafContainerT,
typename BranchContainerT>
void
208 std::vector<LeafContainerT*>& leaf_container_vector_arg,
209 bool do_XOR_encoding_arg)
214 binary_tree_out_arg.clear ();
215 leaf_container_vector_arg.clear ();
217 leaf_container_vector_arg.reserve (leaf_count_);
218 binary_tree_out_arg.reserve (this->branch_count_);
220 serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg, do_XOR_encoding_arg,
false);
223 tree_dirty_flag_ =
false;
227 template<
typename LeafContainerT,
typename BranchContainerT>
void
233 leaf_container_vector_arg.clear ();
235 leaf_container_vector_arg.reserve (leaf_count_);
237 serializeTreeRecursive (root_node_, new_key,
nullptr, &leaf_container_vector_arg,
false,
false);
240 tree_dirty_flag_ =
false;
244 template<
typename LeafContainerT,
typename BranchContainerT>
void
246 bool do_XOR_decoding_arg)
254 std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin ();
255 std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end ();
257 deserializeTreeRecursive (root_node_, depth_mask_, new_key,
258 binary_tree_in_it, binary_tree_in_it_end,
nullptr,
nullptr,
false,
259 do_XOR_decoding_arg);
262 tree_dirty_flag_ =
false;
266 template<
typename LeafContainerT,
typename BranchContainerT>
void
268 std::vector<LeafContainerT*>& leaf_container_vector_arg,
269 bool do_XOR_decoding_arg)
274 typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it = leaf_container_vector_arg.begin ();
277 typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it_end = leaf_container_vector_arg.end ();
283 std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin ();
284 std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end ();
286 deserializeTreeRecursive (root_node_,
290 binary_tree_in_it_end,
291 &leaf_container_vector_it,
292 &leaf_container_vector_it_end,
294 do_XOR_decoding_arg);
298 tree_dirty_flag_ =
false;
303 template<
typename LeafContainerT,
typename BranchContainerT>
void
309 leaf_container_vector_arg.clear ();
310 leaf_container_vector_arg.reserve (leaf_count_);
312 serializeTreeRecursive (root_node_, new_key,
nullptr, &leaf_container_vector_arg,
false,
true);
315 tree_dirty_flag_ =
false;
319 template<
typename LeafContainerT,
typename BranchContainerT>
322 unsigned int depth_mask_arg,
326 bool branch_reset_arg)
329 if (branch_reset_arg)
332 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++)
334 branch_arg->
setChildPtr(buffer_selector_, child_idx,
nullptr);
341 if (depth_mask_arg > 1)
350 if (!branch_arg->
hasChild(buffer_selector_, child_idx))
354 if (branch_arg->
hasChild(!buffer_selector_, child_idx))
359 child_branch =
static_cast<BranchNode*
> (child_node);
360 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
363 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
364 child_branch = createBranchChild (*branch_arg, child_idx);
374 child_branch = createBranchChild (*branch_arg, child_idx);
384 return createLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, return_leaf_arg, parent_of_leaf_arg, doNodeReset);
389 if (!branch_arg->
hasChild(buffer_selector_, child_idx))
394 if (branch_arg->
hasChild(!buffer_selector_, child_idx))
400 child_leaf =
static_cast<LeafNode*
> (child_node);
401 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
404 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
405 child_leaf = createLeafChild (*branch_arg, child_idx);
412 child_leaf = createLeafChild (*branch_arg, child_idx);
417 return_leaf_arg = child_leaf;
418 parent_of_leaf_arg = branch_arg;
423 return_leaf_arg =
static_cast<LeafNode*
> (branch_arg->
getChildPtr(buffer_selector_,child_idx));;
424 parent_of_leaf_arg = branch_arg;
427 return depth_mask_arg;
431 template<
typename LeafContainerT,
typename BranchContainerT>
void
433 unsigned int depth_mask_arg,
435 LeafContainerT*& result_arg)
const
438 unsigned char child_idx;
443 if (depth_mask_arg > 1)
451 findLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, result_arg);
456 if (branch_arg->
hasChild(buffer_selector_, child_idx))
466 template<
typename LeafContainerT,
typename BranchContainerT>
bool
468 unsigned int depth_mask_arg,
472 unsigned char child_idx;
479 if (depth_mask_arg > 1)
483 bool bBranchOccupied;
491 bBranchOccupied = deleteLeafRecursive (key_arg, depth_mask_arg / 2, child_branch);
493 if (!bBranchOccupied)
496 deleteBranchChild (*branch_arg, buffer_selector_, child_idx);
504 deleteBranchChild (*branch_arg, buffer_selector_, child_idx);
510 for (child_idx = 0; child_idx < 8; child_idx++)
512 bNoChilds = branch_arg->
hasChild(buffer_selector_, child_idx);
522 template<
typename LeafContainerT,
typename BranchContainerT>
void Octree2BufBase<
523 LeafContainerT, BranchContainerT>::serializeTreeRecursive (
BranchNode* branch_arg,
525 std::vector<char>* binary_tree_out_arg,
526 typename std::vector<LeafContainerT*>* leaf_container_vector_arg,
527 bool do_XOR_encoding_arg,
528 bool new_leafs_filter_arg)
531 char branch_bit_pattern_curr_buffer;
532 char branch_bit_pattern_prev_buffer;
533 char node_XOR_bit_pattern;
536 branch_bit_pattern_curr_buffer = getBranchBitPattern (*branch_arg, buffer_selector_);
537 branch_bit_pattern_prev_buffer = getBranchBitPattern (*branch_arg, !buffer_selector_);
540 node_XOR_bit_pattern = branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer;
542 if (binary_tree_out_arg)
544 if (do_XOR_encoding_arg)
547 binary_tree_out_arg->push_back (node_XOR_bit_pattern);
552 binary_tree_out_arg->push_back (branch_bit_pattern_curr_buffer);
557 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++)
559 if (branch_arg->
hasChild(buffer_selector_, child_idx))
571 serializeTreeRecursive (
static_cast<BranchNode*
> (child_node), key_arg, binary_tree_out_arg,
572 leaf_container_vector_arg, do_XOR_encoding_arg, new_leafs_filter_arg);
579 if (new_leafs_filter_arg)
581 if (!branch_arg->
hasChild (!buffer_selector_, child_idx))
583 if (leaf_container_vector_arg)
586 serializeTreeCallback (**child_leaf, key_arg);
591 if (leaf_container_vector_arg)
594 serializeTreeCallback (**child_leaf, key_arg);
606 else if (branch_arg->
hasChild (!buffer_selector_, child_idx))
609 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
618 template<
typename LeafContainerT,
typename BranchContainerT>
void
620 unsigned int depth_mask_arg,
OctreeKey& key_arg,
621 typename std::vector<char>::const_iterator& binaryTreeIT_arg,
622 typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
623 typename std::vector<LeafContainerT*>::const_iterator* dataVectorIterator_arg,
624 typename std::vector<LeafContainerT*>::const_iterator* dataVectorEndIterator_arg,
625 bool branch_reset_arg,
bool do_XOR_decoding_arg)
629 char recoveredNodeBits;
632 if (branch_reset_arg)
635 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++)
637 branch_arg->
setChildPtr(buffer_selector_, child_idx,
nullptr);
641 if (binaryTreeIT_arg!=binaryTreeIT_End_arg) {
643 nodeBits = *binaryTreeIT_arg++;
647 if (do_XOR_decoding_arg)
649 recoveredNodeBits = getBranchBitPattern (*branch_arg, !buffer_selector_) ^ nodeBits;
653 recoveredNodeBits = nodeBits;
657 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++)
660 if (recoveredNodeBits & (1 << child_idx))
669 if (depth_mask_arg > 1)
676 if (!branch_arg->
hasChild(buffer_selector_, child_idx))
679 if (branch_arg->
hasChild(!buffer_selector_, child_idx))
684 child_branch =
static_cast<BranchNode*
> (child_node);
685 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
688 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
689 child_branch = createBranchChild (*branch_arg, child_idx);
698 child_branch = createBranchChild (*branch_arg, child_idx);
711 deserializeTreeRecursive (child_branch, depth_mask_arg / 2, key_arg,
712 binaryTreeIT_arg, binaryTreeIT_End_arg,
713 dataVectorIterator_arg, dataVectorEndIterator_arg,
714 doNodeReset, do_XOR_decoding_arg);
723 if (branch_arg->
hasChild(!buffer_selector_, child_idx))
728 child_leaf =
static_cast<LeafNode*
> (child_node);
729 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
732 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
733 child_leaf = createLeafChild (*branch_arg, child_idx);
739 child_leaf = createLeafChild (*branch_arg, child_idx);
744 if (dataVectorIterator_arg
745 && (*dataVectorIterator_arg != *dataVectorEndIterator_arg))
747 LeafContainerT& container = **child_leaf;
748 container = ***dataVectorIterator_arg;
749 ++*dataVectorIterator_arg;
755 deserializeTreeCallback (**child_leaf, key_arg);
762 else if (branch_arg->
hasChild (!buffer_selector_, child_idx))
765 branch_arg->
setChildPtr(buffer_selector_, child_idx,
nullptr);
768 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
776 template<
typename LeafContainerT,
typename BranchContainerT>
void
780 char occupied_children_bit_pattern_prev_buffer = getBranchBitPattern (*branch_arg, !buffer_selector_);
783 char node_XOR_bit_pattern = getBranchXORBitPattern (*branch_arg);
786 char unused_branches_bit_pattern = node_XOR_bit_pattern & occupied_children_bit_pattern_prev_buffer;
789 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++)
791 if (branch_arg->
hasChild(buffer_selector_, child_idx))
800 treeCleanUpRecursive (
static_cast<BranchNode*
> (child_node));
812 if (unused_branches_bit_pattern & (1 << child_idx))
815 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
822 #define PCL_INSTANTIATE_Octree2BufBase(T) template class PCL_EXPORTS pcl::octree::Octree2BufBase<T>;