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);
386 child_branch = static_cast<BranchNode*> (branch_arg->
getChildPtr(buffer_selector_,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)
454 child_branch = static_cast<BranchNode*> (branch_arg->
getChildPtr(buffer_selector_,child_idx));
458 findLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, result_arg);
463 if (branch_arg->
hasChild(buffer_selector_, child_idx))
466 LeafNode* leaf_node = static_cast<LeafNode*> (branch_arg->
getChildPtr(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;
493 child_branch = static_cast<BranchNode*> (branch_arg->
getChildPtr(buffer_selector_,child_idx));
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);
587 LeafNode* child_leaf = static_cast<LeafNode*> (child_node);
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);
720 child_branch = static_cast<BranchNode*> (branch_arg->
getChildPtr(buffer_selector_,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>; Octree2BufBase()
Empty constructor.
OctreeNode * getChildPtr(unsigned char buffer_arg, unsigned char index_arg) const
Get child pointer in current branch 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.
void setTreeDepth(unsigned int depth_arg)
Set the maximum depth of the octree.
void treeCleanUpRecursive(BranchNode *branch_arg)
Recursively explore the octree and remove unused branch and leaf nodes.
static const unsigned char maxDepth
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
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 deleteTree()
Delete the octree structure and its leaf nodes.
void popBranch()
pop child node from octree key
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.
void switchBuffers()
Switch buffers and reset current octree structure.
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 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.
virtual ~Octree2BufBase()
Empty deconstructor.
void setMaxVoxelIndex(unsigned int max_voxel_index_arg)
Set the maximum amount of voxels per dimension.
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).
unsigned char getChildIdxWithDepthMask(unsigned int depthMask) const
get child node index using depthMask
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).
void serializeLeafs(std::vector< LeafContainerT * > &leaf_container_vector_arg)
Outputs a vector of all DataT elements that are stored within the octree leaf nodes.
Abstract octree leaf class
const ContainerT * getContainerPtr() const
Get const pointer to container.
Octree double buffer class
void setChildPtr(unsigned char buffer_arg, unsigned char index_arg, OctreeNode *newNode_arg)
Set child pointer in current branch node.
void pushBranch(unsigned char childIndex)
push a child node to the octree key
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).
Abstract octree node class
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).
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...
bool deleteLeafRecursive(const OctreeKey &key_arg, unsigned int depth_mask_arg, BranchNode *branch_arg)
Recursively search and delete leaf node.
bool hasChild(unsigned char buffer_arg, unsigned char index_arg) const
Check if branch is pointing to a particular child node.