39 #ifndef PCL_OCTREE_2BUF_BASE_HPP
40 #define PCL_OCTREE_2BUF_BASE_HPP
47 template<
typename LeafContainerT,
typename BranchContainerT>
55 tree_dirty_flag_ (false),
57 dynamic_depth_enabled_(false)
62 template<
typename LeafContainerT,
typename BranchContainerT>
71 template<
typename LeafContainerT,
typename BranchContainerT>
void
74 unsigned int treeDepth;
76 assert (max_voxel_index_arg > 0);
80 static_cast<unsigned int> (std::ceil (Log2 (max_voxel_index_arg))))),
81 static_cast<unsigned int> (0));
84 depth_mask_ = (1 << (treeDepth - 1));
88 template<
typename LeafContainerT,
typename BranchContainerT>
void
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> LeafContainerT*
108 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
111 return ( findLeaf (key));
115 template<
typename LeafContainerT,
typename BranchContainerT> LeafContainerT*
119 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
122 return ( createLeaf (key));
126 template<
typename LeafContainerT,
typename BranchContainerT>
bool
130 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
133 return existLeaf (key);
137 template<
typename LeafContainerT,
typename BranchContainerT>
void
141 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
144 return (this->removeLeaf (key));
148 template<
typename LeafContainerT,
typename BranchContainerT>
void
154 deleteBranch (*root_node_);
158 tree_dirty_flag_ =
false;
165 template<
typename LeafContainerT,
typename BranchContainerT>
void
168 if (tree_dirty_flag_)
171 treeCleanUpRecursive (root_node_);
175 buffer_selector_ = !buffer_selector_;
178 tree_dirty_flag_ =
true;
182 unsigned char child_idx;
184 for (child_idx = 0; child_idx < 8; child_idx++)
186 root_node_->setChildPtr(buffer_selector_, child_idx, 0);
191 template<
typename LeafContainerT,
typename BranchContainerT>
void
193 bool do_XOR_encoding_arg)
198 binary_tree_out_arg.clear ();
199 binary_tree_out_arg.reserve (this->branch_count_);
201 serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, 0, do_XOR_encoding_arg,
false);
204 tree_dirty_flag_ =
false;
208 template<
typename LeafContainerT,
typename BranchContainerT>
void
210 std::vector<LeafContainerT*>& leaf_container_vector_arg,
211 bool do_XOR_encoding_arg)
216 binary_tree_out_arg.clear ();
217 leaf_container_vector_arg.clear ();
219 leaf_container_vector_arg.reserve (leaf_count_);
220 binary_tree_out_arg.reserve (this->branch_count_);
222 serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg, do_XOR_encoding_arg,
false);
225 tree_dirty_flag_ =
false;
229 template<
typename LeafContainerT,
typename BranchContainerT>
void
235 leaf_container_vector_arg.clear ();
237 leaf_container_vector_arg.reserve (leaf_count_);
239 serializeTreeRecursive (root_node_, new_key, 0, &leaf_container_vector_arg,
false,
false);
242 tree_dirty_flag_ =
false;
246 template<
typename LeafContainerT,
typename BranchContainerT>
void
248 bool do_XOR_decoding_arg)
256 std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin ();
257 std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end ();
259 deserializeTreeRecursive (root_node_, depth_mask_, new_key,
260 binary_tree_in_it, binary_tree_in_it_end, 0, 0,
false,
261 do_XOR_decoding_arg);
264 tree_dirty_flag_ =
false;
268 template<
typename LeafContainerT,
typename BranchContainerT>
void
270 std::vector<LeafContainerT*>& leaf_container_vector_arg,
271 bool do_XOR_decoding_arg)
276 typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it = leaf_container_vector_arg.begin ();
279 typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it_end = leaf_container_vector_arg.end ();
285 std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin ();
286 std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end ();
288 deserializeTreeRecursive (root_node_,
292 binary_tree_in_it_end,
293 &leaf_container_vector_it,
294 &leaf_container_vector_it_end,
296 do_XOR_decoding_arg);
300 tree_dirty_flag_ =
false;
305 template<
typename LeafContainerT,
typename BranchContainerT>
void
311 leaf_container_vector_arg.clear ();
312 leaf_container_vector_arg.reserve (leaf_count_);
314 serializeTreeRecursive (root_node_, new_key, 0, &leaf_container_vector_arg,
false,
true);
317 tree_dirty_flag_ =
false;
321 template<
typename LeafContainerT,
typename BranchContainerT>
324 unsigned int depth_mask_arg,
328 bool branch_reset_arg)
331 unsigned char child_idx;
334 if (branch_reset_arg)
337 for (child_idx = 0; child_idx < 8; child_idx++)
339 branch_arg->
setChildPtr(buffer_selector_, child_idx, 0);
346 if (depth_mask_arg > 1)
355 if (!branch_arg->
hasChild(buffer_selector_, child_idx))
359 if (branch_arg->
hasChild(!buffer_selector_, child_idx))
364 child_branch =
static_cast<BranchNode*
> (child_node);
365 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
368 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
369 child_branch = createBranchChild (*branch_arg, child_idx);
379 child_branch = createBranchChild (*branch_arg, child_idx);
389 return createLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, return_leaf_arg, parent_of_leaf_arg, doNodeReset);
395 if (!branch_arg->
hasChild(buffer_selector_, child_idx))
400 if (branch_arg->
hasChild(!buffer_selector_, child_idx))
406 child_leaf =
static_cast<LeafNode*
> (child_node);
407 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
410 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
411 child_leaf = createLeafChild (*branch_arg, child_idx);
418 child_leaf = createLeafChild (*branch_arg, child_idx);
423 return_leaf_arg = child_leaf;
424 parent_of_leaf_arg = branch_arg;
429 return_leaf_arg =
static_cast<LeafNode*
> (branch_arg->
getChildPtr(buffer_selector_,child_idx));;
430 parent_of_leaf_arg = branch_arg;
434 return depth_mask_arg;
438 template<
typename LeafContainerT,
typename BranchContainerT>
void
440 unsigned int depth_mask_arg,
442 LeafContainerT*& result_arg)
const
445 unsigned char child_idx;
450 if (depth_mask_arg > 1)
458 findLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, result_arg);
463 if (branch_arg->
hasChild(buffer_selector_, child_idx))
473 template<
typename LeafContainerT,
typename BranchContainerT>
bool
475 unsigned int depth_mask_arg,
479 unsigned char child_idx;
486 if (depth_mask_arg > 1)
490 bool bBranchOccupied;
498 bBranchOccupied = deleteLeafRecursive (key_arg, depth_mask_arg / 2, child_branch);
500 if (!bBranchOccupied)
503 deleteBranchChild (*branch_arg, buffer_selector_, child_idx);
511 deleteBranchChild (*branch_arg, buffer_selector_, child_idx);
517 for (child_idx = 0; child_idx < 8; child_idx++)
519 bNoChilds = branch_arg->
hasChild(buffer_selector_, child_idx);
529 template<
typename LeafContainerT,
typename BranchContainerT>
void Octree2BufBase<
530 LeafContainerT, BranchContainerT>::serializeTreeRecursive (
BranchNode* branch_arg,
532 std::vector<char>* binary_tree_out_arg,
533 typename std::vector<LeafContainerT*>* leaf_container_vector_arg,
534 bool do_XOR_encoding_arg,
535 bool new_leafs_filter_arg)
538 unsigned char child_idx;
541 char branch_bit_pattern_curr_buffer;
542 char branch_bit_pattern_prev_buffer;
543 char node_XOR_bit_pattern;
546 branch_bit_pattern_curr_buffer = getBranchBitPattern (*branch_arg, buffer_selector_);
547 branch_bit_pattern_prev_buffer = getBranchBitPattern (*branch_arg, !buffer_selector_);
550 node_XOR_bit_pattern = branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer;
552 if (binary_tree_out_arg)
554 if (do_XOR_encoding_arg)
557 binary_tree_out_arg->push_back (node_XOR_bit_pattern);
562 binary_tree_out_arg->push_back (branch_bit_pattern_curr_buffer);
567 for (child_idx = 0; child_idx < 8; child_idx++)
569 if (branch_arg->
hasChild(buffer_selector_, child_idx))
581 serializeTreeRecursive (static_cast<BranchNode*> (child_node), key_arg, binary_tree_out_arg,
582 leaf_container_vector_arg, do_XOR_encoding_arg, new_leafs_filter_arg);
589 if (new_leafs_filter_arg)
591 if (!branch_arg->
hasChild (!buffer_selector_, child_idx))
593 if (leaf_container_vector_arg)
596 serializeTreeCallback (**child_leaf, key_arg);
601 if (leaf_container_vector_arg)
604 serializeTreeCallback (**child_leaf, key_arg);
616 else if (branch_arg->
hasChild (!buffer_selector_, child_idx))
619 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
628 template<
typename LeafContainerT,
typename BranchContainerT>
void
630 unsigned int depth_mask_arg,
OctreeKey& key_arg,
631 typename std::vector<char>::const_iterator& binaryTreeIT_arg,
632 typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
633 typename std::vector<LeafContainerT*>::const_iterator* dataVectorIterator_arg,
634 typename std::vector<LeafContainerT*>::const_iterator* dataVectorEndIterator_arg,
635 bool branch_reset_arg,
bool do_XOR_decoding_arg)
638 unsigned char child_idx;
642 char recoveredNodeBits;
645 if (branch_reset_arg)
648 for (child_idx = 0; child_idx < 8; child_idx++)
650 branch_arg->
setChildPtr(buffer_selector_, child_idx, 0);
654 if (binaryTreeIT_arg!=binaryTreeIT_End_arg) {
656 nodeBits = *binaryTreeIT_arg++;
660 if (do_XOR_decoding_arg)
662 recoveredNodeBits = getBranchBitPattern (*branch_arg, !buffer_selector_) ^ nodeBits;
666 recoveredNodeBits = nodeBits;
670 for (child_idx = 0; child_idx < 8; child_idx++)
673 if (recoveredNodeBits & (1 << child_idx))
682 if (depth_mask_arg > 1)
689 if (!branch_arg->
hasChild(buffer_selector_, child_idx))
692 if (branch_arg->
hasChild(!buffer_selector_, child_idx))
697 child_branch =
static_cast<BranchNode*
> (child_node);
698 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
701 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
702 child_branch = createBranchChild (*branch_arg, child_idx);
711 child_branch = createBranchChild (*branch_arg, child_idx);
724 deserializeTreeRecursive (child_branch, depth_mask_arg / 2, key_arg,
725 binaryTreeIT_arg, binaryTreeIT_End_arg,
726 dataVectorIterator_arg, dataVectorEndIterator_arg,
727 doNodeReset, do_XOR_decoding_arg);
736 if (branch_arg->
hasChild(!buffer_selector_, child_idx))
741 child_leaf =
static_cast<LeafNode*
> (child_node);
742 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
745 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
746 child_leaf = createLeafChild (*branch_arg, child_idx);
752 child_leaf = createLeafChild (*branch_arg, child_idx);
757 if (dataVectorIterator_arg
758 && (*dataVectorIterator_arg != *dataVectorEndIterator_arg))
760 LeafContainerT& container = **child_leaf;
761 container = ***dataVectorIterator_arg;
762 ++*dataVectorIterator_arg;
768 deserializeTreeCallback (**child_leaf, key_arg);
775 else if (branch_arg->
hasChild (!buffer_selector_, child_idx))
778 branch_arg->
setChildPtr(buffer_selector_, child_idx, 0);
781 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
789 template<
typename LeafContainerT,
typename BranchContainerT>
void
793 unsigned char child_idx;
796 char occupied_children_bit_pattern_prev_buffer;
797 char node_XOR_bit_pattern;
798 char unused_branches_bit_pattern;
801 occupied_children_bit_pattern_prev_buffer = getBranchBitPattern (*branch_arg, !buffer_selector_);
804 node_XOR_bit_pattern = getBranchXORBitPattern (*branch_arg);
807 unused_branches_bit_pattern = node_XOR_bit_pattern & occupied_children_bit_pattern_prev_buffer;
810 for (child_idx = 0; child_idx < 8; child_idx++)
812 if (branch_arg->
hasChild(buffer_selector_, child_idx))
821 treeCleanUpRecursive (static_cast<BranchNode*> (child_node));
833 if (unused_branches_bit_pattern & (1 << child_idx))
836 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
843 #define PCL_INSTANTIATE_Octree2BufBase(T) template class PCL_EXPORTS pcl::octree::Octree2BufBase<T>;