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