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 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));
92 template <
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;
114 template <
typename LeafContainerT,
typename BranchContainerT>
121 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
124 return (findLeaf(key));
128 template <
typename LeafContainerT,
typename BranchContainerT>
135 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
138 return (createLeaf(key));
142 template <
typename LeafContainerT,
typename BranchContainerT>
149 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
152 return existLeaf(key);
156 template <
typename LeafContainerT,
typename BranchContainerT>
163 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
166 return (this->removeLeaf(key));
170 template <
typename LeafContainerT,
typename BranchContainerT>
176 deleteBranch(*root_node_);
180 tree_dirty_flag_ =
false;
187 template <
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);
211 template <
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;
230 template <
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;
258 template <
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;
278 template <
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;
307 template <
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;
346 template <
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;
365 template <
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)) {
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);
423 branch_arg->
getChildPtr(buffer_selector_, child_idx));
426 return createLeafRecursive(key_arg,
436 if (!branch_arg->
hasChild(buffer_selector_, child_idx)) {
440 if (branch_arg->
hasChild(!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;
469 parent_of_leaf_arg = branch_arg;
472 return depth_mask_arg;
476 template <
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)) {
512 template <
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);
564 template <
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);
649 template <
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);
797 template <
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>;
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 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.