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 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