39#ifndef PCL_OCTREE_2BUF_BASE_HPP
40#define PCL_OCTREE_2BUF_BASE_HPP
45template <
typename LeafContainerT,
typename BranchContainerT>
52, tree_dirty_flag_(false)
54, dynamic_depth_enabled_(false)
58template <
typename LeafContainerT,
typename BranchContainerT>
67template <
typename LeafContainerT,
typename BranchContainerT>
74 assert(max_voxel_index_arg > 0);
79 std::ceil(std::log2(max_voxel_index_arg))),
83 depth_mask_ = (1 << (treeDepth - 1));
87template <
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;
104template <
typename LeafContainerT,
typename BranchContainerT>
111 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
114 return (findLeaf(key));
118template <
typename LeafContainerT,
typename BranchContainerT>
125 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
128 return (createLeaf(key));
132template <
typename LeafContainerT,
typename BranchContainerT>
139 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
142 return existLeaf(key);
146template <
typename LeafContainerT,
typename BranchContainerT>
153 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
156 return (this->removeLeaf(key));
160template <
typename LeafContainerT,
typename BranchContainerT>
166 deleteBranch(*root_node_);
170 tree_dirty_flag_ =
false;
177template <
typename LeafContainerT,
typename BranchContainerT>
181 if (tree_dirty_flag_) {
183 treeCleanUpRecursive(root_node_);
187 buffer_selector_ = !buffer_selector_;
190 tree_dirty_flag_ =
true;
195 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
196 root_node_->setChildPtr(buffer_selector_, child_idx,
nullptr);
201template <
typename LeafContainerT,
typename BranchContainerT>
204 std::vector<char>& binary_tree_out_arg,
bool do_XOR_encoding_arg)
209 binary_tree_out_arg.clear();
210 binary_tree_out_arg.reserve(this->branch_count_);
212 serializeTreeRecursive(
213 root_node_, new_key, &binary_tree_out_arg,
nullptr, do_XOR_encoding_arg,
false);
216 tree_dirty_flag_ =
false;
220template <
typename LeafContainerT,
typename BranchContainerT>
223 std::vector<char>& binary_tree_out_arg,
224 std::vector<LeafContainerT*>& leaf_container_vector_arg,
225 bool do_XOR_encoding_arg)
230 binary_tree_out_arg.clear();
231 leaf_container_vector_arg.clear();
233 leaf_container_vector_arg.reserve(leaf_count_);
234 binary_tree_out_arg.reserve(this->branch_count_);
236 serializeTreeRecursive(root_node_,
238 &binary_tree_out_arg,
239 &leaf_container_vector_arg,
244 tree_dirty_flag_ =
false;
248template <
typename LeafContainerT,
typename BranchContainerT>
251 std::vector<LeafContainerT*>& leaf_container_vector_arg)
256 leaf_container_vector_arg.clear();
258 leaf_container_vector_arg.reserve(leaf_count_);
260 serializeTreeRecursive(
261 root_node_, new_key,
nullptr, &leaf_container_vector_arg,
false,
false);
264 tree_dirty_flag_ =
false;
268template <
typename LeafContainerT,
typename BranchContainerT>
271 std::vector<char>& binary_tree_in_arg,
bool do_XOR_decoding_arg)
279 std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin();
280 std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end();
282 deserializeTreeRecursive(root_node_,
286 binary_tree_in_it_end,
290 do_XOR_decoding_arg);
293 tree_dirty_flag_ =
false;
297template <
typename LeafContainerT,
typename BranchContainerT>
300 std::vector<char>& binary_tree_in_arg,
301 std::vector<LeafContainerT*>& leaf_container_vector_arg,
302 bool do_XOR_decoding_arg)
307 typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it =
308 leaf_container_vector_arg.begin();
311 typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it_end =
312 leaf_container_vector_arg.end();
318 std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin();
319 std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end();
321 deserializeTreeRecursive(root_node_,
325 binary_tree_in_it_end,
326 &leaf_container_vector_it,
327 &leaf_container_vector_it_end,
329 do_XOR_decoding_arg);
332 tree_dirty_flag_ =
false;
336template <
typename LeafContainerT,
typename BranchContainerT>
339 std::vector<LeafContainerT*>& leaf_container_vector_arg)
344 leaf_container_vector_arg.clear();
345 leaf_container_vector_arg.reserve(leaf_count_);
347 serializeTreeRecursive(
348 root_node_, new_key,
nullptr, &leaf_container_vector_arg,
false,
true);
351 tree_dirty_flag_ =
false;
355template <
typename LeafContainerT,
typename BranchContainerT>
363 bool branch_reset_arg)
366 if (branch_reset_arg) {
368 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
369 branch_arg->
setChildPtr(buffer_selector_, child_idx,
nullptr);
376 if (depth_mask_arg > 1) {
384 if (!branch_arg->
hasChild(buffer_selector_, child_idx)) {
387 if (branch_arg->
hasChild(!buffer_selector_, child_idx)) {
391 child_branch =
static_cast<BranchNode*
>(child_node);
396 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
397 child_branch = createBranchChild(*branch_arg, child_idx);
405 child_branch = createBranchChild(*branch_arg, child_idx);
412 child_branch =
static_cast<BranchNode*
>(
413 branch_arg->
getChildPtr(buffer_selector_, child_idx));
416 return createLeafRecursive(key_arg,
425 LeafNode* child_leaf;
426 if (!branch_arg->
hasChild(buffer_selector_, child_idx)) {
430 if (branch_arg->
hasChild(!buffer_selector_, child_idx)) {
434 child_leaf =
static_cast<LeafNode*
>(child_node);
436 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
440 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
441 child_leaf = createLeafChild(*branch_arg, child_idx);
447 child_leaf = createLeafChild(*branch_arg, child_idx);
452 return_leaf_arg = child_leaf;
453 parent_of_leaf_arg = branch_arg;
458 static_cast<LeafNode*
>(branch_arg->
getChildPtr(buffer_selector_, child_idx));
459 parent_of_leaf_arg = branch_arg;
462 return depth_mask_arg;
466template <
typename LeafContainerT,
typename BranchContainerT>
472 LeafContainerT*& result_arg)
const
475 unsigned char child_idx;
480 if (depth_mask_arg > 1) {
488 findLeafRecursive(key_arg, depth_mask_arg / 2, child_branch, result_arg);
492 if (branch_arg->
hasChild(buffer_selector_, child_idx)) {
494 LeafNode* leaf_node =
496 result_arg = leaf_node->getContainerPtr();
502template <
typename LeafContainerT,
typename BranchContainerT>
508 unsigned char child_idx;
515 if (depth_mask_arg > 1) {
521 static_cast<BranchNode*
>(branch_arg->getChildPtr(buffer_selector_, child_idx));
525 bool bBranchOccupied =
526 deleteLeafRecursive(key_arg, depth_mask_arg / 2, child_branch);
528 if (!bBranchOccupied) {
530 deleteBranchChild(*branch_arg, buffer_selector_, child_idx);
537 deleteBranchChild(*branch_arg, buffer_selector_, child_idx);
543 for (child_idx = 0; child_idx < 8; child_idx++) {
544 bNoChilds = branch_arg->hasChild(buffer_selector_, child_idx);
554template <
typename LeafContainerT,
typename BranchContainerT>
559 std::vector<char>* binary_tree_out_arg,
560 typename std::vector<LeafContainerT*>* leaf_container_vector_arg,
561 bool do_XOR_encoding_arg,
562 bool new_leafs_filter_arg)
564 if (binary_tree_out_arg) {
566 const char branch_bit_pattern_curr_buffer =
567 getBranchBitPattern(*branch_arg, buffer_selector_);
568 if (do_XOR_encoding_arg) {
570 const char branch_bit_pattern_prev_buffer =
571 getBranchBitPattern(*branch_arg, !buffer_selector_);
573 const char node_XOR_bit_pattern =
574 branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer;
576 binary_tree_out_arg->push_back(node_XOR_bit_pattern);
580 binary_tree_out_arg->push_back(branch_bit_pattern_curr_buffer);
585 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
586 if (branch_arg->
hasChild(buffer_selector_, child_idx)) {
595 serializeTreeRecursive(
static_cast<BranchNode*
>(child_node),
598 leaf_container_vector_arg,
600 new_leafs_filter_arg);
606 if (new_leafs_filter_arg) {
607 if (!branch_arg->
hasChild(!buffer_selector_, child_idx)) {
608 if (leaf_container_vector_arg)
611 serializeTreeCallback(**child_leaf, key_arg);
616 if (leaf_container_vector_arg)
619 serializeTreeCallback(**child_leaf, key_arg);
631 else if (branch_arg->
hasChild(!buffer_selector_, child_idx)) {
633 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
639template <
typename LeafContainerT,
typename BranchContainerT>
645 typename std::vector<char>::const_iterator& binaryTreeIT_arg,
646 typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
647 typename std::vector<LeafContainerT*>::const_iterator* dataVectorIterator_arg,
648 typename std::vector<LeafContainerT*>::const_iterator* dataVectorEndIterator_arg,
649 bool branch_reset_arg,
650 bool do_XOR_decoding_arg)
654 if (branch_reset_arg) {
656 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
657 branch_arg->
setChildPtr(buffer_selector_, child_idx,
nullptr);
661 if (binaryTreeIT_arg != binaryTreeIT_End_arg) {
663 char nodeBits = *binaryTreeIT_arg++;
666 char recoveredNodeBits;
667 if (do_XOR_decoding_arg) {
669 getBranchBitPattern(*branch_arg, !buffer_selector_) ^ nodeBits;
672 recoveredNodeBits = nodeBits;
676 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
678 if (recoveredNodeBits & (1 << child_idx)) {
682 if (depth_mask_arg > 1) {
685 bool doNodeReset =
false;
690 if (!branch_arg->
hasChild(buffer_selector_, child_idx)) {
692 if (branch_arg->
hasChild(!buffer_selector_, child_idx)) {
694 branch_arg->
getChildPtr(!buffer_selector_, child_idx);
697 child_branch =
static_cast<BranchNode*
>(child_node);
698 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
702 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
703 child_branch = createBranchChild(*branch_arg, child_idx);
711 child_branch = createBranchChild(*branch_arg, child_idx);
719 branch_arg->
getChildPtr(buffer_selector_, child_idx));
723 deserializeTreeRecursive(child_branch,
727 binaryTreeIT_End_arg,
728 dataVectorIterator_arg,
729 dataVectorEndIterator_arg,
731 do_XOR_decoding_arg);
738 if (branch_arg->
hasChild(!buffer_selector_, child_idx)) {
741 branch_arg->
getChildPtr(!buffer_selector_, child_idx);
743 child_leaf =
static_cast<LeafNode*
>(child_node);
744 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
748 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
749 child_leaf = createLeafChild(*branch_arg, child_idx);
754 child_leaf = createLeafChild(*branch_arg, child_idx);
759 if (dataVectorIterator_arg &&
760 (*dataVectorIterator_arg != *dataVectorEndIterator_arg)) {
761 LeafContainerT& container = **child_leaf;
762 container = ***dataVectorIterator_arg;
763 ++*dataVectorIterator_arg;
769 deserializeTreeCallback(**child_leaf, key_arg);
775 else if (branch_arg->
hasChild(!buffer_selector_, child_idx)) {
777 branch_arg->
setChildPtr(buffer_selector_, child_idx,
nullptr);
780 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
787template <
typename LeafContainerT,
typename BranchContainerT>
793 char occupied_children_bit_pattern_prev_buffer =
794 getBranchBitPattern(*branch_arg, !buffer_selector_);
797 char node_XOR_bit_pattern = getBranchXORBitPattern(*branch_arg);
800 char unused_branches_bit_pattern =
801 node_XOR_bit_pattern & occupied_children_bit_pattern_prev_buffer;
804 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
805 if (branch_arg->
hasChild(buffer_selector_, child_idx)) {
811 treeCleanUpRecursive(
static_cast<BranchNode*
>(child_node));
823 if (unused_branches_bit_pattern & (1 << child_idx)) {
825 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
832#define PCL_INSTANTIATE_Octree2BufBase(T) \
833 template class PCL_EXPORTS pcl::octree::Octree2BufBase<T>;
OctreeNode * getChildPtr(unsigned char buffer_arg, unsigned char index_arg) const
Get child pointer in current branch node.
void setChildPtr(unsigned char buffer_arg, unsigned char index_arg, OctreeNode *newNode_arg)
Set 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.
OctreeLeafNode< OctreeContainerPointIndices > LeafNode
uindex_t createLeafRecursive(const OctreeKey &key_arg, uindex_t 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 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.
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...
bool existLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg) const
Check for the existence of leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
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.
LeafContainerT * createLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg)
Create new leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
void setTreeDepth(uindex_t depth_arg)
Set the maximum depth of the octree.
void setMaxVoxelIndex(uindex_t max_voxel_index_arg)
Set the maximum amount of voxels per dimension.
void removeLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg)
Remove leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
bool deleteLeafRecursive(const OctreeKey &key_arg, uindex_t depth_mask_arg, BranchNode *branch_arg)
Recursively search and delete leaf node.
LeafContainerT * findLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg)
Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
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...
BufferedBranchNode< OctreeContainerEmpty > BranchNode
Octree2BufBase()
Empty constructor.
void findLeafRecursive(const OctreeKey &key_arg, uindex_t depth_mask_arg, BranchNode *branch_arg, LeafContainerT *&result_arg) const
Recursively search for a given leaf node and return a pointer.
virtual ~Octree2BufBase()
Empty deconstructor.
void deserializeTreeRecursive(BranchNode *branch_arg, uindex_t 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...
Octree container class that does store a vector of point indices.
void popBranch()
pop child node from octree key
static const unsigned char maxDepth
void pushBranch(unsigned char childIndex)
push a child node to the octree key
unsigned char getChildIdxWithDepthMask(uindex_t depthMask) const
get child node index using depthMask
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)
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.