39 #ifndef PCL_OCTREE_BASE_HPP
40 #define PCL_OCTREE_BASE_HPP
44 #include <pcl/impl/instantiate.hpp>
45 #include <pcl/point_types.h>
46 #include <pcl/octree/octree.h>
53 template<
typename LeafContainerT,
typename BranchContainerT>
60 dynamic_depth_enabled_ (false),
66 template<
typename LeafContainerT,
typename BranchContainerT>
75 template<
typename LeafContainerT,
typename BranchContainerT>
79 unsigned int tree_depth;
81 assert(max_voxel_index_arg>0);
84 tree_depth = std::max ( (std::min (static_cast<unsigned int> (
OctreeKey::maxDepth), static_cast<unsigned int> (std::ceil (Log2 (max_voxel_index_arg))))), 0u);
87 depth_mask_ = (1 << (tree_depth - 1));
91 template<
typename LeafContainerT,
typename BranchContainerT>
98 octree_depth_ = depth_arg;
101 depth_mask_ = (1 << (depth_arg - 1));
104 max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
108 template<
typename LeafContainerT,
typename BranchContainerT>
111 unsigned int idx_y_arg,
112 unsigned int idx_z_arg)
115 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
118 return (findLeaf (key));
122 template<
typename LeafContainerT,
typename BranchContainerT>
125 unsigned int idx_y_arg,
126 unsigned int idx_z_arg)
129 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
132 return (createLeaf (key));
136 template<
typename LeafContainerT,
typename BranchContainerT>
139 unsigned int idx_y_arg,
140 unsigned int idx_z_arg)
const
143 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
146 return (existLeaf (key));
150 template<
typename LeafContainerT,
typename BranchContainerT>
153 unsigned int idx_y_arg,
154 unsigned int idx_z_arg)
157 OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
160 deleteLeafRecursive (key, depth_mask_, root_node_);
164 template<
typename LeafContainerT,
typename BranchContainerT>
172 deleteBranch (*root_node_);
180 template<
typename LeafContainerT,
typename BranchContainerT>
188 binary_tree_out_arg.clear ();
189 binary_tree_out_arg.reserve (this->branch_count_);
191 serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, 0);
195 template<
typename LeafContainerT,
typename BranchContainerT>
198 std::vector<LeafContainerT*>& leaf_container_vector_arg)
204 binary_tree_out_arg.clear ();
205 leaf_container_vector_arg.clear ();
207 binary_tree_out_arg.reserve (this->branch_count_);
208 leaf_container_vector_arg.reserve (this->leaf_count_);
210 serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg);
214 template<
typename LeafContainerT,
typename BranchContainerT>
221 leaf_container_vector_arg.clear ();
223 leaf_container_vector_arg.reserve (this->leaf_count_);
225 serializeTreeRecursive (root_node_, new_key, 0, &leaf_container_vector_arg);
229 template<
typename LeafContainerT,
typename BranchContainerT>
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,
253 template<
typename LeafContainerT,
typename BranchContainerT>
256 std::vector<LeafContainerT*>& leaf_container_vector_arg)
261 typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it = leaf_container_vector_arg.begin ();
264 typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it_end = 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);
284 template<
typename LeafContainerT,
typename BranchContainerT>
287 unsigned int depth_mask_arg,
293 unsigned char child_idx;
298 OctreeNode* child_node = (*branch_arg)[child_idx];
302 if ((!dynamic_depth_enabled_) && (depth_mask_arg > 1))
305 BranchNode* childBranch = createBranchChild (*branch_arg, child_idx);
310 return createLeafRecursive (key_arg, depth_mask_arg / 2, childBranch, return_leaf_arg, parent_of_leaf_arg);
316 LeafNode* leaf_node = createLeafChild (*branch_arg, child_idx);
317 return_leaf_arg = leaf_node;
318 parent_of_leaf_arg = branch_arg;
329 return createLeafRecursive (key_arg, depth_mask_arg / 2, static_cast<BranchNode*> (child_node),
330 return_leaf_arg, parent_of_leaf_arg);
334 return_leaf_arg =
static_cast<LeafNode*
> (child_node);;
335 parent_of_leaf_arg = branch_arg;
341 return (depth_mask_arg >> 1);
345 template<
typename LeafContainerT,
typename BranchContainerT>
348 unsigned int depth_mask_arg,
350 LeafContainerT*& result_arg)
const
353 unsigned char child_idx;
358 OctreeNode* child_node = (*branch_arg)[child_idx];
367 child_branch =
static_cast<BranchNode*
> (child_node);
369 findLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, result_arg);
375 child_leaf =
static_cast<LeafNode*
> (child_node);
384 template<
typename LeafContainerT,
typename BranchContainerT>
387 unsigned int depth_mask_arg,
391 unsigned char child_idx;
398 OctreeNode* child_node = (*branch_arg)[child_idx];
407 child_branch =
static_cast<BranchNode*
> (child_node);
410 b_no_children = deleteLeafRecursive (key_arg, depth_mask_arg / 2, child_branch);
415 deleteBranchChild (*branch_arg, child_idx);
424 deleteBranchChild (*branch_arg, child_idx);
431 b_no_children =
false;
432 for (child_idx = 0; (!b_no_children) && (child_idx < 8); child_idx++)
434 b_no_children = branch_arg->
hasChild (child_idx);
437 return (b_no_children);
441 template<
typename LeafContainerT,
typename BranchContainerT>
445 std::vector<char>* binary_tree_out_arg,
446 typename std::vector<LeafContainerT*>* leaf_container_vector_arg)
const
450 unsigned char child_idx;
451 char node_bit_pattern;
454 node_bit_pattern = getBranchBitPattern (*branch_arg);
457 if (binary_tree_out_arg)
458 binary_tree_out_arg->push_back (node_bit_pattern);
461 for (child_idx = 0; child_idx < 8; child_idx++)
465 if (branch_arg->
hasChild (child_idx))
477 serializeTreeRecursive (static_cast<const BranchNode*> (childNode), key_arg, binary_tree_out_arg,
478 leaf_container_vector_arg);
485 if (leaf_container_vector_arg)
489 serializeTreeCallback (**child_leaf, key_arg);
503 template<
typename LeafContainerT,
typename BranchContainerT>
506 typename std::vector<char>::const_iterator& binary_tree_input_it_arg,
507 typename std::vector<char>::const_iterator& binary_tree_input_it_end_arg,
508 typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_arg,
509 typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_end_arg)
512 unsigned char child_idx;
515 if (binary_tree_input_it_arg != binary_tree_input_it_end_arg)
518 node_bits = (*binary_tree_input_it_arg);
519 binary_tree_input_it_arg++;
522 for (child_idx = 0; child_idx < 8; child_idx++)
525 if (node_bits & (1 << child_idx))
530 if (depth_mask_arg > 1)
533 BranchNode * newBranch = createBranchChild (*branch_arg, child_idx);
538 deserializeTreeRecursive (newBranch, depth_mask_arg / 2, key_arg,
539 binary_tree_input_it_arg, binary_tree_input_it_end_arg,
540 leaf_container_vector_it_arg, leaf_container_vector_it_end_arg);
546 LeafNode* child_leaf = createLeafChild (*branch_arg, child_idx);
548 if (leaf_container_vector_it_arg && (*leaf_container_vector_it_arg != *leaf_container_vector_it_end_arg))
550 LeafContainerT& container = **child_leaf;
551 container = ***leaf_container_vector_it_arg;
552 ++*leaf_container_vector_it_arg;
558 deserializeTreeCallback (**child_leaf, key_arg);
571 #define PCL_INSTANTIATE_OctreeBase(T) template class PCL_EXPORTS pcl::octree::OctreeBase<T>;
void setTreeDepth(unsigned int max_depth_arg)
Set the maximum depth of the octree.
bool hasChild(unsigned char child_idx_arg) const
Check if branch is pointing to a particular child node.
bool deleteLeafRecursive(const OctreeKey &key_arg, unsigned int depth_mask_arg, BranchNode *branch_arg)
Recursively search and delete leaf node.
Abstract octree branch class
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.
virtual ~OctreeBase()
Empty deconstructor.
void setMaxVoxelIndex(unsigned int max_voxel_index_arg)
Set the maximum amount of voxels per dimension.
void deleteTree()
Delete the octree structure and its leaf nodes.
void popBranch()
pop child node from octree key
unsigned char getChildIdxWithDepthMask(unsigned int depthMask) const
get child node index using depthMask
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 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...
Abstract octree node class
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).
OctreeBase()
Empty constructor.
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 * 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 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).
static const unsigned char maxDepth
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.
void serializeTree(std::vector< char > &binary_tree_out_arg)
Serialize octree into a binary output vector describing its branch node structure.
OctreeNode * getChildPtr(unsigned char child_idx_arg) const
Get pointer to child.
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 deserializeTree(std::vector< char > &binary_tree_input_arg)
Deserialize a binary octree description vector and create a corresponding octree structure.
Abstract octree leaf class
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)