Point Cloud Library (PCL)  1.10.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 #include <pcl/impl/instantiate.hpp>
45 
46 namespace pcl {
47 namespace octree {
48 //////////////////////////////////////////////////////////////////////////////////////////////
49 template <typename LeafContainerT, typename BranchContainerT>
51 : leaf_count_(0)
52 , branch_count_(1)
53 , root_node_(new BranchNode())
54 , depth_mask_(0)
55 , octree_depth_(0)
56 , dynamic_depth_enabled_(false)
57 {}
58 
59 //////////////////////////////////////////////////////////////////////////////////////////////
60 template <typename LeafContainerT, typename BranchContainerT>
62 {
63  // deallocate tree structure
64  deleteTree();
65  delete (root_node_);
66 }
67 
68 //////////////////////////////////////////////////////////////////////////////////////////////
69 template <typename LeafContainerT, typename BranchContainerT>
70 void
72  unsigned int max_voxel_index_arg)
73 {
74  unsigned int tree_depth;
75 
76  assert(max_voxel_index_arg > 0);
77 
78  // tree depth == bitlength of maxVoxels
79  tree_depth =
80  std::min(static_cast<unsigned int>(OctreeKey::maxDepth),
81  static_cast<unsigned int>(std::ceil(std::log2(max_voxel_index_arg))));
82 
83  // define depthMask_ by setting a single bit to 1 at bit position == tree depth
84  depth_mask_ = (1 << (tree_depth - 1));
85 }
86 
87 //////////////////////////////////////////////////////////////////////////////////////////////
88 template <typename LeafContainerT, typename BranchContainerT>
89 void
91 {
92  assert(depth_arg > 0);
93 
94  // set octree depth
95  octree_depth_ = depth_arg;
96 
97  // define depthMask_ by setting a single bit to 1 at bit position == tree depth
98  depth_mask_ = (1 << (depth_arg - 1));
99 
100  // define max. keys
101  max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
102 }
103 
104 //////////////////////////////////////////////////////////////////////////////////////////////
105 template <typename LeafContainerT, typename BranchContainerT>
106 LeafContainerT*
108  unsigned int idx_y_arg,
109  unsigned int idx_z_arg)
110 {
111  // generate key
112  OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
113 
114  // check if key exist in octree
115  return (findLeaf(key));
116 }
117 
118 //////////////////////////////////////////////////////////////////////////////////////////////
119 template <typename LeafContainerT, typename BranchContainerT>
120 LeafContainerT*
122  unsigned int idx_y_arg,
123  unsigned int idx_z_arg)
124 {
125  // generate key
126  OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
127 
128  // check if key exist in octree
129  return (createLeaf(key));
130 }
131 
132 //////////////////////////////////////////////////////////////////////////////////////////////
133 template <typename LeafContainerT, typename BranchContainerT>
134 bool
136  unsigned int idx_y_arg,
137  unsigned int idx_z_arg) const
138 {
139  // generate key
140  OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
141 
142  // check if key exist in octree
143  return (existLeaf(key));
144 }
145 
146 //////////////////////////////////////////////////////////////////////////////////////////////
147 template <typename LeafContainerT, typename BranchContainerT>
148 void
150  unsigned int idx_y_arg,
151  unsigned int idx_z_arg)
152 {
153  // generate key
154  OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
155 
156  // check if key exist in octree
157  deleteLeafRecursive(key, depth_mask_, root_node_);
158 }
159 
160 //////////////////////////////////////////////////////////////////////////////////////////////
161 template <typename LeafContainerT, typename BranchContainerT>
162 void
164 {
165 
166  if (root_node_) {
167  // reset octree
168  deleteBranch(*root_node_);
169  leaf_count_ = 0;
170  branch_count_ = 1;
171  }
172 }
173 
174 //////////////////////////////////////////////////////////////////////////////////////////////
175 template <typename LeafContainerT, typename BranchContainerT>
176 void
178  std::vector<char>& binary_tree_out_arg)
179 {
180 
181  OctreeKey new_key;
182 
183  // clear binary vector
184  binary_tree_out_arg.clear();
185  binary_tree_out_arg.reserve(this->branch_count_);
186 
187  serializeTreeRecursive(root_node_, new_key, &binary_tree_out_arg, nullptr);
188 }
189 
190 //////////////////////////////////////////////////////////////////////////////////////////////
191 template <typename LeafContainerT, typename BranchContainerT>
192 void
194  std::vector<char>& binary_tree_out_arg,
195  std::vector<LeafContainerT*>& leaf_container_vector_arg)
196 {
197 
198  OctreeKey new_key;
199 
200  // clear output vectors
201  binary_tree_out_arg.clear();
202  leaf_container_vector_arg.clear();
203 
204  binary_tree_out_arg.reserve(this->branch_count_);
205  leaf_container_vector_arg.reserve(this->leaf_count_);
206 
207  serializeTreeRecursive(
208  root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg);
209 }
210 
211 //////////////////////////////////////////////////////////////////////////////////////////////
212 template <typename LeafContainerT, typename BranchContainerT>
213 void
215  std::vector<LeafContainerT*>& leaf_container_vector_arg)
216 {
217  OctreeKey new_key;
218 
219  // clear output vector
220  leaf_container_vector_arg.clear();
221 
222  leaf_container_vector_arg.reserve(this->leaf_count_);
223 
224  serializeTreeRecursive(root_node_, new_key, nullptr, &leaf_container_vector_arg);
225 }
226 
227 //////////////////////////////////////////////////////////////////////////////////////////////
228 template <typename LeafContainerT, typename BranchContainerT>
229 void
231  std::vector<char>& binary_tree_out_arg)
232 {
233  OctreeKey new_key;
234 
235  // free existing tree before tree rebuild
236  deleteTree();
237 
238  // iterator for binary tree structure vector
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();
241 
242  deserializeTreeRecursive(root_node_,
243  depth_mask_,
244  new_key,
245  binary_tree_out_it,
246  binary_tree_out_it_end,
247  nullptr,
248  nullptr);
249 }
250 
251 //////////////////////////////////////////////////////////////////////////////////////////////
252 template <typename LeafContainerT, typename BranchContainerT>
253 void
255  std::vector<char>& binary_tree_in_arg,
256  std::vector<LeafContainerT*>& leaf_container_vector_arg)
257 {
258  OctreeKey new_key;
259 
260  // set data iterator to first element
261  typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it =
262  leaf_container_vector_arg.begin();
263 
264  // set data iterator to last element
265  typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it_end =
266  leaf_container_vector_arg.end();
267 
268  // free existing tree before tree rebuild
269  deleteTree();
270 
271  // iterator for binary tree structure vector
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();
274 
275  deserializeTreeRecursive(root_node_,
276  depth_mask_,
277  new_key,
278  binary_tree_input_it,
279  binary_tree_input_it_end,
280  &leaf_vector_it,
281  &leaf_vector_it_end);
282 }
283 
284 //////////////////////////////////////////////////////////////////////////////////////////////
285 template <typename LeafContainerT, typename BranchContainerT>
286 unsigned int
288  const OctreeKey& key_arg,
289  unsigned int depth_mask_arg,
290  BranchNode* branch_arg,
291  LeafNode*& return_leaf_arg,
292  BranchNode*& parent_of_leaf_arg)
293 {
294  // index to branch child
295  unsigned char child_idx;
296 
297  // find branch child from key
298  child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
299 
300  OctreeNode* child_node = (*branch_arg)[child_idx];
301 
302  if (!child_node) {
303  if ((!dynamic_depth_enabled_) && (depth_mask_arg > 1)) {
304  // if required branch does not exist -> create it
305  BranchNode* childBranch = createBranchChild(*branch_arg, child_idx);
306 
307  branch_count_++;
308 
309  // recursively proceed with indexed child branch
310  return createLeafRecursive(key_arg,
311  depth_mask_arg / 2,
312  childBranch,
313  return_leaf_arg,
314  parent_of_leaf_arg);
315  }
316  // if leaf node at child_idx does not exist
317  LeafNode* leaf_node = createLeafChild(*branch_arg, child_idx);
318  return_leaf_arg = leaf_node;
319  parent_of_leaf_arg = branch_arg;
320  this->leaf_count_++;
321  }
322  else {
323  // Node exists already
324  switch (child_node->getNodeType()) {
325  case BRANCH_NODE:
326  // recursively proceed with indexed child branch
327  return createLeafRecursive(key_arg,
328  depth_mask_arg / 2,
329  static_cast<BranchNode*>(child_node),
330  return_leaf_arg,
331  parent_of_leaf_arg);
332  break;
333 
334  case LEAF_NODE:
335  return_leaf_arg = static_cast<LeafNode*>(child_node);
336  parent_of_leaf_arg = branch_arg;
337  break;
338  }
339  }
340 
341  return (depth_mask_arg >> 1);
342 }
343 
344 //////////////////////////////////////////////////////////////////////////////////////////////
345 template <typename LeafContainerT, typename BranchContainerT>
346 void
348  const OctreeKey& key_arg,
349  unsigned int depth_mask_arg,
350  BranchNode* branch_arg,
351  LeafContainerT*& result_arg) const
352 {
353  // index to branch child
354  unsigned char child_idx;
355 
356  // find branch child from key
357  child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
358 
359  OctreeNode* child_node = (*branch_arg)[child_idx];
360 
361  if (child_node) {
362  switch (child_node->getNodeType()) {
363  case BRANCH_NODE:
364  // we have not reached maximum tree depth
365  BranchNode* child_branch;
366  child_branch = static_cast<BranchNode*>(child_node);
367 
368  findLeafRecursive(key_arg, depth_mask_arg / 2, child_branch, result_arg);
369  break;
370 
371  case LEAF_NODE:
372  // return existing leaf node
373  LeafNode* child_leaf;
374  child_leaf = static_cast<LeafNode*>(child_node);
375 
376  result_arg = child_leaf->getContainerPtr();
377  break;
378  }
379  }
380 }
381 
382 //////////////////////////////////////////////////////////////////////////////////////////////
383 template <typename LeafContainerT, typename BranchContainerT>
384 bool
386  const OctreeKey& key_arg, unsigned int depth_mask_arg, BranchNode* branch_arg)
387 {
388  // index to branch child
389  unsigned char child_idx;
390  // indicates if branch is empty and can be safely removed
391  bool b_no_children;
392 
393  // find branch child from key
394  child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
395 
396  OctreeNode* child_node = (*branch_arg)[child_idx];
397 
398  if (child_node) {
399  switch (child_node->getNodeType()) {
400 
401  case BRANCH_NODE:
402  BranchNode* child_branch;
403  child_branch = static_cast<BranchNode*>(child_node);
404 
405  // recursively explore the indexed child branch
406  b_no_children = deleteLeafRecursive(key_arg, depth_mask_arg / 2, child_branch);
407 
408  if (!b_no_children) {
409  // child branch does not own any sub-child nodes anymore -> delete child branch
410  deleteBranchChild(*branch_arg, child_idx);
411  branch_count_--;
412  }
413  break;
414 
415  case LEAF_NODE:
416  // return existing leaf node
417 
418  // our child is a leaf node -> delete it
419  deleteBranchChild(*branch_arg, child_idx);
420  this->leaf_count_--;
421  break;
422  }
423  }
424 
425  // check if current branch still owns children
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);
429  }
430  // return true if current branch can be deleted
431  return (b_no_children);
432 }
433 
434 //////////////////////////////////////////////////////////////////////////////////////////////
435 template <typename LeafContainerT, typename BranchContainerT>
436 void
438  const BranchNode* branch_arg,
439  OctreeKey& key_arg,
440  std::vector<char>* binary_tree_out_arg,
441  typename std::vector<LeafContainerT*>* leaf_container_vector_arg) const
442 {
443  char node_bit_pattern;
444 
445  // branch occupancy bit pattern
446  node_bit_pattern = getBranchBitPattern(*branch_arg);
447 
448  // write bit pattern to output vector
449  if (binary_tree_out_arg)
450  binary_tree_out_arg->push_back(node_bit_pattern);
451 
452  // iterate over all children
453  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
454 
455  // if child exist
456  if (branch_arg->hasChild(child_idx)) {
457  // add current branch voxel to key
458  key_arg.pushBranch(child_idx);
459 
460  OctreeNode* childNode = branch_arg->getChildPtr(child_idx);
461 
462  switch (childNode->getNodeType()) {
463  case BRANCH_NODE: {
464  // recursively proceed with indexed child branch
465  serializeTreeRecursive(static_cast<const BranchNode*>(childNode),
466  key_arg,
467  binary_tree_out_arg,
468  leaf_container_vector_arg);
469  break;
470  }
471  case LEAF_NODE: {
472  LeafNode* child_leaf = static_cast<LeafNode*>(childNode);
473 
474  if (leaf_container_vector_arg)
475  leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());
476 
477  // we reached a leaf node -> execute serialization callback
478  serializeTreeCallback(**child_leaf, key_arg);
479  break;
480  }
481  default:
482  break;
483  }
484 
485  // pop current branch voxel from key
486  key_arg.popBranch();
487  }
488  }
489 }
490 
491 //////////////////////////////////////////////////////////////////////////////////////////////
492 template <typename LeafContainerT, typename BranchContainerT>
493 void
495  BranchNode* branch_arg,
496  unsigned int depth_mask_arg,
497  OctreeKey& key_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)
503 {
504 
505  if (binary_tree_input_it_arg != binary_tree_input_it_end_arg) {
506  // read branch occupancy bit pattern from input vector
507  char node_bits = (*binary_tree_input_it_arg);
508  binary_tree_input_it_arg++;
509 
510  // iterate over all children
511  for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
512  // if occupancy bit for child_idx is set..
513  if (node_bits & (1 << child_idx)) {
514  // add current branch voxel to key
515  key_arg.pushBranch(child_idx);
516 
517  if (depth_mask_arg > 1) {
518  // we have not reached maximum tree depth
519  BranchNode* newBranch = createBranchChild(*branch_arg, child_idx);
520 
521  branch_count_++;
522 
523  // recursively proceed with new child branch
524  deserializeTreeRecursive(newBranch,
525  depth_mask_arg / 2,
526  key_arg,
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);
531  }
532  else {
533  // we reached leaf node level
534 
535  LeafNode* child_leaf = createLeafChild(*branch_arg, child_idx);
536 
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;
542  }
543 
544  leaf_count_++;
545 
546  // execute deserialization callback
547  deserializeTreeCallback(**child_leaf, key_arg);
548  }
549 
550  // pop current branch voxel from key
551  key_arg.popBranch();
552  }
553  }
554  }
555 }
556 
557 } // namespace octree
558 } // namespace pcl
559 
560 #define PCL_INSTANTIATE_OctreeBase(T) \
561  template class PCL_EXPORTS pcl::octree::OctreeBase<T>;
562 
563 #endif
bool hasChild(unsigned char child_idx_arg) const
Check if branch is pointing to a particular child node.
Definition: octree_nodes.h:246
Octree class.
Definition: octree_base.h:61
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
OctreeNode * getChildPtr(unsigned char child_idx_arg) const
Get pointer to child.
Definition: octree_nodes.h:225
OctreeBase()
Empty constructor.
Definition: octree_base.hpp:50
void popBranch()
pop child node from octree key
Definition: octree_key.h:122
unsigned char getChildIdxWithDepthMask(unsigned int depthMask) const
get child node index using depthMask
Definition: octree_key.h:134
Octree key class
Definition: octree_key.h:52
Abstract octree leaf class
Definition: octree_nodes.h:85
const ContainerT * getContainerPtr() const
Get const pointer to container.
Definition: octree_nodes.h:142
Abstract octree branch class
Definition: octree_nodes.h:168
void pushBranch(unsigned char childIndex)
push a child node to the octree key
Definition: octree_key.h:112
Abstract octree node class
Definition: octree_nodes.h:63