39 #ifndef PCL_OCTREE_2BUF_BASE_HPP
40 #define PCL_OCTREE_2BUF_BASE_HPP
45 template <
typename LeafContainerT,
typename BranchContainerT>
52 , tree_dirty_flag_(false)
54 , dynamic_depth_enabled_(false)
58 template <
typename LeafContainerT,
typename BranchContainerT>
67 template <
typename LeafContainerT,
typename BranchContainerT>
70 unsigned int max_voxel_index_arg)
72 unsigned int treeDepth;
74 assert(max_voxel_index_arg > 0);
78 (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>
91 assert(depth_arg > 0);
94 octree_depth_ = depth_arg;
97 depth_mask_ = (1 << (depth_arg - 1));
100 max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
104 template <
typename LeafContainerT,
typename BranchContainerT>
107 unsigned int idx_y_arg,
108 unsigned int idx_z_arg)
111 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
114 return (findLeaf(key));
118 template <
typename LeafContainerT,
typename BranchContainerT>
121 unsigned int idx_y_arg,
122 unsigned int idx_z_arg)
125 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
128 return (createLeaf(key));
132 template <
typename LeafContainerT,
typename BranchContainerT>
135 unsigned int idx_x_arg,
unsigned int idx_y_arg,
unsigned int idx_z_arg)
const
138 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
141 return existLeaf(key);
145 template <
typename LeafContainerT,
typename BranchContainerT>
148 unsigned int idx_y_arg,
149 unsigned int idx_z_arg)
152 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
155 return (this->removeLeaf(key));
159 template <
typename LeafContainerT,
typename BranchContainerT>
165 deleteBranch(*root_node_);
169 tree_dirty_flag_ =
false;
176 template <
typename LeafContainerT,
typename BranchContainerT>
180 if (tree_dirty_flag_) {
182 treeCleanUpRecursive(root_node_);
186 buffer_selector_ = !buffer_selector_;
189 tree_dirty_flag_ =
true;
194 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
195 root_node_->setChildPtr(buffer_selector_, child_idx,
nullptr);
200 template <
typename LeafContainerT,
typename BranchContainerT>
203 std::vector<char>& binary_tree_out_arg,
bool do_XOR_encoding_arg)
208 binary_tree_out_arg.clear();
209 binary_tree_out_arg.reserve(this->branch_count_);
211 serializeTreeRecursive(
212 root_node_, new_key, &binary_tree_out_arg,
nullptr, do_XOR_encoding_arg,
false);
215 tree_dirty_flag_ =
false;
219 template <
typename LeafContainerT,
typename BranchContainerT>
222 std::vector<char>& binary_tree_out_arg,
223 std::vector<LeafContainerT*>& leaf_container_vector_arg,
224 bool do_XOR_encoding_arg)
229 binary_tree_out_arg.clear();
230 leaf_container_vector_arg.clear();
232 leaf_container_vector_arg.reserve(leaf_count_);
233 binary_tree_out_arg.reserve(this->branch_count_);
235 serializeTreeRecursive(root_node_,
237 &binary_tree_out_arg,
238 &leaf_container_vector_arg,
243 tree_dirty_flag_ =
false;
247 template <
typename LeafContainerT,
typename BranchContainerT>
250 std::vector<LeafContainerT*>& leaf_container_vector_arg)
255 leaf_container_vector_arg.clear();
257 leaf_container_vector_arg.reserve(leaf_count_);
259 serializeTreeRecursive(
260 root_node_, new_key,
nullptr, &leaf_container_vector_arg,
false,
false);
263 tree_dirty_flag_ =
false;
267 template <
typename LeafContainerT,
typename BranchContainerT>
270 std::vector<char>& binary_tree_in_arg,
bool do_XOR_decoding_arg)
278 std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin();
279 std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end();
281 deserializeTreeRecursive(root_node_,
285 binary_tree_in_it_end,
289 do_XOR_decoding_arg);
292 tree_dirty_flag_ =
false;
296 template <
typename LeafContainerT,
typename BranchContainerT>
299 std::vector<char>& binary_tree_in_arg,
300 std::vector<LeafContainerT*>& leaf_container_vector_arg,
301 bool do_XOR_decoding_arg)
306 typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it =
307 leaf_container_vector_arg.begin();
310 typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it_end =
311 leaf_container_vector_arg.end();
317 std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin();
318 std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end();
320 deserializeTreeRecursive(root_node_,
324 binary_tree_in_it_end,
325 &leaf_container_vector_it,
326 &leaf_container_vector_it_end,
328 do_XOR_decoding_arg);
331 tree_dirty_flag_ =
false;
335 template <
typename LeafContainerT,
typename BranchContainerT>
338 std::vector<LeafContainerT*>& leaf_container_vector_arg)
343 leaf_container_vector_arg.clear();
344 leaf_container_vector_arg.reserve(leaf_count_);
346 serializeTreeRecursive(
347 root_node_, new_key,
nullptr, &leaf_container_vector_arg,
false,
true);
350 tree_dirty_flag_ =
false;
354 template <
typename LeafContainerT,
typename BranchContainerT>
358 unsigned int depth_mask_arg,
362 bool branch_reset_arg)
365 if (branch_reset_arg) {
367 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
368 branch_arg->
setChildPtr(buffer_selector_, child_idx,
nullptr);
375 if (depth_mask_arg > 1) {
383 if (!branch_arg->
hasChild(buffer_selector_, child_idx)) {
386 if (branch_arg->
hasChild(!buffer_selector_, child_idx)) {
390 child_branch =
static_cast<BranchNode*
>(child_node);
391 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
395 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
396 child_branch = createBranchChild(*branch_arg, child_idx);
404 child_branch = createBranchChild(*branch_arg, child_idx);
412 branch_arg->
getChildPtr(buffer_selector_, child_idx));
415 return createLeafRecursive(key_arg,
425 if (!branch_arg->
hasChild(buffer_selector_, child_idx)) {
429 if (branch_arg->
hasChild(!buffer_selector_, child_idx)) {
433 child_leaf =
static_cast<LeafNode*
>(child_node);
434 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
438 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
439 child_leaf = createLeafChild(*branch_arg, child_idx);
445 child_leaf = createLeafChild(*branch_arg, child_idx);
450 return_leaf_arg = child_leaf;
451 parent_of_leaf_arg = branch_arg;
457 parent_of_leaf_arg = branch_arg;
460 return depth_mask_arg;
464 template <
typename LeafContainerT,
typename BranchContainerT>
468 unsigned int depth_mask_arg,
470 LeafContainerT*& result_arg)
const
473 unsigned char child_idx;
478 if (depth_mask_arg > 1) {
486 findLeafRecursive(key_arg, depth_mask_arg / 2, child_branch, result_arg);
490 if (branch_arg->
hasChild(buffer_selector_, child_idx)) {
500 template <
typename LeafContainerT,
typename BranchContainerT>
506 unsigned char child_idx;
513 if (depth_mask_arg > 1) {
523 bool bBranchOccupied =
524 deleteLeafRecursive(key_arg, depth_mask_arg / 2, child_branch);
526 if (!bBranchOccupied) {
528 deleteBranchChild(*branch_arg, buffer_selector_, child_idx);
535 deleteBranchChild(*branch_arg, buffer_selector_, child_idx);
541 for (child_idx = 0; child_idx < 8; child_idx++) {
542 bNoChilds = branch_arg->
hasChild(buffer_selector_, child_idx);
552 template <
typename LeafContainerT,
typename BranchContainerT>
557 std::vector<char>* binary_tree_out_arg,
558 typename std::vector<LeafContainerT*>* leaf_container_vector_arg,
559 bool do_XOR_encoding_arg,
560 bool new_leafs_filter_arg)
563 char branch_bit_pattern_curr_buffer;
564 char branch_bit_pattern_prev_buffer;
565 char node_XOR_bit_pattern;
568 branch_bit_pattern_curr_buffer = getBranchBitPattern(*branch_arg, buffer_selector_);
569 branch_bit_pattern_prev_buffer = getBranchBitPattern(*branch_arg, !buffer_selector_);
572 node_XOR_bit_pattern =
573 branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer;
575 if (binary_tree_out_arg) {
576 if (do_XOR_encoding_arg) {
578 binary_tree_out_arg->push_back(node_XOR_bit_pattern);
582 binary_tree_out_arg->push_back(branch_bit_pattern_curr_buffer);
587 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
588 if (branch_arg->
hasChild(buffer_selector_, child_idx)) {
597 serializeTreeRecursive(
static_cast<BranchNode*
>(child_node),
600 leaf_container_vector_arg,
602 new_leafs_filter_arg);
608 if (new_leafs_filter_arg) {
609 if (!branch_arg->
hasChild(!buffer_selector_, child_idx)) {
610 if (leaf_container_vector_arg)
613 serializeTreeCallback(**child_leaf, key_arg);
618 if (leaf_container_vector_arg)
621 serializeTreeCallback(**child_leaf, key_arg);
633 else if (branch_arg->
hasChild(!buffer_selector_, child_idx)) {
635 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
641 template <
typename LeafContainerT,
typename BranchContainerT>
645 unsigned int depth_mask_arg,
647 typename std::vector<char>::const_iterator& binaryTreeIT_arg,
648 typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
649 typename std::vector<LeafContainerT*>::const_iterator* dataVectorIterator_arg,
650 typename std::vector<LeafContainerT*>::const_iterator* dataVectorEndIterator_arg,
651 bool branch_reset_arg,
652 bool do_XOR_decoding_arg)
656 if (branch_reset_arg) {
658 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
659 branch_arg->
setChildPtr(buffer_selector_, child_idx,
nullptr);
663 if (binaryTreeIT_arg != binaryTreeIT_End_arg) {
665 char nodeBits = *binaryTreeIT_arg++;
668 char recoveredNodeBits;
669 if (do_XOR_decoding_arg) {
671 getBranchBitPattern(*branch_arg, !buffer_selector_) ^ nodeBits;
674 recoveredNodeBits = nodeBits;
678 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
680 if (recoveredNodeBits & (1 << child_idx)) {
684 if (depth_mask_arg > 1) {
687 bool doNodeReset =
false;
692 if (!branch_arg->
hasChild(buffer_selector_, child_idx)) {
694 if (branch_arg->
hasChild(!buffer_selector_, child_idx)) {
696 branch_arg->
getChildPtr(!buffer_selector_, child_idx);
699 child_branch =
static_cast<BranchNode*
>(child_node);
700 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
704 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
705 child_branch = createBranchChild(*branch_arg, child_idx);
713 child_branch = createBranchChild(*branch_arg, child_idx);
721 branch_arg->
getChildPtr(buffer_selector_, child_idx));
725 deserializeTreeRecursive(child_branch,
729 binaryTreeIT_End_arg,
730 dataVectorIterator_arg,
731 dataVectorEndIterator_arg,
733 do_XOR_decoding_arg);
740 if (branch_arg->
hasChild(!buffer_selector_, child_idx)) {
743 branch_arg->
getChildPtr(!buffer_selector_, child_idx);
745 child_leaf =
static_cast<LeafNode*
>(child_node);
746 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
750 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
751 child_leaf = createLeafChild(*branch_arg, child_idx);
756 child_leaf = createLeafChild(*branch_arg, child_idx);
761 if (dataVectorIterator_arg &&
762 (*dataVectorIterator_arg != *dataVectorEndIterator_arg)) {
763 LeafContainerT& container = **child_leaf;
764 container = ***dataVectorIterator_arg;
765 ++*dataVectorIterator_arg;
771 deserializeTreeCallback(**child_leaf, key_arg);
777 else if (branch_arg->
hasChild(!buffer_selector_, child_idx)) {
779 branch_arg->
setChildPtr(buffer_selector_, child_idx,
nullptr);
782 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
789 template <
typename LeafContainerT,
typename BranchContainerT>
795 char occupied_children_bit_pattern_prev_buffer =
796 getBranchBitPattern(*branch_arg, !buffer_selector_);
799 char node_XOR_bit_pattern = getBranchXORBitPattern(*branch_arg);
802 char unused_branches_bit_pattern =
803 node_XOR_bit_pattern & occupied_children_bit_pattern_prev_buffer;
806 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
807 if (branch_arg->
hasChild(buffer_selector_, child_idx)) {
813 treeCleanUpRecursive(
static_cast<BranchNode*
>(child_node));
825 if (unused_branches_bit_pattern & (1 << child_idx)) {
827 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
834 #define PCL_INSTANTIATE_Octree2BufBase(T) \
835 template class PCL_EXPORTS pcl::octree::Octree2BufBase<T>;