39 #ifndef PCL_OCTREE_BASE_HPP 40 #define PCL_OCTREE_BASE_HPP 42 #include <pcl/impl/instantiate.hpp> 49 template <
typename LeafContainerT,
typename BranchContainerT>
56 , dynamic_depth_enabled_(false)
60 template <
typename LeafContainerT,
typename BranchContainerT>
69 template <
typename LeafContainerT,
typename BranchContainerT>
72 unsigned int max_voxel_index_arg)
74 unsigned int tree_depth;
76 assert(max_voxel_index_arg > 0);
80 std::min(static_cast<unsigned int>(OctreeKey::maxDepth),
81 static_cast<unsigned int>(std::ceil(std::log2(max_voxel_index_arg))));
84 depth_mask_ = (1 << (tree_depth - 1));
88 template <
typename LeafContainerT,
typename BranchContainerT>
92 assert(depth_arg > 0);
95 octree_depth_ = depth_arg;
98 depth_mask_ = (1 << (depth_arg - 1));
101 max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
105 template <
typename LeafContainerT,
typename BranchContainerT>
108 unsigned int idx_y_arg,
109 unsigned int idx_z_arg)
112 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
115 return (findLeaf(key));
119 template <
typename LeafContainerT,
typename BranchContainerT>
122 unsigned int idx_y_arg,
123 unsigned int idx_z_arg)
126 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
129 return (createLeaf(key));
133 template <
typename LeafContainerT,
typename BranchContainerT>
136 unsigned int idx_y_arg,
137 unsigned int idx_z_arg)
const 140 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
143 return (existLeaf(key));
147 template <
typename LeafContainerT,
typename BranchContainerT>
150 unsigned int idx_y_arg,
151 unsigned int idx_z_arg)
154 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
157 deleteLeafRecursive(key, depth_mask_, root_node_);
161 template <
typename LeafContainerT,
typename BranchContainerT>
168 deleteBranch(*root_node_);
175 template <
typename LeafContainerT,
typename BranchContainerT>
178 std::vector<char>& binary_tree_out_arg)
184 binary_tree_out_arg.clear();
185 binary_tree_out_arg.reserve(this->branch_count_);
187 serializeTreeRecursive(root_node_, new_key, &binary_tree_out_arg,
nullptr);
191 template <
typename LeafContainerT,
typename BranchContainerT>
194 std::vector<char>& binary_tree_out_arg,
195 std::vector<LeafContainerT*>& leaf_container_vector_arg)
201 binary_tree_out_arg.clear();
202 leaf_container_vector_arg.clear();
204 binary_tree_out_arg.reserve(this->branch_count_);
205 leaf_container_vector_arg.reserve(this->leaf_count_);
207 serializeTreeRecursive(
208 root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg);
212 template <
typename LeafContainerT,
typename BranchContainerT>
215 std::vector<LeafContainerT*>& leaf_container_vector_arg)
220 leaf_container_vector_arg.clear();
222 leaf_container_vector_arg.reserve(this->leaf_count_);
224 serializeTreeRecursive(root_node_, new_key,
nullptr, &leaf_container_vector_arg);
228 template <
typename LeafContainerT,
typename BranchContainerT>
231 std::vector<char>& binary_tree_out_arg)
239 std::vector<char>::const_iterator binary_tree_out_it = binary_tree_out_arg.begin();
240 std::vector<char>::const_iterator binary_tree_out_it_end = binary_tree_out_arg.end();
242 deserializeTreeRecursive(root_node_,
246 binary_tree_out_it_end,
252 template <
typename LeafContainerT,
typename BranchContainerT>
255 std::vector<char>& binary_tree_in_arg,
256 std::vector<LeafContainerT*>& leaf_container_vector_arg)
261 typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it =
262 leaf_container_vector_arg.begin();
265 typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it_end =
266 leaf_container_vector_arg.end();
272 std::vector<char>::const_iterator binary_tree_input_it = binary_tree_in_arg.begin();
273 std::vector<char>::const_iterator binary_tree_input_it_end = binary_tree_in_arg.end();
275 deserializeTreeRecursive(root_node_,
278 binary_tree_input_it,
279 binary_tree_input_it_end,
281 &leaf_vector_it_end);
285 template <
typename LeafContainerT,
typename BranchContainerT>
289 unsigned int depth_mask_arg,
295 unsigned char child_idx;
300 OctreeNode* child_node = (*branch_arg)[child_idx];
303 if ((!dynamic_depth_enabled_) && (depth_mask_arg > 1)) {
305 BranchNode* childBranch = createBranchChild(*branch_arg, child_idx);
310 return createLeafRecursive(key_arg,
317 LeafNode* leaf_node = createLeafChild(*branch_arg, child_idx);
318 return_leaf_arg = leaf_node;
319 parent_of_leaf_arg = branch_arg;
327 return createLeafRecursive(key_arg,
329 static_cast<BranchNode*>(child_node),
335 return_leaf_arg =
static_cast<LeafNode*
>(child_node);
336 parent_of_leaf_arg = branch_arg;
341 return (depth_mask_arg >> 1);
345 template <
typename LeafContainerT,
typename BranchContainerT>
349 unsigned int depth_mask_arg,
351 LeafContainerT*& result_arg)
const 354 unsigned char child_idx;
359 OctreeNode* child_node = (*branch_arg)[child_idx];
366 child_branch =
static_cast<BranchNode*
>(child_node);
368 findLeafRecursive(key_arg, depth_mask_arg / 2, child_branch, result_arg);
374 child_leaf =
static_cast<LeafNode*
>(child_node);
383 template <
typename LeafContainerT,
typename BranchContainerT>
389 unsigned char child_idx;
396 OctreeNode* child_node = (*branch_arg)[child_idx];
403 child_branch =
static_cast<BranchNode*
>(child_node);
406 b_no_children = deleteLeafRecursive(key_arg, depth_mask_arg / 2, child_branch);
408 if (!b_no_children) {
410 deleteBranchChild(*branch_arg, child_idx);
419 deleteBranchChild(*branch_arg, child_idx);
426 b_no_children =
false;
427 for (child_idx = 0; (!b_no_children) && (child_idx < 8); child_idx++) {
428 b_no_children = branch_arg->
hasChild(child_idx);
431 return (b_no_children);
435 template <
typename LeafContainerT,
typename BranchContainerT>
440 std::vector<char>* binary_tree_out_arg,
441 typename std::vector<LeafContainerT*>* leaf_container_vector_arg)
const 443 char node_bit_pattern;
446 node_bit_pattern = getBranchBitPattern(*branch_arg);
449 if (binary_tree_out_arg)
450 binary_tree_out_arg->push_back(node_bit_pattern);
453 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
456 if (branch_arg->
hasChild(child_idx)) {
465 serializeTreeRecursive(static_cast<const BranchNode*>(childNode),
468 leaf_container_vector_arg);
474 if (leaf_container_vector_arg)
478 serializeTreeCallback(**child_leaf, key_arg);
492 template <
typename LeafContainerT,
typename BranchContainerT>
496 unsigned int depth_mask_arg,
498 typename std::vector<char>::const_iterator& binary_tree_input_it_arg,
499 typename std::vector<char>::const_iterator& binary_tree_input_it_end_arg,
500 typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_arg,
501 typename std::vector<LeafContainerT*>::const_iterator*
502 leaf_container_vector_it_end_arg)
505 if (binary_tree_input_it_arg != binary_tree_input_it_end_arg) {
507 char node_bits = (*binary_tree_input_it_arg);
508 binary_tree_input_it_arg++;
511 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
513 if (node_bits & (1 << child_idx)) {
517 if (depth_mask_arg > 1) {
519 BranchNode* newBranch = createBranchChild(*branch_arg, child_idx);
524 deserializeTreeRecursive(newBranch,
527 binary_tree_input_it_arg,
528 binary_tree_input_it_end_arg,
529 leaf_container_vector_it_arg,
530 leaf_container_vector_it_end_arg);
535 LeafNode* child_leaf = createLeafChild(*branch_arg, child_idx);
537 if (leaf_container_vector_it_arg &&
538 (*leaf_container_vector_it_arg != *leaf_container_vector_it_end_arg)) {
539 LeafContainerT& container = **child_leaf;
540 container = ***leaf_container_vector_it_arg;
541 ++*leaf_container_vector_it_arg;
547 deserializeTreeCallback(**child_leaf, key_arg);
560 #define PCL_INSTANTIATE_OctreeBase(T) \ 561 template class PCL_EXPORTS pcl::octree::OctreeBase<T>; bool hasChild(unsigned char child_idx_arg) const
Check if branch is pointing to a particular child node.
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
OctreeNode * getChildPtr(unsigned char child_idx_arg) const
Get pointer to child.
OctreeBase()
Empty constructor.
void popBranch()
pop child node from octree key
unsigned char getChildIdxWithDepthMask(unsigned int depthMask) const
get child node index using depthMask
Abstract octree leaf class
const ContainerT * getContainerPtr() const
Get const pointer to container.
Abstract octree branch class
void pushBranch(unsigned char childIndex)
push a child node to the octree key
Abstract octree node class