39 #ifndef PCL_OCTREE_BASE_HPP
40 #define PCL_OCTREE_BASE_HPP
47 template <
typename LeafContainerT,
typename BranchContainerT>
54 , dynamic_depth_enabled_(false)
58 template <
typename LeafContainerT,
typename BranchContainerT>
67 template <
typename LeafContainerT,
typename BranchContainerT>
74 assert(max_voxel_index_arg > 0);
79 static_cast<uindex_t>(std::ceil(std::log2(max_voxel_index_arg))));
82 depth_mask_ = (1 << (tree_depth - 1));
86 template <
typename LeafContainerT,
typename BranchContainerT>
90 assert(depth_arg > 0);
93 octree_depth_ = depth_arg;
96 depth_mask_ = (1 << (depth_arg - 1));
99 max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
103 template <
typename LeafContainerT,
typename BranchContainerT>
110 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
113 return (findLeaf(key));
117 template <
typename LeafContainerT,
typename BranchContainerT>
124 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
127 return (createLeaf(key));
131 template <
typename LeafContainerT,
typename BranchContainerT>
138 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
141 return (existLeaf(key));
145 template <
typename LeafContainerT,
typename BranchContainerT>
152 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
155 deleteLeafRecursive(key, depth_mask_, root_node_);
159 template <
typename LeafContainerT,
typename BranchContainerT>
166 deleteBranch(*root_node_);
173 template <
typename LeafContainerT,
typename BranchContainerT>
176 std::vector<char>& binary_tree_out_arg)
182 binary_tree_out_arg.clear();
183 binary_tree_out_arg.reserve(this->branch_count_);
185 serializeTreeRecursive(root_node_, new_key, &binary_tree_out_arg,
nullptr);
189 template <
typename LeafContainerT,
typename BranchContainerT>
192 std::vector<char>& binary_tree_out_arg,
193 std::vector<LeafContainerT*>& leaf_container_vector_arg)
199 binary_tree_out_arg.clear();
200 leaf_container_vector_arg.clear();
202 binary_tree_out_arg.reserve(this->branch_count_);
203 leaf_container_vector_arg.reserve(this->leaf_count_);
205 serializeTreeRecursive(
206 root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg);
210 template <
typename LeafContainerT,
typename BranchContainerT>
213 std::vector<LeafContainerT*>& leaf_container_vector_arg)
218 leaf_container_vector_arg.clear();
220 leaf_container_vector_arg.reserve(this->leaf_count_);
222 serializeTreeRecursive(root_node_, new_key,
nullptr, &leaf_container_vector_arg);
226 template <
typename LeafContainerT,
typename BranchContainerT>
229 std::vector<char>& binary_tree_out_arg)
237 std::vector<char>::const_iterator binary_tree_out_it = binary_tree_out_arg.begin();
238 std::vector<char>::const_iterator binary_tree_out_it_end = binary_tree_out_arg.end();
240 deserializeTreeRecursive(root_node_,
244 binary_tree_out_it_end,
250 template <
typename LeafContainerT,
typename BranchContainerT>
253 std::vector<char>& binary_tree_in_arg,
254 std::vector<LeafContainerT*>& leaf_container_vector_arg)
259 typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it =
260 leaf_container_vector_arg.begin();
263 typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it_end =
264 leaf_container_vector_arg.end();
270 std::vector<char>::const_iterator binary_tree_input_it = binary_tree_in_arg.begin();
271 std::vector<char>::const_iterator binary_tree_input_it_end = binary_tree_in_arg.end();
273 deserializeTreeRecursive(root_node_,
276 binary_tree_input_it,
277 binary_tree_input_it_end,
279 &leaf_vector_it_end);
283 template <
typename LeafContainerT,
typename BranchContainerT>
293 unsigned char child_idx;
298 OctreeNode* child_node = (*branch_arg)[child_idx];
301 if ((!dynamic_depth_enabled_) && (depth_mask_arg > 1)) {
303 BranchNode* childBranch = createBranchChild(*branch_arg, child_idx);
308 return createLeafRecursive(key_arg,
315 LeafNode* leaf_node = createLeafChild(*branch_arg, child_idx);
316 return_leaf_arg = leaf_node;
317 parent_of_leaf_arg = branch_arg;
325 return createLeafRecursive(key_arg,
327 static_cast<BranchNode*
>(child_node),
333 return_leaf_arg =
static_cast<LeafNode*
>(child_node);
334 parent_of_leaf_arg = branch_arg;
339 return (depth_mask_arg >> 1);
343 template <
typename LeafContainerT,
typename BranchContainerT>
349 LeafContainerT*& result_arg)
const
352 unsigned char child_idx;
357 OctreeNode* child_node = (*branch_arg)[child_idx];
364 child_branch =
static_cast<BranchNode*
>(child_node);
366 findLeafRecursive(key_arg, depth_mask_arg / 2, child_branch, result_arg);
381 template <
typename LeafContainerT,
typename BranchContainerT>
387 unsigned char child_idx;
394 OctreeNode* child_node = (*branch_arg)[child_idx];
401 child_branch =
static_cast<BranchNode*
>(child_node);
404 b_no_children = deleteLeafRecursive(key_arg, depth_mask_arg / 2, child_branch);
406 if (!b_no_children) {
408 deleteBranchChild(*branch_arg, child_idx);
417 deleteBranchChild(*branch_arg, child_idx);
424 b_no_children =
false;
425 for (child_idx = 0; (!b_no_children) && (child_idx < 8); child_idx++) {
426 b_no_children = branch_arg->
hasChild(child_idx);
429 return (b_no_children);
433 template <
typename LeafContainerT,
typename BranchContainerT>
438 std::vector<char>* binary_tree_out_arg,
439 typename std::vector<LeafContainerT*>* leaf_container_vector_arg)
const
441 char node_bit_pattern;
444 node_bit_pattern = getBranchBitPattern(*branch_arg);
447 if (binary_tree_out_arg)
448 binary_tree_out_arg->push_back(node_bit_pattern);
451 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
454 if (branch_arg->
hasChild(child_idx)) {
463 serializeTreeRecursive(
static_cast<const BranchNode*
>(childNode),
466 leaf_container_vector_arg);
472 if (leaf_container_vector_arg)
476 serializeTreeCallback(**child_leaf, key_arg);
490 template <
typename LeafContainerT,
typename BranchContainerT>
496 typename std::vector<char>::const_iterator& binary_tree_input_it_arg,
497 typename std::vector<char>::const_iterator& binary_tree_input_it_end_arg,
498 typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_arg,
499 typename std::vector<LeafContainerT*>::const_iterator*
500 leaf_container_vector_it_end_arg)
503 if (binary_tree_input_it_arg != binary_tree_input_it_end_arg) {
505 char node_bits = (*binary_tree_input_it_arg);
506 binary_tree_input_it_arg++;
509 for (
unsigned char child_idx = 0; child_idx < 8; child_idx++) {
511 if (node_bits & (1 << child_idx)) {
515 if (depth_mask_arg > 1) {
517 BranchNode* newBranch = createBranchChild(*branch_arg, child_idx);
522 deserializeTreeRecursive(newBranch,
525 binary_tree_input_it_arg,
526 binary_tree_input_it_end_arg,
527 leaf_container_vector_it_arg,
528 leaf_container_vector_it_end_arg);
533 LeafNode* child_leaf = createLeafChild(*branch_arg, child_idx);
535 if (leaf_container_vector_it_arg &&
536 (*leaf_container_vector_it_arg != *leaf_container_vector_it_end_arg)) {
537 LeafContainerT& container = **child_leaf;
538 container = ***leaf_container_vector_it_arg;
539 ++*leaf_container_vector_it_arg;
545 deserializeTreeCallback(**child_leaf, key_arg);
558 #define PCL_INSTANTIATE_OctreeBase(T) \
559 template class PCL_EXPORTS pcl::octree::OctreeBase<T>;
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.
void setTreeDepth(uindex_t max_depth_arg)
Set the maximum depth of the octree.
void deserializeTreeRecursive(BranchNode *branch_arg, uindex_t 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.
void serializeLeafs(std::vector< LeafContainerT * > &leaf_container_vector_arg)
Outputs a vector of all LeafContainerT elements that are stored within the octree leaf nodes.
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).
virtual ~OctreeBase()
Empty deconstructor.
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).
bool deleteLeafRecursive(const OctreeKey &key_arg, uindex_t depth_mask_arg, BranchNode *branch_arg)
Recursively search and delete leaf node.
uindex_t createLeafRecursive(const OctreeKey &key_arg, uindex_t depth_mask_arg, BranchNode *branch_arg, LeafNode *&return_leaf_arg, BranchNode *&parent_of_leaf_arg)
Create a leaf node at octree key.
void deserializeTree(std::vector< char > &binary_tree_input_arg)
Deserialize a binary octree description vector and create a corresponding octree structure.
void deleteTree()
Delete the octree structure and its leaf nodes.
void serializeTree(std::vector< char > &binary_tree_out_arg)
Serialize octree into a binary output vector describing its branch node structure.
bool existLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg) const
idx_x_arg for the existence of leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
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...
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).
void setMaxVoxelIndex(uindex_t max_voxel_index_arg)
Set the maximum amount of voxels per dimension.
OctreeBase()
Empty constructor.
Abstract octree branch class
bool hasChild(unsigned char child_idx_arg) const
Check if branch is pointing to a particular child node.
OctreeNode * getChildPtr(unsigned char child_idx_arg) const
Get pointer to child.
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 * getContainerPtr() const
Get const pointer to container.
Abstract octree node class
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving 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.