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);
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>;
void setChildPtr(unsigned char buffer_arg, unsigned char index_arg, OctreeNode *newNode_arg)
Set child pointer in current branch node.
OctreeNode * getChildPtr(unsigned char buffer_arg, unsigned char index_arg) const
Get child pointer in current branch node.
bool hasChild(unsigned char buffer_arg, unsigned char index_arg) const
Check if branch is pointing to a particular child node.
void serializeLeafs(std::vector< LeafContainerT * > &leaf_container_vector_arg)
Outputs a vector of all DataT elements that are stored within the octree leaf nodes.
void switchBuffers()
Switch buffers and reset current octree structure.
LeafContainerT * createLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg)
Create new leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
LeafContainerT * findLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg)
Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
void serializeTree(std::vector< char > &binary_tree_out_arg, bool do_XOR_encoding_arg=false)
Serialize octree into a binary output vector describing its branch node structure.
void treeCleanUpRecursive(BranchNode *branch_arg)
Recursively explore the octree and remove unused branch and leaf nodes.
bool existLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const
Check for the existence of leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
void deleteTree()
Delete the octree structure and its leaf nodes.
void serializeTreeRecursive(BranchNode *branch_arg, OctreeKey &key_arg, std::vector< char > *binary_tree_out_arg, typename std::vector< LeafContainerT * > *leaf_container_vector_arg, bool do_XOR_encoding_arg=false, bool new_leafs_filter_arg=false)
Recursively explore the octree and output binary octree description together with a vector of leaf no...
void setMaxVoxelIndex(unsigned int max_voxel_index_arg)
Set the maximum amount of voxels per dimension.
void deserializeTree(std::vector< char > &binary_tree_in_arg, bool do_XOR_decoding_arg=false)
Deserialize a binary octree description vector and create a corresponding octree structure.
unsigned int createLeafRecursive(const OctreeKey &key_arg, unsigned int depth_mask_arg, BranchNode *branch_arg, LeafNode *&return_leaf_arg, BranchNode *&parent_of_leaf_arg, bool branch_reset_arg=false)
Create a leaf node at octree key.
void setTreeDepth(unsigned int depth_arg)
Set the maximum depth of the octree.
void deserializeTreeRecursive(BranchNode *branch_arg, unsigned int depth_mask_arg, OctreeKey &key_arg, typename std::vector< char >::const_iterator &binary_tree_in_it_arg, typename std::vector< char >::const_iterator &binary_tree_in_it_end_arg, typename std::vector< LeafContainerT * >::const_iterator *leaf_container_vector_it_arg, typename std::vector< LeafContainerT * >::const_iterator *leaf_container_vector_it_end_arg, bool branch_reset_arg=false, bool do_XOR_decoding_arg=false)
Rebuild an octree based on binary XOR octree description and DataT objects for leaf node initializati...
void serializeNewLeafs(std::vector< LeafContainerT * > &leaf_container_vector_arg)
Outputs a vector of all DataT elements from leaf nodes, that do not exist in the previous octree buff...
void removeLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg)
Remove leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
void findLeafRecursive(const OctreeKey &key_arg, unsigned int depth_mask_arg, BranchNode *branch_arg, LeafContainerT *&result_arg) const
Recursively search for a given leaf node and return a pointer.
Octree2BufBase()
Empty constructor.
bool deleteLeafRecursive(const OctreeKey &key_arg, unsigned int depth_mask_arg, BranchNode *branch_arg)
Recursively search and delete leaf node.
virtual ~Octree2BufBase()
Empty deconstructor.
void popBranch()
pop child node from octree key
unsigned char getChildIdxWithDepthMask(unsigned int depthMask) const
get child node index using depthMask
static const unsigned char maxDepth
void pushBranch(unsigned char childIndex)
push a child node to the octree key
Abstract octree leaf class
const ContainerT * getContainerPtr() const
Get const pointer to container.
Abstract octree node class
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)