Point Cloud Library (PCL)  1.14.1-dev
octree_base.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  */
38 
39 #ifndef PCL_OCTREE_BASE_HPP
40 #define PCL_OCTREE_BASE_HPP
41 
42 #include <vector>
43 
44 namespace pcl {
45 namespace octree {
46 //////////////////////////////////////////////////////////////////////////////////////////////
47 template <typename LeafContainerT, typename BranchContainerT>
49 : root_node_(new BranchNode())
50 {}
51 
52 //////////////////////////////////////////////////////////////////////////////////////////////
53 template <typename LeafContainerT, typename BranchContainerT>
55 {
56  // deallocate tree structure
57  deleteTree();
58  delete (root_node_);
59 }
60 
61 //////////////////////////////////////////////////////////////////////////////////////////////
62 template <typename LeafContainerT, typename BranchContainerT>
63 void
65  uindex_t max_voxel_index_arg)
66 {
67  uindex_t tree_depth;
68 
69  if (max_voxel_index_arg <= 0) {
70  PCL_ERROR("[pcl::octree::OctreeBase::setMaxVoxelIndex] Max voxel index (%lu) must "
71  "be > 0!\n",
72  max_voxel_index_arg);
73  return;
74  }
75 
76  // tree depth == bitlength of maxVoxels
77  tree_depth =
78  std::min(static_cast<uindex_t>(OctreeKey::maxDepth),
79  static_cast<uindex_t>(std::ceil(std::log2(max_voxel_index_arg))));
80 
81  setTreeDepth(tree_depth);
82 }
83 
84 //////////////////////////////////////////////////////////////////////////////////////////////
85 template <typename LeafContainerT, typename BranchContainerT>
86 void
88 {
89  if (depth_arg <= 0) {
90  PCL_ERROR("[pcl::octree::OctreeBase::setTreeDepth] Tree depth (%lu) must be > 0!\n",
91  depth_arg);
92  return;
93  }
94  if (depth_arg > OctreeKey::maxDepth) {
95  PCL_ERROR("[pcl::octree::OctreeBase::setTreeDepth] Tree depth (%lu) must be <= max "
96  "depth(%lu)!\n",
97  depth_arg,
99  return;
100  }
101 
102  // set octree depth
103  octree_depth_ = depth_arg;
104 
105  // define depth_mask_ by setting a single bit to 1 at bit position == tree depth
106  depth_mask_ = (1 << (depth_arg - 1));
107 
108  // define max_key_
109  max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
110 }
111 
112 //////////////////////////////////////////////////////////////////////////////////////////////
113 template <typename LeafContainerT, typename BranchContainerT>
114 LeafContainerT*
116  uindex_t idx_y_arg,
117  uindex_t idx_z_arg) const
118 {
119  // generate key
120  OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
121 
122  // find the leaf node addressed by key
123  return (findLeaf(key));
124 }
125 
126 //////////////////////////////////////////////////////////////////////////////////////////////
127 template <typename LeafContainerT, typename BranchContainerT>
128 LeafContainerT*
130  uindex_t idx_y_arg,
131  uindex_t idx_z_arg)
132 {
133  // generate key
134  OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
135 
136  // create a leaf node addressed by key
137  return (createLeaf(key));
138 }
139 
140 //////////////////////////////////////////////////////////////////////////////////////////////
141 template <typename LeafContainerT, typename BranchContainerT>
142 bool
144  uindex_t idx_y_arg,
145  uindex_t idx_z_arg) const
146 {
147  // generate key
148  OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
149 
150  // check if key exist in octree
151  return (existLeaf(key));
152 }
153 
154 //////////////////////////////////////////////////////////////////////////////////////////////
155 template <typename LeafContainerT, typename BranchContainerT>
156 void
158  uindex_t idx_y_arg,
159  uindex_t idx_z_arg)
160 {
161  // generate key
162  OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
163 
164  // delete the leaf node addressed by key
165  deleteLeafRecursive(key, depth_mask_, root_node_);
166 }
167 
168 //////////////////////////////////////////////////////////////////////////////////////////////
169 template <typename LeafContainerT, typename BranchContainerT>
170 void
172 {
173 
174  if (root_node_) {
175  // reset octree
176  deleteBranch(*root_node_);
177  leaf_count_ = 0;
178  branch_count_ = 1;
179  }
180 }
181 
182 //////////////////////////////////////////////////////////////////////////////////////////////
183 template <typename LeafContainerT, typename BranchContainerT>
184 void
186  std::vector<char>& binary_tree_out_arg) const
187 {
188 
189  OctreeKey new_key;
190 
191  // clear binary vector
192  binary_tree_out_arg.clear();
193  binary_tree_out_arg.reserve(this->branch_count_);
194 
195  serializeTreeRecursive(root_node_, new_key, &binary_tree_out_arg, nullptr);
196 }
197 
198 //////////////////////////////////////////////////////////////////////////////////////////////
199 template <typename LeafContainerT, typename BranchContainerT>
200 void
202  std::vector<char>& binary_tree_out_arg,
203  std::vector<LeafContainerT*>& leaf_container_vector_arg) const
204 {
205 
206  OctreeKey new_key;
207 
208  // clear output vectors
209  binary_tree_out_arg.clear();
210  leaf_container_vector_arg.clear();
211 
212  binary_tree_out_arg.reserve(this->branch_count_);
213  leaf_container_vector_arg.reserve(this->leaf_count_);
214 
215  serializeTreeRecursive(
216  root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg);
217 }
218 
219 //////////////////////////////////////////////////////////////////////////////////////////////
220 template <typename LeafContainerT, typename BranchContainerT>
221 void
223  std::vector<LeafContainerT*>& leaf_container_vector_arg)
224 {
225  OctreeKey new_key;
226 
227  // clear output vector
228  leaf_container_vector_arg.clear();
229 
230  leaf_container_vector_arg.reserve(this->leaf_count_);
231 
232  serializeTreeRecursive(root_node_, new_key, nullptr, &leaf_container_vector_arg);
233 }
234 
235 //////////////////////////////////////////////////////////////////////////////////////////////
236 template <typename LeafContainerT, typename BranchContainerT>
237 void
239  std::vector<char>& binary_tree_out_arg)
240 {
241  OctreeKey new_key;
242 
243  // free existing tree before tree rebuild
244  deleteTree();
245 
246  // iterator for binary tree structure vector
247  std::vector<char>::const_iterator binary_tree_out_it = binary_tree_out_arg.begin();
248  std::vector<char>::const_iterator binary_tree_out_it_end = binary_tree_out_arg.end();
249 
250  deserializeTreeRecursive(root_node_,
251  depth_mask_,
252  new_key,
253  binary_tree_out_it,
254  binary_tree_out_it_end,
255  nullptr,
256  nullptr);
257 }
258 
259 //////////////////////////////////////////////////////////////////////////////////////////////
260 template <typename LeafContainerT, typename BranchContainerT>
261 void
263  std::vector<char>& binary_tree_in_arg,
264  std::vector<LeafContainerT*>& leaf_container_vector_arg)
265 {
266  OctreeKey new_key;
267 
268  // set data iterator to first element
269  typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it =
270  leaf_container_vector_arg.begin();
271 
272  // set data iterator to last element
273  typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it_end =
274  leaf_container_vector_arg.end();
275 
276  // free existing tree before tree rebuild
277  deleteTree();
278 
279  // iterator for binary tree structure vector
280  std::vector<char>::const_iterator binary_tree_input_it = binary_tree_in_arg.begin();
281  std::vector<char>::const_iterator binary_tree_input_it_end = binary_tree_in_arg.end();
282 
283  deserializeTreeRecursive(root_node_,
284  depth_mask_,
285  new_key,
286  binary_tree_input_it,
287  binary_tree_input_it_end,
288  &leaf_vector_it,
289  &leaf_vector_it_end);
290 }
291 
292 //////////////////////////////////////////////////////////////////////////////////////////////
293 template <typename LeafContainerT, typename BranchContainerT>
294 uindex_t
296  const OctreeKey& key_arg,
297  uindex_t depth_mask_arg,
298  BranchNode* branch_arg,
299  LeafNode*& return_leaf_arg,
300  BranchNode*& parent_of_leaf_arg)
301 {
302  // index to branch child
303  unsigned char child_idx;
304 
305  // find branch child from key
306  child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
307 
308  OctreeNode* child_node = (*branch_arg)[child_idx];
309 
310  if (!child_node) {
311  if ((!dynamic_depth_enabled_) && (depth_mask_arg > 1)) {
312  // if required branch does not exist -> create it
313  BranchNode* childBranch = createBranchChild(*branch_arg, child_idx);
314 
315  branch_count_++;
316 
317  // recursively proceed with indexed child branch
318  return createLeafRecursive(key_arg,
319  depth_mask_arg >> 1,
320  childBranch,
321  return_leaf_arg,
322  parent_of_leaf_arg);
323  }
324  // if leaf node at child_idx does not exist
325  LeafNode* leaf_node = createLeafChild(*branch_arg, child_idx);
326  return_leaf_arg = leaf_node;
327  parent_of_leaf_arg = branch_arg;
328  this->leaf_count_++;
329  }
330  else {
331  // Node exists already
332  switch (child_node->getNodeType()) {
333  case BRANCH_NODE:
334  // recursively proceed with indexed child branch
335  return createLeafRecursive(key_arg,
336  depth_mask_arg >> 1,
337  static_cast<BranchNode*>(child_node),
338  return_leaf_arg,
339  parent_of_leaf_arg);
340  break;
341 
342  case LEAF_NODE:
343  return_leaf_arg = static_cast<LeafNode*>(child_node);
344  parent_of_leaf_arg = branch_arg;
345  break;
346  }
347  }
348 
349  return (depth_mask_arg >> 1);
350 }
351 
352 //////////////////////////////////////////////////////////////////////////////////////////////
353 template <typename LeafContainerT, typename BranchContainerT>
354 void
356  const OctreeKey& key_arg,
357  uindex_t depth_mask_arg,
358  BranchNode* branch_arg,
359  LeafContainerT*& result_arg) const
360 {
361  // index to branch child
362  unsigned char child_idx;
363 
364  // find branch child from key
365  child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
366 
367  OctreeNode* child_node = (*branch_arg)[child_idx];
368 
369  if (child_node) {
370  switch (child_node->getNodeType()) {
371  case BRANCH_NODE:
372  // we have not reached maximum tree depth
373  BranchNode* child_branch;
374  child_branch = static_cast<BranchNode*>(child_node);
375 
376  findLeafRecursive(key_arg, depth_mask_arg >> 1, child_branch, result_arg);
377  break;
378 
379  case LEAF_NODE:
380  // return existing leaf node
381  LeafNode* child_leaf;
382  child_leaf = static_cast<LeafNode*>(child_node);
383 
384  result_arg = child_leaf->getContainerPtr();
385  break;
386  }
387  }
388 }
389 
390 //////////////////////////////////////////////////////////////////////////////////////////////
391 template <typename LeafContainerT, typename BranchContainerT>
392 bool
394  const OctreeKey& key_arg, uindex_t depth_mask_arg, BranchNode* branch_arg)
395 {
396  // index to branch child
397  unsigned char child_idx;
398  // indicates if branch has children, if so, it can't be removed
399  bool b_has_children;
400 
401  // find branch child from key
402  child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
403 
404  OctreeNode* child_node = (*branch_arg)[child_idx];
405 
406  if (child_node) {
407  switch (child_node->getNodeType()) {
408 
409  case BRANCH_NODE:
410  BranchNode* child_branch;
411  child_branch = static_cast<BranchNode*>(child_node);
412 
413  // recursively explore the indexed child branch
414  b_has_children = deleteLeafRecursive(key_arg, depth_mask_arg >> 1, child_branch);
415 
416  if (!b_has_children) {
417  // child branch does not own any sub-child nodes anymore -> delete child branch
418  deleteBranchChild(*branch_arg, child_idx);
419  branch_count_--;
420  }
421  break;
422 
423  case LEAF_NODE:
424  // return existing leaf node
425 
426  // our child is a leaf node -> delete it
427  deleteBranchChild(*branch_arg, child_idx);
428  this->leaf_count_--;
429  break;
430  }
431  }
432 
433  // check if current branch still owns children
434  b_has_children = false;
435  for (child_idx = 0; (!b_has_children) && (child_idx < 8); child_idx++) {
436  b_has_children = branch_arg->hasChild(child_idx);
437  }
438  // return false if current branch can be deleted
439  return (b_has_children);
440 }
441 
442 //////////////////////////////////////////////////////////////////////////////////////////////
443 template <typename LeafContainerT, typename BranchContainerT>
444 void
446  const BranchNode* branch_arg,
447  OctreeKey& key_arg,
448  std::vector<char>* binary_tree_out_arg,
449  typename std::vector<LeafContainerT*>* leaf_container_vector_arg) const
450 {
451  char node_bit_pattern;
452 
453  // branch occupancy bit pattern
454  node_bit_pattern = getBranchBitPattern(*branch_arg);
455 
456  // write bit pattern to output vector
457  if (binary_tree_out_arg)
458  binary_tree_out_arg->push_back(node_bit_pattern);
459 
460  // iterate over all children
461  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
462 
463  // if child exist
464  if (branch_arg->hasChild(child_idx)) {
465  // add current branch voxel to key
466  key_arg.pushBranch(child_idx);
467 
468  OctreeNode* childNode = branch_arg->getChildPtr(child_idx);
469 
470  switch (childNode->getNodeType()) {
471  case BRANCH_NODE: {
472  // recursively proceed with indexed child branch
473  serializeTreeRecursive(static_cast<const BranchNode*>(childNode),
474  key_arg,
475  binary_tree_out_arg,
476  leaf_container_vector_arg);
477  break;
478  }
479  case LEAF_NODE: {
480  auto* child_leaf = static_cast<LeafNode*>(childNode);
481 
482  if (leaf_container_vector_arg)
483  leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());
484 
485  // we reached a leaf node -> execute serialization callback
486  serializeTreeCallback(**child_leaf, key_arg);
487  break;
488  }
489  default:
490  break;
491  }
492 
493  // pop current branch voxel from key
494  key_arg.popBranch();
495  }
496  }
497 }
498 
499 //////////////////////////////////////////////////////////////////////////////////////////////
500 template <typename LeafContainerT, typename BranchContainerT>
501 void
503  BranchNode* branch_arg,
504  uindex_t depth_mask_arg,
505  OctreeKey& key_arg,
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*
510  leaf_container_vector_it_end_arg)
511 {
512 
513  if (binary_tree_input_it_arg != binary_tree_input_it_end_arg) {
514  // read branch occupancy bit pattern from input vector
515  char node_bits = (*binary_tree_input_it_arg);
516  binary_tree_input_it_arg++;
517 
518  // iterate over all children
519  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
520  // if occupancy bit for child_idx is set..
521  if (node_bits & (1 << child_idx)) {
522  // add current branch voxel to key
523  key_arg.pushBranch(child_idx);
524 
525  if (depth_mask_arg > 1) {
526  // we have not reached maximum tree depth
527  BranchNode* newBranch = createBranchChild(*branch_arg, child_idx);
528 
529  branch_count_++;
530 
531  // recursively proceed with new child branch
532  deserializeTreeRecursive(newBranch,
533  depth_mask_arg >> 1,
534  key_arg,
535  binary_tree_input_it_arg,
536  binary_tree_input_it_end_arg,
537  leaf_container_vector_it_arg,
538  leaf_container_vector_it_end_arg);
539  }
540  else {
541  // we reached leaf node level
542 
543  LeafNode* child_leaf = createLeafChild(*branch_arg, child_idx);
544 
545  if (leaf_container_vector_it_arg &&
546  (*leaf_container_vector_it_arg != *leaf_container_vector_it_end_arg)) {
547  LeafContainerT& container = **child_leaf;
548  LeafContainerT* src_container_ptr = **leaf_container_vector_it_arg;
549  container = *src_container_ptr;
550  ++*leaf_container_vector_it_arg;
551  }
552 
553  leaf_count_++;
554 
555  // execute deserialization callback
556  deserializeTreeCallback(**child_leaf, key_arg);
557  }
558 
559  // pop current branch voxel from key
560  key_arg.popBranch();
561  }
562  }
563  }
564 }
565 
566 } // namespace octree
567 } // namespace pcl
568 
569 #define PCL_INSTANTIATE_OctreeBase(T) \
570  template class PCL_EXPORTS pcl::octree::OctreeBase<T>;
571 
572 #endif
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.
Definition: octree_base.hpp:87
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.
Definition: octree_base.hpp:54
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) const
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...
LeafContainerT * findLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg) const
Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
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.
Definition: octree_base.hpp:64
OctreeBase()
Empty constructor.
Definition: octree_base.hpp:48
Abstract octree branch class
Definition: octree_nodes.h:179
bool hasChild(unsigned char child_idx_arg) const
Check if branch is pointing to a particular child node.
Definition: octree_nodes.h:256
OctreeNode * getChildPtr(unsigned char child_idx_arg) const
Get pointer to child.
Definition: octree_nodes.h:234
Octree key class
Definition: octree_key.h:54
void popBranch()
pop child node from octree key
Definition: octree_key.h:122
static const unsigned char maxDepth
Definition: octree_key.h:142
void pushBranch(unsigned char childIndex)
push a child node to the octree key
Definition: octree_key.h:112
unsigned char getChildIdxWithDepthMask(uindex_t depthMask) const
get child node index using depthMask
Definition: octree_key.h:134
Abstract octree leaf class
Definition: octree_nodes.h:81
const ContainerT * getContainerPtr() const
Get const pointer to container.
Definition: octree_nodes.h:153
Abstract octree node class
Definition: octree_nodes.h:59
virtual node_type_t getNodeType() const =0
Pure virtual method for retrieving 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.
Definition: types.h:120