39 #ifndef PCL_OCTREE_2BUF_BASE_HPP
40 #define PCL_OCTREE_2BUF_BASE_HPP
47 template<
typename LeafContainerT,
typename BranchContainerT>
55 tree_dirty_flag_ (false),
57 dynamic_depth_enabled_(false)
62 template<
typename LeafContainerT,
typename BranchContainerT>
71 template<
typename LeafContainerT,
typename BranchContainerT>
void
74 unsigned int treeDepth;
76 assert (max_voxel_index_arg > 0);
80 static_cast<unsigned int> (std::ceil (Log2 (max_voxel_index_arg))))),
81 static_cast<unsigned int> (0));
84 depth_mask_ = (1 << (treeDepth - 1));
88 template<
typename LeafContainerT,
typename BranchContainerT>
void
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> LeafContainerT*
108 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
111 return ( findLeaf (key));
115 template<
typename LeafContainerT,
typename BranchContainerT> LeafContainerT*
119 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
122 return ( createLeaf (key));
126 template<
typename LeafContainerT,
typename BranchContainerT>
bool
130 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
133 return existLeaf (key);
137 template<
typename LeafContainerT,
typename BranchContainerT>
void
141 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
144 return (this->removeLeaf (key));
148 template<
typename LeafContainerT,
typename BranchContainerT>
void
154 deleteBranch (*root_node_);
158 tree_dirty_flag_ =
false;
165 template<
typename LeafContainerT,
typename BranchContainerT>
void
168 if (tree_dirty_flag_)
171 treeCleanUpRecursive (root_node_);
175 buffer_selector_ = !buffer_selector_;
178 tree_dirty_flag_ =
true;
182 unsigned char child_idx;
184 for (child_idx = 0; child_idx < 8; child_idx++)
186 root_node_->setChildPtr(buffer_selector_, child_idx, 0);
191 template<
typename LeafContainerT,
typename BranchContainerT>
void
193 bool do_XOR_encoding_arg)
198 binary_tree_out_arg.clear ();
199 binary_tree_out_arg.reserve (this->branch_count_);
201 serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, 0, do_XOR_encoding_arg,
false);
204 tree_dirty_flag_ =
false;
208 template<
typename LeafContainerT,
typename BranchContainerT>
void
210 std::vector<LeafContainerT*>& leaf_container_vector_arg,
211 bool do_XOR_encoding_arg)
216 binary_tree_out_arg.clear ();
217 leaf_container_vector_arg.clear ();
219 leaf_container_vector_arg.reserve (leaf_count_);
220 binary_tree_out_arg.reserve (this->branch_count_);
222 serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg, do_XOR_encoding_arg,
false);
225 tree_dirty_flag_ =
false;
229 template<
typename LeafContainerT,
typename BranchContainerT>
void
235 leaf_container_vector_arg.clear ();
237 leaf_container_vector_arg.reserve (leaf_count_);
239 serializeTreeRecursive (root_node_, new_key, 0, &leaf_container_vector_arg,
false,
false);
242 tree_dirty_flag_ =
false;
246 template<
typename LeafContainerT,
typename BranchContainerT>
void
248 bool do_XOR_decoding_arg)
256 std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin ();
257 std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end ();
259 deserializeTreeRecursive (root_node_, depth_mask_, new_key,
260 binary_tree_in_it, binary_tree_in_it_end, 0, 0,
false,
261 do_XOR_decoding_arg);
264 tree_dirty_flag_ =
false;
268 template<
typename LeafContainerT,
typename BranchContainerT>
void
270 std::vector<LeafContainerT*>& leaf_container_vector_arg,
271 bool do_XOR_decoding_arg)
276 typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it = leaf_container_vector_arg.begin ();
279 typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it_end = leaf_container_vector_arg.end ();
285 std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin ();
286 std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end ();
288 deserializeTreeRecursive (root_node_,
292 binary_tree_in_it_end,
293 &leaf_container_vector_it,
294 &leaf_container_vector_it_end,
296 do_XOR_decoding_arg);
300 tree_dirty_flag_ =
false;
305 template<
typename LeafContainerT,
typename BranchContainerT>
void
311 leaf_container_vector_arg.clear ();
312 leaf_container_vector_arg.reserve (leaf_count_);
314 serializeTreeRecursive (root_node_, new_key, 0, &leaf_container_vector_arg,
false,
true);
317 tree_dirty_flag_ =
false;
321 template<
typename LeafContainerT,
typename BranchContainerT>
324 unsigned int depth_mask_arg,
328 bool branch_reset_arg)
331 unsigned char child_idx;
334 if (branch_reset_arg)
337 for (child_idx = 0; child_idx < 8; child_idx++)
339 branch_arg->
setChildPtr(buffer_selector_, child_idx, 0);
346 if (depth_mask_arg > 1)
355 if (!branch_arg->
hasChild(buffer_selector_, child_idx))
359 if (branch_arg->
hasChild(!buffer_selector_, child_idx))
364 child_branch =
static_cast<BranchNode*
> (child_node);
365 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
368 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
369 child_branch = createBranchChild (*branch_arg, child_idx);
379 child_branch = createBranchChild (*branch_arg, child_idx);
389 return createLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, return_leaf_arg, parent_of_leaf_arg, doNodeReset);
395 if (!branch_arg->
hasChild(buffer_selector_, child_idx))
400 if (branch_arg->
hasChild(!buffer_selector_, child_idx))
406 child_leaf =
static_cast<LeafNode*
> (child_node);
407 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
410 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
411 child_leaf = createLeafChild (*branch_arg, child_idx);
418 child_leaf = createLeafChild (*branch_arg, child_idx);
423 return_leaf_arg = child_leaf;
424 parent_of_leaf_arg = branch_arg;
429 return_leaf_arg =
static_cast<LeafNode*
> (branch_arg->
getChildPtr(buffer_selector_,child_idx));;
430 parent_of_leaf_arg = branch_arg;
434 return depth_mask_arg;
438 template<
typename LeafContainerT,
typename BranchContainerT>
void
440 unsigned int depth_mask_arg,
442 LeafContainerT*& result_arg)
const
445 unsigned char child_idx;
450 if (depth_mask_arg > 1)
458 findLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, result_arg);
463 if (branch_arg->
hasChild(buffer_selector_, child_idx))
473 template<
typename LeafContainerT,
typename BranchContainerT>
bool
475 unsigned int depth_mask_arg,
479 unsigned char child_idx;
486 if (depth_mask_arg > 1)
490 bool bBranchOccupied;
498 bBranchOccupied = deleteLeafRecursive (key_arg, depth_mask_arg / 2, child_branch);
500 if (!bBranchOccupied)
503 deleteBranchChild (*branch_arg, buffer_selector_, child_idx);
511 deleteBranchChild (*branch_arg, buffer_selector_, child_idx);
517 for (child_idx = 0; child_idx < 8; child_idx++)
519 bNoChilds = branch_arg->
hasChild(buffer_selector_, child_idx);
529 template<
typename LeafContainerT,
typename BranchContainerT>
void Octree2BufBase<
530 LeafContainerT, BranchContainerT>::serializeTreeRecursive (
BranchNode* branch_arg,
532 std::vector<char>* binary_tree_out_arg,
533 typename std::vector<LeafContainerT*>* leaf_container_vector_arg,
534 bool do_XOR_encoding_arg,
535 bool new_leafs_filter_arg)
538 unsigned char child_idx;
541 char branch_bit_pattern_curr_buffer;
542 char branch_bit_pattern_prev_buffer;
543 char node_XOR_bit_pattern;
546 branch_bit_pattern_curr_buffer = getBranchBitPattern (*branch_arg, buffer_selector_);
547 branch_bit_pattern_prev_buffer = getBranchBitPattern (*branch_arg, !buffer_selector_);
550 node_XOR_bit_pattern = branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer;
552 if (binary_tree_out_arg)
554 if (do_XOR_encoding_arg)
557 binary_tree_out_arg->push_back (node_XOR_bit_pattern);
562 binary_tree_out_arg->push_back (branch_bit_pattern_curr_buffer);
567 for (child_idx = 0; child_idx < 8; child_idx++)
569 if (branch_arg->
hasChild(buffer_selector_, child_idx))
581 serializeTreeRecursive (static_cast<BranchNode*> (child_node), key_arg, binary_tree_out_arg,
582 leaf_container_vector_arg, do_XOR_encoding_arg, new_leafs_filter_arg);
589 if (new_leafs_filter_arg)
591 if (!branch_arg->
hasChild (!buffer_selector_, child_idx))
593 if (leaf_container_vector_arg)
596 serializeTreeCallback (**child_leaf, key_arg);
601 if (leaf_container_vector_arg)
604 serializeTreeCallback (**child_leaf, key_arg);
616 else if (branch_arg->
hasChild (!buffer_selector_, child_idx))
619 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
628 template<
typename LeafContainerT,
typename BranchContainerT>
void
630 unsigned int depth_mask_arg,
OctreeKey& key_arg,
631 typename std::vector<char>::const_iterator& binaryTreeIT_arg,
632 typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
633 typename std::vector<LeafContainerT*>::const_iterator* dataVectorIterator_arg,
634 typename std::vector<LeafContainerT*>::const_iterator* dataVectorEndIterator_arg,
635 bool branch_reset_arg,
bool do_XOR_decoding_arg)
638 unsigned char child_idx;
642 char recoveredNodeBits;
645 if (branch_reset_arg)
648 for (child_idx = 0; child_idx < 8; child_idx++)
650 branch_arg->
setChildPtr(buffer_selector_, child_idx, 0);
654 if (binaryTreeIT_arg!=binaryTreeIT_End_arg) {
656 nodeBits = *binaryTreeIT_arg++;
660 if (do_XOR_decoding_arg)
662 recoveredNodeBits = getBranchBitPattern (*branch_arg, !buffer_selector_) ^ nodeBits;
666 recoveredNodeBits = nodeBits;
670 for (child_idx = 0; child_idx < 8; child_idx++)
673 if (recoveredNodeBits & (1 << child_idx))
682 if (depth_mask_arg > 1)
689 if (!branch_arg->
hasChild(buffer_selector_, child_idx))
692 if (branch_arg->
hasChild(!buffer_selector_, child_idx))
697 child_branch =
static_cast<BranchNode*
> (child_node);
698 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
701 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
702 child_branch = createBranchChild (*branch_arg, child_idx);
711 child_branch = createBranchChild (*branch_arg, child_idx);
724 deserializeTreeRecursive (child_branch, depth_mask_arg / 2, key_arg,
725 binaryTreeIT_arg, binaryTreeIT_End_arg,
726 dataVectorIterator_arg, dataVectorEndIterator_arg,
727 doNodeReset, do_XOR_decoding_arg);
736 if (branch_arg->
hasChild(!buffer_selector_, child_idx))
741 child_leaf =
static_cast<LeafNode*
> (child_node);
742 branch_arg->
setChildPtr(buffer_selector_, child_idx, child_node);
745 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
746 child_leaf = createLeafChild (*branch_arg, child_idx);
752 child_leaf = createLeafChild (*branch_arg, child_idx);
757 if (dataVectorIterator_arg
758 && (*dataVectorIterator_arg != *dataVectorEndIterator_arg))
760 LeafContainerT& container = **child_leaf;
761 container = ***dataVectorIterator_arg;
762 ++*dataVectorIterator_arg;
768 deserializeTreeCallback (**child_leaf, key_arg);
775 else if (branch_arg->
hasChild (!buffer_selector_, child_idx))
778 branch_arg->
setChildPtr(buffer_selector_, child_idx, 0);
781 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
789 template<
typename LeafContainerT,
typename BranchContainerT>
void
793 unsigned char child_idx;
796 char occupied_children_bit_pattern_prev_buffer;
797 char node_XOR_bit_pattern;
798 char unused_branches_bit_pattern;
801 occupied_children_bit_pattern_prev_buffer = getBranchBitPattern (*branch_arg, !buffer_selector_);
804 node_XOR_bit_pattern = getBranchXORBitPattern (*branch_arg);
807 unused_branches_bit_pattern = node_XOR_bit_pattern & occupied_children_bit_pattern_prev_buffer;
810 for (child_idx = 0; child_idx < 8; child_idx++)
812 if (branch_arg->
hasChild(buffer_selector_, child_idx))
821 treeCleanUpRecursive (static_cast<BranchNode*> (child_node));
833 if (unused_branches_bit_pattern & (1 << child_idx))
836 deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
843 #define PCL_INSTANTIATE_Octree2BufBase(T) template class PCL_EXPORTS pcl::octree::Octree2BufBase<T>;
void serializeLeafs(std::vector< LeafContainerT * > &leaf_container_vector_arg)
Outputs a vector of all DataT elements that are stored within the octree leaf nodes.
LeafContainerT * createLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg)
Create new leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
void removeLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg)
Remove leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
Octree double buffer class
void switchBuffers()
Switch buffers and reset current octree structure.
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.
void popBranch()
pop child node from octree key
void setChildPtr(unsigned char buffer_arg, unsigned char index_arg, OctreeNode *newNode_arg)
Set child pointer in current branch node.
unsigned char getChildIdxWithDepthMask(unsigned int depthMask) const
get child node index using depthMask
void deleteTree()
Delete the octree structure and its leaf nodes.
Abstract octree node class
bool deleteLeafRecursive(const OctreeKey &key_arg, unsigned int depth_mask_arg, BranchNode *branch_arg)
Recursively search and delete leaf node.
unsigned int createLeafRecursive(const OctreeKey &key_arg, unsigned int 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.
bool hasChild(unsigned char buffer_arg, unsigned char index_arg) const
Check if branch is pointing to a particular child node.
void treeCleanUpRecursive(BranchNode *branch_arg)
Recursively explore the octree and remove unused branch and leaf nodes.
virtual ~Octree2BufBase()
Empty deconstructor.
void findLeafRecursive(const OctreeKey &key_arg, unsigned int depth_mask_arg, BranchNode *branch_arg, LeafContainerT *&result_arg) const
Recursively search for a given leaf node and return a pointer.
OctreeNode * getChildPtr(unsigned char buffer_arg, unsigned char index_arg) const
Get child pointer in current branch node.
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...
void deserializeTreeRecursive(BranchNode *branch_arg, unsigned int 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...
const ContainerT * getContainerPtr() const
Get const pointer to container.
void pushBranch(unsigned char childIndex)
push a child node to the octree key
void setTreeDepth(unsigned int depth_arg)
Set the maximum depth of the octree.
static const unsigned char maxDepth
void setMaxVoxelIndex(unsigned int max_voxel_index_arg)
Set the maximum amount of voxels per dimension.
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.
LeafContainerT * findLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg)
Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
bool existLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const
Check for the existence of leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
Abstract octree leaf class
Octree2BufBase()
Empty constructor.
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)