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>
74 assert(max_voxel_index_arg > 0);
79 std::ceil(std::log2(max_voxel_index_arg))),
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>
111 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
114 return (findLeaf(key));
118 template <
typename LeafContainerT,
typename BranchContainerT>
125 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
128 return (createLeaf(key));
132 template <
typename LeafContainerT,
typename BranchContainerT>
139 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
142 return existLeaf(key);
146 template <
typename LeafContainerT,
typename BranchContainerT>
153 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
156 return (this->removeLeaf(key));
160 template <
typename LeafContainerT,
typename BranchContainerT>
166 deleteBranch(*root_node_);
170 tree_dirty_flag_ =
false;
177 template <
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);
201 template <
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;
220 template <
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;
248 template <
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;
268 template <
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;
297 template <
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;
336 template <
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;
355 template <
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);
392 branch_arg->
setChildPtr(buffer_selector_, child_idx, 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);
413 branch_arg->
getChildPtr(buffer_selector_, child_idx));
416 return createLeafRecursive(key_arg,
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;
459 parent_of_leaf_arg = branch_arg;
462 return depth_mask_arg;
466 template <
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)) {
502 template <
typename LeafContainerT,
typename BranchContainerT>
508 unsigned char child_idx;
515 if (depth_mask_arg > 1) {
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);
554 template <
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);
639 template <
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);
787 template <
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>;
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.
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...
LeafContainerT LeafContainer
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...
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 & getContainer() const
Get const reference to container.
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.