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);
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>;
void serializeTreeRecursive(const BranchNode *branch_arg, OctreeKey &key_arg, std::vector< char > *binary_tree_out_arg, typename std::vector< LeafContainerT * > *leaf_container_vector_arg) const
Recursively explore the octree and output binary octree description together with a vector of leaf no...
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 deleteTree()
Delete the octree structure and its leaf nodes.
void setMaxVoxelIndex(unsigned int max_voxel_index_arg)
Set the maximum amount of voxels per dimension.
void deserializeTree(std::vector< char > &binary_tree_input_arg)
Deserialize a binary octree description vector and create a corresponding octree structure.
static const unsigned char maxDepth
bool deleteLeafRecursive(const OctreeKey &key_arg, unsigned int depth_mask_arg, BranchNode *branch_arg)
Recursively search and delete leaf node.
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
virtual ~OctreeBase()
Empty deconstructor.
OctreeNode * getChildPtr(unsigned char child_idx_arg) const
Get pointer to child.
bool existLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const
idx_x_arg for the existence of leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
void setTreeDepth(unsigned int max_depth_arg)
Set the maximum depth of the octree.
OctreeBase()
Empty constructor.
void popBranch()
pop child node from octree key
unsigned int createLeafRecursive(const OctreeKey &key_arg, unsigned int depth_mask_arg, BranchNode *branch_arg, LeafNode *&return_leaf_arg, BranchNode *&parent_of_leaf_arg)
Create a leaf node at octree key.
void serializeTree(std::vector< char > &binary_tree_out_arg)
Serialize octree into a binary output vector describing its branch node structure.
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).
bool hasChild(unsigned char child_idx_arg) const
Check if branch is pointing to a particular child node.
unsigned char getChildIdxWithDepthMask(unsigned int depthMask) const
get child node index using depthMask
void serializeLeafs(std::vector< LeafContainerT * > &leaf_container_vector_arg)
Outputs a vector of all LeafContainerT elements that are stored within the octree leaf nodes...
const ContainerT * getContainerPtr() const
Get const pointer to container.
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).
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.
Abstract octree leaf class
void deserializeTreeRecursive(BranchNode *branch_arg, unsigned int depth_mask_arg, OctreeKey &key_arg, typename std::vector< char >::const_iterator &binary_tree_input_it_arg, typename std::vector< char >::const_iterator &binary_tree_input_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)
Recursive method for deserializing octree structure.
Abstract octree branch class
void pushBranch(unsigned char childIndex)
push a child node to the octree key
Abstract octree node class