39#ifndef PCL_OCTREE_2BUF_BASE_HPP
40#define PCL_OCTREE_2BUF_BASE_HPP
45template <
typename LeafContainerT,
typename BranchContainerT>
51template <
typename LeafContainerT,
typename BranchContainerT>
60template <
typename LeafContainerT,
typename BranchContainerT>
67 if (max_voxel_index_arg <= 0) {
68 PCL_ERROR(
"[pcl::octree::Octree2BufBase::setMaxVoxelIndex] Max voxel index (%lu) "
77 std::ceil(std::log2(max_voxel_index_arg))),
81 depth_mask_ = (1 << (treeDepth - 1));
85template <
typename LeafContainerT,
typename BranchContainerT>
91 "[pcl::octree::Octree2BufBase::setTreeDepth] Tree depth (%lu) must be > 0!\n",
97 octree_depth_ = depth_arg;
100 depth_mask_ = (1 << (depth_arg - 1));
103 max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
107template <
typename LeafContainerT,
typename BranchContainerT>
114 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
117 return (findLeaf(key));
121template <
typename LeafContainerT,
typename BranchContainerT>
128 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
131 return (createLeaf(key));
135template <
typename LeafContainerT,
typename BranchContainerT>
142 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
145 return existLeaf(key);
149template <
typename LeafContainerT,
typename BranchContainerT>
156 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
159 return (this->removeLeaf(key));
163template <
typename LeafContainerT,
typename BranchContainerT>
169 deleteBranch(*root_node_);
173 tree_dirty_flag_ =
false;
180template <
typename LeafContainerT,
typename BranchContainerT>
184 if (tree_dirty_flag_) {
186 treeCleanUpRecursive(root_node_);
190 buffer_selector_ = !buffer_selector_;
193 tree_dirty_flag_ =
true;
198 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
199 root_node_->setChildPtr(buffer_selector_, child_idx,
nullptr);
204template <
typename LeafContainerT,
typename BranchContainerT>
207 std::vector<char>& binary_tree_out_arg,
bool do_XOR_encoding_arg)
212 binary_tree_out_arg.clear();
213 binary_tree_out_arg.reserve(this->branch_count_);
215 serializeTreeRecursive(
216 root_node_, new_key, &binary_tree_out_arg,
nullptr, do_XOR_encoding_arg,
false);
219 tree_dirty_flag_ =
false;
223template <
typename LeafContainerT,
typename BranchContainerT>
226 std::vector<char>& binary_tree_out_arg,
227 std::vector<LeafContainerT*>& leaf_container_vector_arg,
228 bool do_XOR_encoding_arg)
233 binary_tree_out_arg.clear();
234 leaf_container_vector_arg.clear();
236 leaf_container_vector_arg.reserve(leaf_count_);
237 binary_tree_out_arg.reserve(this->branch_count_);
239 serializeTreeRecursive(root_node_,
241 &binary_tree_out_arg,
242 &leaf_container_vector_arg,
247 tree_dirty_flag_ =
false;
251template <
typename LeafContainerT,
typename BranchContainerT>
254 std::vector<LeafContainerT*>& leaf_container_vector_arg)
259 leaf_container_vector_arg.clear();
261 leaf_container_vector_arg.reserve(leaf_count_);
263 serializeTreeRecursive(
264 root_node_, new_key,
nullptr, &leaf_container_vector_arg,
false,
false);
267 tree_dirty_flag_ =
false;
271template <
typename LeafContainerT,
typename BranchContainerT>
274 std::vector<char>& binary_tree_in_arg,
bool do_XOR_decoding_arg)
282 std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin();
283 std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end();
285 deserializeTreeRecursive(root_node_,
289 binary_tree_in_it_end,
293 do_XOR_decoding_arg);
296 tree_dirty_flag_ =
false;
300template <
typename LeafContainerT,
typename BranchContainerT>
303 std::vector<char>& binary_tree_in_arg,
304 std::vector<LeafContainerT*>& leaf_container_vector_arg,
305 bool do_XOR_decoding_arg)
310 typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it =
311 leaf_container_vector_arg.begin();
314 typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it_end =
315 leaf_container_vector_arg.end();
321 std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin();
322 std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end();
324 deserializeTreeRecursive(root_node_,
328 binary_tree_in_it_end,
329 &leaf_container_vector_it,
330 &leaf_container_vector_it_end,
332 do_XOR_decoding_arg);
335 tree_dirty_flag_ =
false;
339template <
typename LeafContainerT,
typename BranchContainerT>
342 std::vector<LeafContainerT*>& leaf_container_vector_arg)
347 leaf_container_vector_arg.clear();
348 leaf_container_vector_arg.reserve(leaf_count_);
350 serializeTreeRecursive(
351 root_node_, new_key,
nullptr, &leaf_container_vector_arg,
false,
true);
354 tree_dirty_flag_ =
false;
358template <
typename LeafContainerT,
typename BranchContainerT>
366 bool branch_reset_arg)
369 if (branch_reset_arg) {
371 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
372 branch_arg->setChildPtr(buffer_selector_, child_idx,
nullptr);
379 if (depth_mask_arg > 1) {
381 BranchNode* child_branch;
387 if (!branch_arg->hasChild(buffer_selector_, child_idx)) {
390 if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
391 OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_, child_idx);
394 child_branch =
static_cast<BranchNode*
>(child_node);
395 branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
399 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
400 child_branch = createBranchChild(*branch_arg, child_idx);
408 child_branch = createBranchChild(*branch_arg, child_idx);
416 branch_arg->getChildPtr(buffer_selector_, child_idx));
419 return createLeafRecursive(key_arg,
428 LeafNode* child_leaf;
429 if (!branch_arg->hasChild(buffer_selector_, child_idx)) {
433 if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
435 OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_, child_idx);
439 branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
443 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
444 child_leaf = createLeafChild(*branch_arg, child_idx);
450 child_leaf = createLeafChild(*branch_arg, child_idx);
455 return_leaf_arg = child_leaf;
456 parent_of_leaf_arg = branch_arg;
461 static_cast<LeafNode*
>(branch_arg->getChildPtr(buffer_selector_, child_idx));
462 parent_of_leaf_arg = branch_arg;
465 return depth_mask_arg;
469template <
typename LeafContainerT,
typename BranchContainerT>
475 LeafContainerT*& result_arg)
const
478 unsigned char child_idx;
483 if (depth_mask_arg > 1) {
491 findLeafRecursive(key_arg, depth_mask_arg >> 1, child_branch, result_arg);
495 if (branch_arg->
hasChild(buffer_selector_, child_idx)) {
498 static_cast<LeafNode*
>(branch_arg->
getChildPtr(buffer_selector_, child_idx));
499 result_arg = leaf_node->getContainerPtr();
505template <
typename LeafContainerT,
typename BranchContainerT>
511 unsigned char child_idx;
518 if (depth_mask_arg > 1) {
528 bool bBranchOccupied =
529 deleteLeafRecursive(key_arg, depth_mask_arg >> 1, child_branch);
531 if (!bBranchOccupied) {
533 deleteBranchChild(*branch_arg, buffer_selector_, child_idx);
540 deleteBranchChild(*branch_arg, buffer_selector_, child_idx);
546 for (child_idx = 0; child_idx < 8; child_idx++) {
547 bNoChilds = branch_arg->
hasChild(buffer_selector_, child_idx);
557template <
typename LeafContainerT,
typename BranchContainerT>
562 std::vector<char>* binary_tree_out_arg,
563 typename std::vector<LeafContainerT*>* leaf_container_vector_arg,
564 bool do_XOR_encoding_arg,
565 bool new_leafs_filter_arg)
567 if (binary_tree_out_arg) {
569 const char branch_bit_pattern_curr_buffer =
570 getBranchBitPattern(*branch_arg, buffer_selector_);
571 if (do_XOR_encoding_arg) {
573 const char branch_bit_pattern_prev_buffer =
574 getBranchBitPattern(*branch_arg, !buffer_selector_);
576 const char node_XOR_bit_pattern =
577 branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer;
579 binary_tree_out_arg->push_back(node_XOR_bit_pattern);
583 binary_tree_out_arg->push_back(branch_bit_pattern_curr_buffer);
588 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
589 if (branch_arg->
hasChild(buffer_selector_, child_idx)) {
598 serializeTreeRecursive(
static_cast<BranchNode*
>(child_node),
601 leaf_container_vector_arg,
603 new_leafs_filter_arg);
607 auto* child_leaf =
static_cast<LeafNode*
>(child_node);
609 if (new_leafs_filter_arg) {
610 if (!branch_arg->
hasChild(!buffer_selector_, child_idx)) {
611 if (leaf_container_vector_arg)
612 leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());
614 serializeTreeCallback(**child_leaf, key_arg);
619 if (leaf_container_vector_arg)
620 leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());
622 serializeTreeCallback(**child_leaf, key_arg);
634 else if (branch_arg->
hasChild(!buffer_selector_, child_idx)) {
636 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
642template <
typename LeafContainerT,
typename BranchContainerT>
648 typename std::vector<char>::const_iterator& binaryTreeIT_arg,
649 typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
650 typename std::vector<LeafContainerT*>::const_iterator* dataVectorIterator_arg,
651 typename std::vector<LeafContainerT*>::const_iterator* dataVectorEndIterator_arg,
652 bool branch_reset_arg,
653 bool do_XOR_decoding_arg)
657 if (branch_reset_arg) {
659 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
660 branch_arg->
setChildPtr(buffer_selector_, child_idx,
nullptr);
664 if (binaryTreeIT_arg != binaryTreeIT_End_arg) {
666 char nodeBits = *binaryTreeIT_arg++;
669 char recoveredNodeBits;
670 if (do_XOR_decoding_arg) {
672 getBranchBitPattern(*branch_arg, !buffer_selector_) ^ nodeBits;
675 recoveredNodeBits = nodeBits;
679 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
681 if (recoveredNodeBits & (1 << child_idx)) {
685 if (depth_mask_arg > 1) {
688 bool doNodeReset =
false;
693 if (!branch_arg->
hasChild(buffer_selector_, child_idx)) {
695 if (branch_arg->
hasChild(!buffer_selector_, child_idx)) {
697 branch_arg->
getChildPtr(!buffer_selector_, child_idx);
700 child_branch =
static_cast<BranchNode*
>(child_node);
701 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
705 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
706 child_branch = createBranchChild(*branch_arg, child_idx);
714 child_branch = createBranchChild(*branch_arg, child_idx);
722 branch_arg->
getChildPtr(buffer_selector_, child_idx));
726 deserializeTreeRecursive(child_branch,
730 binaryTreeIT_End_arg,
731 dataVectorIterator_arg,
732 dataVectorEndIterator_arg,
734 do_XOR_decoding_arg);
741 if (branch_arg->
hasChild(!buffer_selector_, child_idx)) {
744 branch_arg->
getChildPtr(!buffer_selector_, child_idx);
746 child_leaf =
static_cast<LeafNode*
>(child_node);
747 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
751 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
752 child_leaf = createLeafChild(*branch_arg, child_idx);
757 child_leaf = createLeafChild(*branch_arg, child_idx);
762 if (dataVectorIterator_arg &&
763 (*dataVectorIterator_arg != *dataVectorEndIterator_arg)) {
764 LeafContainerT& container = **child_leaf;
765 container = ***dataVectorIterator_arg;
766 ++*dataVectorIterator_arg;
772 deserializeTreeCallback(**child_leaf, key_arg);
778 else if (branch_arg->
hasChild(!buffer_selector_, child_idx)) {
780 branch_arg->
setChildPtr(buffer_selector_, child_idx,
nullptr);
783 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
790template <
typename LeafContainerT,
typename BranchContainerT>
796 char occupied_children_bit_pattern_prev_buffer =
797 getBranchBitPattern(*branch_arg, !buffer_selector_);
800 char node_XOR_bit_pattern = getBranchXORBitPattern(*branch_arg);
803 char unused_branches_bit_pattern =
804 node_XOR_bit_pattern & occupied_children_bit_pattern_prev_buffer;
807 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
808 if (branch_arg->
hasChild(buffer_selector_, child_idx)) {
814 treeCleanUpRecursive(
static_cast<BranchNode*
>(child_node));
826 if (unused_branches_bit_pattern & (1 << child_idx)) {
828 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
835#define PCL_INSTANTIATE_Octree2BufBase(T) \
836 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.