Point Cloud Library (PCL)  1.10.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 #include <pcl/impl/instantiate.hpp>
45 
46 namespace pcl
47 {
48  namespace octree
49  {
50  //////////////////////////////////////////////////////////////////////////////////////////////
51  template<typename LeafContainerT, typename BranchContainerT>
53  leaf_count_ (0),
54  branch_count_ (1),
55  root_node_ (new BranchNode ()),
56  depth_mask_ (0),
57  octree_depth_ (0),
58  dynamic_depth_enabled_ (false)
59  {
60  }
61 
62  //////////////////////////////////////////////////////////////////////////////////////////////
63  template<typename LeafContainerT, typename BranchContainerT>
65  {
66  // deallocate tree structure
67  deleteTree ();
68  delete (root_node_);
69  }
70 
71  //////////////////////////////////////////////////////////////////////////////////////////////
72  template<typename LeafContainerT, typename BranchContainerT>
73  void
75  {
76  unsigned int tree_depth;
77 
78  assert(max_voxel_index_arg>0);
79 
80  // tree depth == bitlength of maxVoxels
81  tree_depth = std::min (static_cast<unsigned int> (OctreeKey::maxDepth), 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  {
168  // reset octree
169  deleteBranch (*root_node_);
170  leaf_count_ = 0;
171  branch_count_ = 1;
172  }
173 
174  }
175 
176  //////////////////////////////////////////////////////////////////////////////////////////////
177  template<typename LeafContainerT, typename BranchContainerT>
178  void
179  OctreeBase<LeafContainerT, BranchContainerT>::serializeTree (std::vector<char>& binary_tree_out_arg)
180  {
181 
182  OctreeKey new_key;
183 
184  // clear binary vector
185  binary_tree_out_arg.clear ();
186  binary_tree_out_arg.reserve (this->branch_count_);
187 
188  serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, nullptr);
189  }
190 
191  //////////////////////////////////////////////////////////////////////////////////////////////
192  template<typename LeafContainerT, typename BranchContainerT>
193  void
194  OctreeBase<LeafContainerT, BranchContainerT>::serializeTree (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 (root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg);
208  }
209 
210  //////////////////////////////////////////////////////////////////////////////////////////////
211  template<typename LeafContainerT, typename BranchContainerT>
212  void
213  OctreeBase<LeafContainerT, BranchContainerT>::serializeLeafs (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
228  OctreeBase<LeafContainerT, BranchContainerT>::deserializeTree (std::vector<char>& binary_tree_out_arg)
229  {
230  OctreeKey new_key;
231 
232  // free existing tree before tree rebuild
233  deleteTree ();
234 
235  //iterator for binary tree structure vector
236  std::vector<char>::const_iterator binary_tree_out_it = binary_tree_out_arg.begin ();
237  std::vector<char>::const_iterator binary_tree_out_it_end = binary_tree_out_arg.end ();
238 
239  deserializeTreeRecursive (root_node_,
240  depth_mask_,
241  new_key,
242  binary_tree_out_it,
243  binary_tree_out_it_end,
244  nullptr,
245  nullptr);
246 
247  }
248 
249  //////////////////////////////////////////////////////////////////////////////////////////////
250  template<typename LeafContainerT, typename BranchContainerT>
251  void
253  std::vector<LeafContainerT*>& leaf_container_vector_arg)
254  {
255  OctreeKey new_key;
256 
257  // set data iterator to first element
258  typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it = leaf_container_vector_arg.begin ();
259 
260  // set data iterator to last element
261  typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it_end = leaf_container_vector_arg.end ();
262 
263  // free existing tree before tree rebuild
264  deleteTree ();
265 
266  //iterator for binary tree structure vector
267  std::vector<char>::const_iterator binary_tree_input_it = binary_tree_in_arg.begin ();
268  std::vector<char>::const_iterator binary_tree_input_it_end = binary_tree_in_arg.end ();
269 
270  deserializeTreeRecursive (root_node_,
271  depth_mask_,
272  new_key,
273  binary_tree_input_it,
274  binary_tree_input_it_end,
275  &leaf_vector_it,
276  &leaf_vector_it_end);
277 
278  }
279 
280  //////////////////////////////////////////////////////////////////////////////////////////////
281  template<typename LeafContainerT, typename BranchContainerT>
282  unsigned int
284  unsigned int depth_mask_arg,
285  BranchNode* branch_arg,
286  LeafNode*& return_leaf_arg,
287  BranchNode*& parent_of_leaf_arg)
288  {
289  // index to branch child
290  unsigned char child_idx;
291 
292  // find branch child from key
293  child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg);
294 
295  OctreeNode* child_node = (*branch_arg)[child_idx];
296 
297  if (!child_node)
298  {
299  if ((!dynamic_depth_enabled_) && (depth_mask_arg > 1))
300  {
301  // if required branch does not exist -> create it
302  BranchNode* childBranch = createBranchChild (*branch_arg, child_idx);
303 
304  branch_count_++;
305 
306  // recursively proceed with indexed child branch
307  return createLeafRecursive (key_arg, depth_mask_arg / 2, childBranch, return_leaf_arg, parent_of_leaf_arg);
308 
309  }
310  // if leaf node at child_idx does not exist
311  LeafNode* leaf_node = createLeafChild (*branch_arg, child_idx);
312  return_leaf_arg = leaf_node;
313  parent_of_leaf_arg = branch_arg;
314  this->leaf_count_++;
315  }
316  else
317  {
318  // Node exists already
319  switch (child_node->getNodeType ())
320  {
321  case BRANCH_NODE:
322  // recursively proceed with indexed child branch
323  return createLeafRecursive (key_arg, depth_mask_arg / 2, static_cast<BranchNode*> (child_node),
324  return_leaf_arg, parent_of_leaf_arg);
325  break;
326 
327  case LEAF_NODE:
328  return_leaf_arg = static_cast<LeafNode*> (child_node);;
329  parent_of_leaf_arg = branch_arg;
330  break;
331  }
332 
333  }
334 
335  return (depth_mask_arg >> 1);
336  }
337 
338  //////////////////////////////////////////////////////////////////////////////////////////////
339  template<typename LeafContainerT, typename BranchContainerT>
340  void
342  unsigned int depth_mask_arg,
343  BranchNode* branch_arg,
344  LeafContainerT*& result_arg) const
345  {
346  // index to branch child
347  unsigned char child_idx;
348 
349  // find branch child from key
350  child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg);
351 
352  OctreeNode* child_node = (*branch_arg)[child_idx];
353 
354  if (child_node)
355  {
356  switch (child_node->getNodeType ())
357  {
358  case BRANCH_NODE:
359  // we have not reached maximum tree depth
360  BranchNode* child_branch;
361  child_branch = static_cast<BranchNode*> (child_node);
362 
363  findLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, result_arg);
364  break;
365 
366  case LEAF_NODE:
367  // return existing leaf node
368  LeafNode* child_leaf;
369  child_leaf = static_cast<LeafNode*> (child_node);
370 
371  result_arg = child_leaf->getContainerPtr ();
372  break;
373  }
374  }
375  }
376 
377  //////////////////////////////////////////////////////////////////////////////////////////////
378  template<typename LeafContainerT, typename BranchContainerT>
379  bool
381  unsigned int depth_mask_arg,
382  BranchNode* branch_arg)
383  {
384  // index to branch child
385  unsigned char child_idx;
386  // indicates if branch is empty and can be safely removed
387  bool b_no_children;
388 
389  // find branch child from key
390  child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg);
391 
392  OctreeNode* child_node = (*branch_arg)[child_idx];
393 
394  if (child_node)
395  {
396  switch (child_node->getNodeType ())
397  {
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  {
408  // child branch does not own any sub-child nodes anymore -> delete child branch
409  deleteBranchChild (*branch_arg, child_idx);
410  branch_count_--;
411  }
412  break;
413 
414  case LEAF_NODE:
415  // return existing leaf node
416 
417  // our child is a leaf node -> delete it
418  deleteBranchChild (*branch_arg, child_idx);
419  this->leaf_count_--;
420  break;
421  }
422  }
423 
424  // check if current branch still owns children
425  b_no_children = false;
426  for (child_idx = 0; (!b_no_children) && (child_idx < 8); child_idx++)
427  {
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  OctreeKey& key_arg,
439  std::vector<char>* binary_tree_out_arg,
440  typename std::vector<LeafContainerT*>* leaf_container_vector_arg) const
441  {
442  char node_bit_pattern;
443 
444  // branch occupancy bit pattern
445  node_bit_pattern = getBranchBitPattern (*branch_arg);
446 
447  // write bit pattern to output vector
448  if (binary_tree_out_arg)
449  binary_tree_out_arg->push_back (node_bit_pattern);
450 
451  // iterate over all children
452  for (unsigned char child_idx = 0; child_idx < 8; child_idx++)
453  {
454 
455  // if child exist
456  if (branch_arg->hasChild (child_idx))
457  {
458  // add current branch voxel to key
459  key_arg.pushBranch (child_idx);
460 
461  OctreeNode *childNode = branch_arg->getChildPtr (child_idx);
462 
463  switch (childNode->getNodeType ())
464  {
465  case BRANCH_NODE:
466  {
467  // recursively proceed with indexed child branch
468  serializeTreeRecursive (static_cast<const BranchNode*> (childNode), key_arg, binary_tree_out_arg,
469  leaf_container_vector_arg);
470  break;
471  }
472  case LEAF_NODE:
473  {
474  LeafNode* child_leaf = static_cast<LeafNode*> (childNode);
475 
476  if (leaf_container_vector_arg)
477  leaf_container_vector_arg->push_back (child_leaf->getContainerPtr ());
478 
479  // we reached a leaf node -> execute serialization callback
480  serializeTreeCallback (**child_leaf, key_arg);
481  break;
482  }
483  default:
484  break;
485  }
486 
487  // pop current branch voxel from key
488  key_arg.popBranch ();
489  }
490  }
491  }
492 
493  //////////////////////////////////////////////////////////////////////////////////////////////
494  template<typename LeafContainerT, typename BranchContainerT>
495  void
497  typename std::vector<char>::const_iterator& binary_tree_input_it_arg,
498  typename std::vector<char>::const_iterator& binary_tree_input_it_end_arg,
499  typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_arg,
500  typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_end_arg)
501  {
502  char node_bits;
503 
504  if (binary_tree_input_it_arg != binary_tree_input_it_end_arg)
505  {
506  // read branch occupancy bit pattern from input vector
507  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  {
513  // if occupancy bit for child_idx is set..
514  if (node_bits & (1 << child_idx))
515  {
516  // add current branch voxel to key
517  key_arg.pushBranch (child_idx);
518 
519  if (depth_mask_arg > 1)
520  {
521  // we have not reached maximum tree depth
522  BranchNode * newBranch = createBranchChild (*branch_arg, child_idx);
523 
524  branch_count_++;
525 
526  // recursively proceed with new child branch
527  deserializeTreeRecursive (newBranch, depth_mask_arg / 2, key_arg,
528  binary_tree_input_it_arg, binary_tree_input_it_end_arg,
529  leaf_container_vector_it_arg, leaf_container_vector_it_end_arg);
530  }
531  else
532  {
533  // we reached leaf node level
534 
535  LeafNode* child_leaf = createLeafChild (*branch_arg, child_idx);
536 
537  if (leaf_container_vector_it_arg && (*leaf_container_vector_it_arg != *leaf_container_vector_it_end_arg))
538  {
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  }
558 }
559 
560 #define PCL_INSTANTIATE_OctreeBase(T) template class PCL_EXPORTS pcl::octree::OctreeBase<T>;
561 
562 #endif
563 
bool hasChild(unsigned char child_idx_arg) const
Check if branch is pointing to a particular child node.
Definition: octree_nodes.h:287
Octree class.
Definition: octree_base.h:61
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
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:267
OctreeBase()
Empty constructor.
Definition: octree_base.hpp:52
void popBranch()
pop child node from octree key
Definition: octree_key.h:124
unsigned char getChildIdxWithDepthMask(unsigned int depthMask) const
get child node index using depthMask
Definition: octree_key.h:136
Octree key class
Definition: octree_key.h:50
Abstract octree leaf class
Definition: octree_nodes.h:96
const ContainerT * getContainerPtr() const
Get const pointer to container.
Definition: octree_nodes.h:177
Abstract octree branch class
Definition: octree_nodes.h:203
void pushBranch(unsigned char childIndex)
push a child node to the octree key
Definition: octree_key.h:114
Abstract octree node class
Definition: octree_nodes.h:67