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 if (max_voxel_index_arg <= 0) {
75 PCL_ERROR(
"[pcl::octree::Octree2BufBase::setMaxVoxelIndex] Max voxel index (%lu) "
84 std::ceil(std::log2(max_voxel_index_arg))),
88 depth_mask_ = (1 << (treeDepth - 1));
92template <
typename LeafContainerT,
typename BranchContainerT>
98 "[pcl::octree::Octree2BufBase::setTreeDepth] Tree depth (%lu) must be > 0!\n",
104 octree_depth_ = depth_arg;
107 depth_mask_ = (1 << (depth_arg - 1));
110 max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
114template <
typename LeafContainerT,
typename BranchContainerT>
121 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
124 return (findLeaf(key));
128template <
typename LeafContainerT,
typename BranchContainerT>
135 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
138 return (createLeaf(key));
142template <
typename LeafContainerT,
typename BranchContainerT>
149 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
152 return existLeaf(key);
156template <
typename LeafContainerT,
typename BranchContainerT>
163 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
166 return (this->removeLeaf(key));
170template <
typename LeafContainerT,
typename BranchContainerT>
176 deleteBranch(*root_node_);
180 tree_dirty_flag_ =
false;
187template <
typename LeafContainerT,
typename BranchContainerT>
191 if (tree_dirty_flag_) {
193 treeCleanUpRecursive(root_node_);
197 buffer_selector_ = !buffer_selector_;
200 tree_dirty_flag_ =
true;
205 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
206 root_node_->setChildPtr(buffer_selector_, child_idx,
nullptr);
211template <
typename LeafContainerT,
typename BranchContainerT>
214 std::vector<char>& binary_tree_out_arg,
bool do_XOR_encoding_arg)
219 binary_tree_out_arg.clear();
220 binary_tree_out_arg.reserve(this->branch_count_);
222 serializeTreeRecursive(
223 root_node_, new_key, &binary_tree_out_arg,
nullptr, do_XOR_encoding_arg,
false);
226 tree_dirty_flag_ =
false;
230template <
typename LeafContainerT,
typename BranchContainerT>
233 std::vector<char>& binary_tree_out_arg,
234 std::vector<LeafContainerT*>& leaf_container_vector_arg,
235 bool do_XOR_encoding_arg)
240 binary_tree_out_arg.clear();
241 leaf_container_vector_arg.clear();
243 leaf_container_vector_arg.reserve(leaf_count_);
244 binary_tree_out_arg.reserve(this->branch_count_);
246 serializeTreeRecursive(root_node_,
248 &binary_tree_out_arg,
249 &leaf_container_vector_arg,
254 tree_dirty_flag_ =
false;
258template <
typename LeafContainerT,
typename BranchContainerT>
261 std::vector<LeafContainerT*>& leaf_container_vector_arg)
266 leaf_container_vector_arg.clear();
268 leaf_container_vector_arg.reserve(leaf_count_);
270 serializeTreeRecursive(
271 root_node_, new_key,
nullptr, &leaf_container_vector_arg,
false,
false);
274 tree_dirty_flag_ =
false;
278template <
typename LeafContainerT,
typename BranchContainerT>
281 std::vector<char>& binary_tree_in_arg,
bool do_XOR_decoding_arg)
289 std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin();
290 std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end();
292 deserializeTreeRecursive(root_node_,
296 binary_tree_in_it_end,
300 do_XOR_decoding_arg);
303 tree_dirty_flag_ =
false;
307template <
typename LeafContainerT,
typename BranchContainerT>
310 std::vector<char>& binary_tree_in_arg,
311 std::vector<LeafContainerT*>& leaf_container_vector_arg,
312 bool do_XOR_decoding_arg)
317 typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it =
318 leaf_container_vector_arg.begin();
321 typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it_end =
322 leaf_container_vector_arg.end();
328 std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin();
329 std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end();
331 deserializeTreeRecursive(root_node_,
335 binary_tree_in_it_end,
336 &leaf_container_vector_it,
337 &leaf_container_vector_it_end,
339 do_XOR_decoding_arg);
342 tree_dirty_flag_ =
false;
346template <
typename LeafContainerT,
typename BranchContainerT>
349 std::vector<LeafContainerT*>& leaf_container_vector_arg)
354 leaf_container_vector_arg.clear();
355 leaf_container_vector_arg.reserve(leaf_count_);
357 serializeTreeRecursive(
358 root_node_, new_key,
nullptr, &leaf_container_vector_arg,
false,
true);
361 tree_dirty_flag_ =
false;
365template <
typename LeafContainerT,
typename BranchContainerT>
373 bool branch_reset_arg)
376 if (branch_reset_arg) {
378 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
379 branch_arg->setChildPtr(buffer_selector_, child_idx,
nullptr);
386 if (depth_mask_arg > 1) {
394 if (!branch_arg->hasChild(buffer_selector_, child_idx)) {
397 if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
398 OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_, child_idx);
401 child_branch =
static_cast<BranchNode*
>(child_node);
402 branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
406 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
407 child_branch = createBranchChild(*branch_arg, child_idx);
415 child_branch = createBranchChild(*branch_arg, child_idx);
422 child_branch =
static_cast<BranchNode*
>(
423 branch_arg->getChildPtr(buffer_selector_, child_idx));
426 return createLeafRecursive(key_arg,
435 LeafNode* child_leaf;
436 if (!branch_arg->hasChild(buffer_selector_, child_idx)) {
440 if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
442 OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_, child_idx);
444 child_leaf =
static_cast<LeafNode*
>(child_node);
446 branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
450 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
451 child_leaf = createLeafChild(*branch_arg, child_idx);
457 child_leaf = createLeafChild(*branch_arg, child_idx);
462 return_leaf_arg = child_leaf;
463 parent_of_leaf_arg = branch_arg;
468 static_cast<LeafNode*
>(branch_arg->getChildPtr(buffer_selector_, child_idx));
469 parent_of_leaf_arg = branch_arg;
472 return depth_mask_arg;
476template <
typename LeafContainerT,
typename BranchContainerT>
482 LeafContainerT*& result_arg)
const
485 unsigned char child_idx;
490 if (depth_mask_arg > 1) {
498 findLeafRecursive(key_arg, depth_mask_arg >> 1, child_branch, result_arg);
502 if (branch_arg->
hasChild(buffer_selector_, child_idx)) {
506 result_arg = leaf_node->getContainerPtr();
512template <
typename LeafContainerT,
typename BranchContainerT>
518 unsigned char child_idx;
525 if (depth_mask_arg > 1) {
535 bool bBranchOccupied =
536 deleteLeafRecursive(key_arg, depth_mask_arg >> 1, child_branch);
538 if (!bBranchOccupied) {
540 deleteBranchChild(*branch_arg, buffer_selector_, child_idx);
547 deleteBranchChild(*branch_arg, buffer_selector_, child_idx);
553 for (child_idx = 0; child_idx < 8; child_idx++) {
554 bNoChilds = branch_arg->
hasChild(buffer_selector_, child_idx);
564template <
typename LeafContainerT,
typename BranchContainerT>
569 std::vector<char>* binary_tree_out_arg,
570 typename std::vector<LeafContainerT*>* leaf_container_vector_arg,
571 bool do_XOR_encoding_arg,
572 bool new_leafs_filter_arg)
574 if (binary_tree_out_arg) {
576 const char branch_bit_pattern_curr_buffer =
577 getBranchBitPattern(*branch_arg, buffer_selector_);
578 if (do_XOR_encoding_arg) {
580 const char branch_bit_pattern_prev_buffer =
581 getBranchBitPattern(*branch_arg, !buffer_selector_);
583 const char node_XOR_bit_pattern =
584 branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer;
586 binary_tree_out_arg->push_back(node_XOR_bit_pattern);
590 binary_tree_out_arg->push_back(branch_bit_pattern_curr_buffer);
595 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
596 if (branch_arg->
hasChild(buffer_selector_, child_idx)) {
605 serializeTreeRecursive(
static_cast<BranchNode*
>(child_node),
608 leaf_container_vector_arg,
610 new_leafs_filter_arg);
614 auto* child_leaf =
static_cast<LeafNode*
>(child_node);
616 if (new_leafs_filter_arg) {
617 if (!branch_arg->
hasChild(!buffer_selector_, child_idx)) {
618 if (leaf_container_vector_arg)
619 leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());
621 serializeTreeCallback(**child_leaf, key_arg);
626 if (leaf_container_vector_arg)
627 leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());
629 serializeTreeCallback(**child_leaf, key_arg);
641 else if (branch_arg->
hasChild(!buffer_selector_, child_idx)) {
643 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
649template <
typename LeafContainerT,
typename BranchContainerT>
655 typename std::vector<char>::const_iterator& binaryTreeIT_arg,
656 typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
657 typename std::vector<LeafContainerT*>::const_iterator* dataVectorIterator_arg,
658 typename std::vector<LeafContainerT*>::const_iterator* dataVectorEndIterator_arg,
659 bool branch_reset_arg,
660 bool do_XOR_decoding_arg)
664 if (branch_reset_arg) {
666 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
667 branch_arg->
setChildPtr(buffer_selector_, child_idx,
nullptr);
671 if (binaryTreeIT_arg != binaryTreeIT_End_arg) {
673 char nodeBits = *binaryTreeIT_arg++;
676 char recoveredNodeBits;
677 if (do_XOR_decoding_arg) {
679 getBranchBitPattern(*branch_arg, !buffer_selector_) ^ nodeBits;
682 recoveredNodeBits = nodeBits;
686 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
688 if (recoveredNodeBits & (1 << child_idx)) {
692 if (depth_mask_arg > 1) {
695 bool doNodeReset =
false;
700 if (!branch_arg->
hasChild(buffer_selector_, child_idx)) {
702 if (branch_arg->
hasChild(!buffer_selector_, child_idx)) {
704 branch_arg->
getChildPtr(!buffer_selector_, child_idx);
707 child_branch =
static_cast<BranchNode*
>(child_node);
708 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
712 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
713 child_branch = createBranchChild(*branch_arg, child_idx);
721 child_branch = createBranchChild(*branch_arg, child_idx);
729 branch_arg->
getChildPtr(buffer_selector_, child_idx));
733 deserializeTreeRecursive(child_branch,
737 binaryTreeIT_End_arg,
738 dataVectorIterator_arg,
739 dataVectorEndIterator_arg,
741 do_XOR_decoding_arg);
748 if (branch_arg->
hasChild(!buffer_selector_, child_idx)) {
751 branch_arg->
getChildPtr(!buffer_selector_, child_idx);
753 child_leaf =
static_cast<LeafNode*
>(child_node);
754 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
758 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
759 child_leaf = createLeafChild(*branch_arg, child_idx);
764 child_leaf = createLeafChild(*branch_arg, child_idx);
769 if (dataVectorIterator_arg &&
770 (*dataVectorIterator_arg != *dataVectorEndIterator_arg)) {
771 LeafContainerT& container = **child_leaf;
772 container = ***dataVectorIterator_arg;
773 ++*dataVectorIterator_arg;
779 deserializeTreeCallback(**child_leaf, key_arg);
785 else if (branch_arg->
hasChild(!buffer_selector_, child_idx)) {
787 branch_arg->
setChildPtr(buffer_selector_, child_idx,
nullptr);
790 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
797template <
typename LeafContainerT,
typename BranchContainerT>
803 char occupied_children_bit_pattern_prev_buffer =
804 getBranchBitPattern(*branch_arg, !buffer_selector_);
807 char node_XOR_bit_pattern = getBranchXORBitPattern(*branch_arg);
810 char unused_branches_bit_pattern =
811 node_XOR_bit_pattern & occupied_children_bit_pattern_prev_buffer;
814 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
815 if (branch_arg->
hasChild(buffer_selector_, child_idx)) {
821 treeCleanUpRecursive(
static_cast<BranchNode*
>(child_node));
833 if (unused_branches_bit_pattern & (1 << child_idx)) {
835 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
842#define PCL_INSTANTIATE_Octree2BufBase(T) \
843 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
Abstract octree node class
virtual node_type_t getNodeType() const =0
Pure virtual method for retrieving 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.