Point Cloud Library (PCL)  1.8.0
octree2buf_base.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, 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_2BUF_BASE_HPP
40 #define PCL_OCTREE_2BUF_BASE_HPP
41 
42 namespace pcl
43 {
44  namespace octree
45  {
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  max_key_ (),
54  buffer_selector_ (0),
55  tree_dirty_flag_ (false),
56  octree_depth_ (0),
57  dynamic_depth_enabled_(false)
58  {
59  }
60 
61  //////////////////////////////////////////////////////////////////////////////////////////////
62  template<typename LeafContainerT, typename BranchContainerT>
64  {
65  // deallocate tree structure
66  deleteTree ();
67  delete (root_node_);
68  }
69 
70  //////////////////////////////////////////////////////////////////////////////////////////////
71  template<typename LeafContainerT, typename BranchContainerT> void
73  {
74  unsigned int treeDepth;
75 
76  assert (max_voxel_index_arg > 0);
77 
78  // tree depth == amount of bits of maxVoxels
79  treeDepth = std::max ((std::min (static_cast<unsigned int> (OctreeKey::maxDepth),
80  static_cast<unsigned int> (std::ceil (Log2 (max_voxel_index_arg))))),
81  static_cast<unsigned int> (0));
82 
83  // define depthMask_ by setting a single bit to 1 at bit position == tree depth
84  depth_mask_ = (1 << (treeDepth - 1));
85  }
86 
87  //////////////////////////////////////////////////////////////////////////////////////////////
88  template<typename LeafContainerT, typename BranchContainerT> void
90  {
91  assert (depth_arg > 0);
92 
93  // set octree depth
94  octree_depth_ = depth_arg;
95 
96  // define depthMask_ by setting a single bit to 1 at bit position == tree depth
97  depth_mask_ = (1 << (depth_arg - 1));
98 
99  // define max. keys
100  max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
101  }
102 
103  //////////////////////////////////////////////////////////////////////////////////////////////
104  template<typename LeafContainerT, typename BranchContainerT> LeafContainerT*
105  Octree2BufBase<LeafContainerT, BranchContainerT>::findLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg)
106  {
107  // generate key
108  OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
109 
110  // check if key exist in octree
111  return ( findLeaf (key));
112  }
113 
114  //////////////////////////////////////////////////////////////////////////////////////////////
115  template<typename LeafContainerT, typename BranchContainerT> LeafContainerT*
116  Octree2BufBase<LeafContainerT, BranchContainerT>::createLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg)
117  {
118  // generate key
119  OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
120 
121  // check if key exist in octree
122  return ( createLeaf (key));
123  }
124 
125  //////////////////////////////////////////////////////////////////////////////////////////////
126  template<typename LeafContainerT, typename BranchContainerT> bool
127  Octree2BufBase<LeafContainerT, BranchContainerT>::existLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const
128  {
129  // generate key
130  OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
131 
132  // check if key exist in octree
133  return existLeaf (key);
134  }
135 
136  //////////////////////////////////////////////////////////////////////////////////////////////
137  template<typename LeafContainerT, typename BranchContainerT> void
138  Octree2BufBase<LeafContainerT, BranchContainerT>::removeLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg)
139  {
140  // generate key
141  OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
142 
143  // free voxel at key
144  return (this->removeLeaf (key));
145  }
146 
147  //////////////////////////////////////////////////////////////////////////////////////////////
148  template<typename LeafContainerT, typename BranchContainerT> void
150  {
151  if (root_node_)
152  {
153  // reset octree
155  leaf_count_ = 0;
156  branch_count_ = 1;
157 
158  tree_dirty_flag_ = false;
159  depth_mask_ = 0;
160  octree_depth_ = 0;
161  }
162  }
163 
164  //////////////////////////////////////////////////////////////////////////////////////////////
165  template<typename LeafContainerT, typename BranchContainerT> void
167  {
168  if (tree_dirty_flag_)
169  {
170  // make sure that all unused branch nodes from previous buffer are deleted
172  }
173 
174  // switch butter selector
176 
177  // reset flags
178  tree_dirty_flag_ = true;
179  leaf_count_ = 0;
180  branch_count_ = 1;
181 
182  unsigned char child_idx;
183  // we can safely remove children references of root node
184  for (child_idx = 0; child_idx < 8; child_idx++)
185  {
186  root_node_->setChildPtr(buffer_selector_, child_idx, 0);
187  }
188  }
189 
190  //////////////////////////////////////////////////////////////////////////////////////////////
191  template<typename LeafContainerT, typename BranchContainerT> void
193  bool do_XOR_encoding_arg)
194  {
195  OctreeKey new_key;
196 
197  // clear binary vector
198  binary_tree_out_arg.clear ();
199  binary_tree_out_arg.reserve (this->branch_count_);
200 
201  serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, 0, do_XOR_encoding_arg, false);
202 
203  // serializeTreeRecursive cleans-up unused octree nodes in previous octree
204  tree_dirty_flag_ = false;
205  }
206 
207  //////////////////////////////////////////////////////////////////////////////////////////////
208  template<typename LeafContainerT, typename BranchContainerT> void
210  std::vector<LeafContainerT*>& leaf_container_vector_arg,
211  bool do_XOR_encoding_arg)
212  {
213  OctreeKey new_key;
214 
215  // clear output vectors
216  binary_tree_out_arg.clear ();
217  leaf_container_vector_arg.clear ();
218 
219  leaf_container_vector_arg.reserve (leaf_count_);
220  binary_tree_out_arg.reserve (this->branch_count_);
221 
222  serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg, do_XOR_encoding_arg, false);
223 
224  // serializeTreeRecursive cleans-up unused octree nodes in previous octree
225  tree_dirty_flag_ = false;
226  }
227 
228  //////////////////////////////////////////////////////////////////////////////////////////////
229  template<typename LeafContainerT, typename BranchContainerT> void
230  Octree2BufBase<LeafContainerT, BranchContainerT>::serializeLeafs (std::vector<LeafContainerT*>& leaf_container_vector_arg)
231  {
232  OctreeKey new_key;
233 
234  // clear output vector
235  leaf_container_vector_arg.clear ();
236 
237  leaf_container_vector_arg.reserve (leaf_count_);
238 
239  serializeTreeRecursive (root_node_, new_key, 0, &leaf_container_vector_arg, false, false);
240 
241  // serializeLeafsRecursive cleans-up unused octree nodes in previous octree
242  tree_dirty_flag_ = false;
243  }
244 
245  //////////////////////////////////////////////////////////////////////////////////////////////
246  template<typename LeafContainerT, typename BranchContainerT> void
248  bool do_XOR_decoding_arg)
249  {
250  OctreeKey new_key;
251 
252  // we will rebuild an octree -> reset leafCount
253  leaf_count_ = 0;
254 
255  // iterator for binary tree structure vector
256  std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin ();
257  std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end ();
258 
260  binary_tree_in_it, binary_tree_in_it_end, 0, 0, false,
261  do_XOR_decoding_arg);
262 
263  // we modified the octree structure -> clean-up/tree-reset might be required
264  tree_dirty_flag_ = false;
265  }
266 
267  //////////////////////////////////////////////////////////////////////////////////////////////
268  template<typename LeafContainerT, typename BranchContainerT> void
270  std::vector<LeafContainerT*>& leaf_container_vector_arg,
271  bool do_XOR_decoding_arg)
272  {
273  OctreeKey new_key;
274 
275  // set data iterator to first element
276  typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it = leaf_container_vector_arg.begin ();
277 
278  // set data iterator to last element
279  typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it_end = leaf_container_vector_arg.end ();
280 
281  // we will rebuild an octree -> reset leafCount
282  leaf_count_ = 0;
283 
284  // iterator for binary tree structure vector
285  std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin ();
286  std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end ();
287 
289  depth_mask_,
290  new_key,
291  binary_tree_in_it,
292  binary_tree_in_it_end,
293  &leaf_container_vector_it,
294  &leaf_container_vector_it_end,
295  false,
296  do_XOR_decoding_arg);
297 
298 
299  // we modified the octree structure -> clean-up/tree-reset might be required
300  tree_dirty_flag_ = false;
301  }
302 
303 
304  //////////////////////////////////////////////////////////////////////////////////////////////
305  template<typename LeafContainerT, typename BranchContainerT> void
306  Octree2BufBase<LeafContainerT, BranchContainerT>::serializeNewLeafs (std::vector<LeafContainerT*>& leaf_container_vector_arg)
307  {
308  OctreeKey new_key;
309 
310  // clear output vector
311  leaf_container_vector_arg.clear ();
312  leaf_container_vector_arg.reserve (leaf_count_);
313 
314  serializeTreeRecursive (root_node_, new_key, 0, &leaf_container_vector_arg, false, true);
315 
316  // serializeLeafsRecursive cleans-up unused octree nodes in previous octree buffer
317  tree_dirty_flag_ = false;
318  }
319 
320  //////////////////////////////////////////////////////////////////////////////////////////////
321  template<typename LeafContainerT, typename BranchContainerT>
322  unsigned int
324  unsigned int depth_mask_arg,
325  BranchNode* branch_arg,
326  LeafNode*& return_leaf_arg,
327  BranchNode*& parent_of_leaf_arg,
328  bool branch_reset_arg)
329  {
330  // index to branch child
331  unsigned char child_idx;
332 
333  // branch reset -> this branch has been taken from previous buffer
334  if (branch_reset_arg)
335  {
336  // we can safely remove children references
337  for (child_idx = 0; child_idx < 8; child_idx++)
338  {
339  branch_arg->setChildPtr(buffer_selector_, child_idx, 0);
340  }
341  }
342 
343  // find branch child from key
344  child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg);
345 
346  if (depth_mask_arg > 1)
347  {
348  // we have not reached maximum tree depth
349  BranchNode* child_branch;
350  bool doNodeReset;
351 
352  doNodeReset = false;
353 
354  // if required branch does not exist
355  if (!branch_arg->hasChild(buffer_selector_, child_idx))
356  {
357  // check if we find a branch node reference in previous buffer
358 
359  if (branch_arg->hasChild(!buffer_selector_, child_idx))
360  {
361  OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_,child_idx);
362 
363  if (child_node->getNodeType()==BRANCH_NODE) {
364  child_branch = static_cast<BranchNode*> (child_node);
365  branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
366  } else {
367  // depth has changed.. child in preceeding buffer is a leaf node.
368  deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
369  child_branch = createBranchChild (*branch_arg, child_idx);
370  }
371 
372  // take child branch from previous buffer
373  doNodeReset = true; // reset the branch pointer array of stolen child node
374 
375  }
376  else
377  {
378  // if required branch does not exist -> create it
379  child_branch = createBranchChild (*branch_arg, child_idx);
380  }
381 
382  branch_count_++;
383  }
384  // required branch node already exists - use it
385  else
386  child_branch = static_cast<BranchNode*> (branch_arg->getChildPtr(buffer_selector_,child_idx));
387 
388  // recursively proceed with indexed child branch
389  return createLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, return_leaf_arg, parent_of_leaf_arg, doNodeReset);
390  }
391  else
392  {
393  // branch childs are leaf nodes
394  LeafNode* child_leaf;
395  if (!branch_arg->hasChild(buffer_selector_, child_idx))
396  {
397  // leaf node at child_idx does not exist
398 
399  // check if we can take copy a reference from previous buffer
400  if (branch_arg->hasChild(!buffer_selector_, child_idx))
401  {
402 
403  OctreeNode * child_node = branch_arg->getChildPtr(!buffer_selector_,child_idx);
404  if (child_node->getNodeType () == LEAF_NODE)
405  {
406  child_leaf = static_cast<LeafNode*> (child_node);
407  branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
408  } else {
409  // depth has changed.. child in preceeding buffer is a leaf node.
410  deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
411  child_leaf = createLeafChild (*branch_arg, child_idx);
412  }
413  leaf_count_++;
414  }
415  else
416  {
417  // if required leaf does not exist -> create it
418  child_leaf = createLeafChild (*branch_arg, child_idx);
419  leaf_count_++;
420  }
421 
422  // return leaf node
423  return_leaf_arg = child_leaf;
424  parent_of_leaf_arg = branch_arg;
425  }
426  else
427  {
428  // leaf node already exist
429  return_leaf_arg = static_cast<LeafNode*> (branch_arg->getChildPtr(buffer_selector_,child_idx));;
430  parent_of_leaf_arg = branch_arg;
431  }
432  }
433 
434  return depth_mask_arg;
435  }
436 
437  //////////////////////////////////////////////////////////////////////////////////////////////
438  template<typename LeafContainerT, typename BranchContainerT> void
440  unsigned int depth_mask_arg,
441  BranchNode* branch_arg,
442  LeafContainerT*& result_arg) const
443  {
444  // return leaf node
445  unsigned char child_idx;
446 
447  // find branch child from key
448  child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg);
449 
450  if (depth_mask_arg > 1)
451  {
452  // we have not reached maximum tree depth
453  BranchNode* child_branch;
454  child_branch = static_cast<BranchNode*> (branch_arg->getChildPtr(buffer_selector_,child_idx));
455 
456  if (child_branch)
457  // recursively proceed with indexed child branch
458  findLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, result_arg);
459  }
460  else
461  {
462  // we reached leaf node level
463  if (branch_arg->hasChild(buffer_selector_, child_idx))
464  {
465  // return existing leaf node
466  LeafNode* leaf_node = static_cast<LeafNode*> (branch_arg->getChildPtr(buffer_selector_,child_idx));
467  result_arg = leaf_node->getContainerPtr();;
468  }
469  }
470  }
471 
472  //////////////////////////////////////////////////////////////////////////////////////////////
473  template<typename LeafContainerT, typename BranchContainerT> bool
475  unsigned int depth_mask_arg,
476  BranchNode* branch_arg)
477  {
478  // index to branch child
479  unsigned char child_idx;
480  // indicates if branch is empty and can be safely removed
481  bool bNoChilds;
482 
483  // find branch child from key
484  child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg);
485 
486  if (depth_mask_arg > 1)
487  {
488  // we have not reached maximum tree depth
489  BranchNode* child_branch;
490  bool bBranchOccupied;
491 
492  // next branch child on our path through the tree
493  child_branch = static_cast<BranchNode*> (branch_arg->getChildPtr(buffer_selector_,child_idx));
494 
495  if (child_branch)
496  {
497  // recursively explore the indexed child branch
498  bBranchOccupied = deleteLeafRecursive (key_arg, depth_mask_arg / 2, child_branch);
499 
500  if (!bBranchOccupied)
501  {
502  // child branch does not own any sub-child nodes anymore -> delete child branch
503  deleteBranchChild (*branch_arg, buffer_selector_, child_idx);
504  branch_count_--;
505  }
506  }
507  }
508  else
509  {
510  // our child is a leaf node -> delete it
511  deleteBranchChild (*branch_arg, buffer_selector_, child_idx);
512  leaf_count_--;
513  }
514 
515  // check if current branch still owns childs
516  bNoChilds = false;
517  for (child_idx = 0; child_idx < 8; child_idx++)
518  {
519  bNoChilds = branch_arg->hasChild(buffer_selector_, child_idx);
520  if (bNoChilds)
521  break;
522  }
523 
524  // return true if current branch can be deleted
525  return (bNoChilds);
526  }
527 
528  //////////////////////////////////////////////////////////////////////////////////////////////
529  template<typename LeafContainerT, typename BranchContainerT> void Octree2BufBase<
530  LeafContainerT, BranchContainerT>::serializeTreeRecursive (BranchNode* branch_arg,
531  OctreeKey& key_arg,
532  std::vector<char>* binary_tree_out_arg,
533  typename std::vector<LeafContainerT*>* leaf_container_vector_arg,
534  bool do_XOR_encoding_arg,
535  bool new_leafs_filter_arg)
536  {
537  // child iterator
538  unsigned char child_idx;
539 
540  // bit pattern
541  char branch_bit_pattern_curr_buffer;
542  char branch_bit_pattern_prev_buffer;
543  char node_XOR_bit_pattern;
544 
545  // occupancy bit patterns of branch node (current and previous octree buffer)
546  branch_bit_pattern_curr_buffer = getBranchBitPattern (*branch_arg, buffer_selector_);
547  branch_bit_pattern_prev_buffer = getBranchBitPattern (*branch_arg, !buffer_selector_);
548 
549  // XOR of current and previous occupancy bit patterns
550  node_XOR_bit_pattern = branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer;
551 
552  if (binary_tree_out_arg)
553  {
554  if (do_XOR_encoding_arg)
555  {
556  // write XOR bit pattern to output vector
557  binary_tree_out_arg->push_back (node_XOR_bit_pattern);
558  }
559  else
560  {
561  // write bit pattern of current buffer to output vector
562  binary_tree_out_arg->push_back (branch_bit_pattern_curr_buffer);
563  }
564  }
565 
566  // iterate over all children
567  for (child_idx = 0; child_idx < 8; child_idx++)
568  {
569  if (branch_arg->hasChild(buffer_selector_, child_idx))
570  {
571  // add current branch voxel to key
572  key_arg.pushBranch(child_idx);
573 
574  OctreeNode *child_node = branch_arg->getChildPtr(buffer_selector_,child_idx);
575 
576  switch (child_node->getNodeType ())
577  {
578  case BRANCH_NODE:
579  {
580  // recursively proceed with indexed child branch
581  serializeTreeRecursive (static_cast<BranchNode*> (child_node), key_arg, binary_tree_out_arg,
582  leaf_container_vector_arg, do_XOR_encoding_arg, new_leafs_filter_arg);
583  break;
584  }
585  case LEAF_NODE:
586  {
587  LeafNode* child_leaf = static_cast<LeafNode*> (child_node);
588 
589  if (new_leafs_filter_arg)
590  {
591  if (!branch_arg->hasChild (!buffer_selector_, child_idx))
592  {
593  if (leaf_container_vector_arg)
594  leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());
595 
596  serializeTreeCallback (**child_leaf, key_arg);
597  }
598  } else
599  {
600 
601  if (leaf_container_vector_arg)
602  leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());
603 
604  serializeTreeCallback (**child_leaf, key_arg);
605  }
606 
607  break;
608  }
609  default:
610  break;
611  }
612 
613  // pop current branch voxel from key
614  key_arg.popBranch();
615  }
616  else if (branch_arg->hasChild (!buffer_selector_, child_idx))
617  {
618  // delete branch, free memory
619  deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
620 
621  }
622 
623  }
624  }
625 
626 
627  //////////////////////////////////////////////////////////////////////////////////////////////
628  template<typename LeafContainerT, typename BranchContainerT> void
630  unsigned int depth_mask_arg, OctreeKey& key_arg,
631  typename std::vector<char>::const_iterator& binaryTreeIT_arg,
632  typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
633  typename std::vector<LeafContainerT*>::const_iterator* dataVectorIterator_arg,
634  typename std::vector<LeafContainerT*>::const_iterator* dataVectorEndIterator_arg,
635  bool branch_reset_arg, bool do_XOR_decoding_arg)
636  {
637  // child iterator
638  unsigned char child_idx;
639 
640  // node bits
641  char nodeBits;
642  char recoveredNodeBits;
643 
644  // branch reset -> this branch has been taken from previous buffer
645  if (branch_reset_arg)
646  {
647  // we can safely remove children references
648  for (child_idx = 0; child_idx < 8; child_idx++)
649  {
650  branch_arg->setChildPtr(buffer_selector_, child_idx, 0);
651  }
652  }
653 
654  if (binaryTreeIT_arg!=binaryTreeIT_End_arg) {
655  // read branch occupancy bit pattern from vector
656  nodeBits = *binaryTreeIT_arg++;
657 
658 
659  // recover branch occupancy bit pattern
660  if (do_XOR_decoding_arg)
661  {
662  recoveredNodeBits = getBranchBitPattern (*branch_arg, !buffer_selector_) ^ nodeBits;
663  }
664  else
665  {
666  recoveredNodeBits = nodeBits;
667  }
668 
669  // iterate over all children
670  for (child_idx = 0; child_idx < 8; child_idx++)
671  {
672  // if occupancy bit for child_idx is set..
673  if (recoveredNodeBits & (1 << child_idx))
674  {
675  // add current branch voxel to key
676  key_arg.pushBranch(child_idx);
677 
678  bool doNodeReset;
679 
680  doNodeReset = false;
681 
682  if (depth_mask_arg > 1)
683  {
684  // we have not reached maximum tree depth
685 
686  BranchNode* child_branch;
687 
688  // check if we find a branch node reference in previous buffer
689  if (!branch_arg->hasChild(buffer_selector_, child_idx))
690  {
691 
692  if (branch_arg->hasChild(!buffer_selector_, child_idx))
693  {
694  OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_,child_idx);
695 
696  if (child_node->getNodeType()==BRANCH_NODE) {
697  child_branch = static_cast<BranchNode*> (child_node);
698  branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
699  } else {
700  // depth has changed.. child in preceeding buffer is a leaf node.
701  deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
702  child_branch = createBranchChild (*branch_arg, child_idx);
703  }
704 
705  // take child branch from previous buffer
706  doNodeReset = true; // reset the branch pointer array of stolen child node
707  }
708  else
709  {
710  // if required branch does not exist -> create it
711  child_branch = createBranchChild (*branch_arg, child_idx);
712  }
713 
714  branch_count_++;
715 
716  }
717  else
718  {
719  // required branch node already exists - use it
720  child_branch = static_cast<BranchNode*> (branch_arg->getChildPtr(buffer_selector_,child_idx));
721  }
722 
723  // recursively proceed with indexed child branch
724  deserializeTreeRecursive (child_branch, depth_mask_arg / 2, key_arg,
725  binaryTreeIT_arg, binaryTreeIT_End_arg,
726  dataVectorIterator_arg, dataVectorEndIterator_arg,
727  doNodeReset, do_XOR_decoding_arg);
728 
729  }
730  else
731  {
732  // branch childs are leaf nodes
733  LeafNode* child_leaf;
734 
735  // check if we can take copy a reference pointer from previous buffer
736  if (branch_arg->hasChild(!buffer_selector_, child_idx))
737  {
738  // take child leaf node from previous buffer
739  OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_,child_idx);
740  if (child_node->getNodeType()==LEAF_NODE) {
741  child_leaf = static_cast<LeafNode*> (child_node);
742  branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
743  } else {
744  // depth has changed.. child in preceeding buffer is a leaf node.
745  deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
746  child_leaf = createLeafChild (*branch_arg, child_idx);
747  }
748  }
749  else
750  {
751  // if required leaf does not exist -> create it
752  child_leaf = createLeafChild (*branch_arg, child_idx);
753  }
754 
755  // we reached leaf node level
756 
757  if (dataVectorIterator_arg
758  && (*dataVectorIterator_arg != *dataVectorEndIterator_arg))
759  {
760  LeafContainerT& container = **child_leaf;
761  container = ***dataVectorIterator_arg;
762  ++*dataVectorIterator_arg;
763  }
764 
765  leaf_count_++;
766 
767  // execute deserialization callback
768  deserializeTreeCallback (**child_leaf, key_arg);
769 
770  }
771 
772  // pop current branch voxel from key
773  key_arg.popBranch();
774  }
775  else if (branch_arg->hasChild (!buffer_selector_, child_idx))
776  {
777  // remove old branch pointer information in current branch
778  branch_arg->setChildPtr(buffer_selector_, child_idx, 0);
779 
780  // remove unused branches in previous buffer
781  deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
782  }
783  }
784  }
785 
786  }
787 
788  //////////////////////////////////////////////////////////////////////////////////////////////
789  template<typename LeafContainerT, typename BranchContainerT> void
791  {
792  // child iterator
793  unsigned char child_idx;
794 
795  // bit pattern
796  char occupied_children_bit_pattern_prev_buffer;
797  char node_XOR_bit_pattern;
798  char unused_branches_bit_pattern;
799 
800  // occupancy bit pattern of branch node (previous octree buffer)
801  occupied_children_bit_pattern_prev_buffer = getBranchBitPattern (*branch_arg, !buffer_selector_);
802 
803  // XOR of current and previous occupancy bit patterns
804  node_XOR_bit_pattern = getBranchXORBitPattern (*branch_arg);
805 
806  // bit pattern indicating unused octree nodes in previous branch
807  unused_branches_bit_pattern = node_XOR_bit_pattern & occupied_children_bit_pattern_prev_buffer;
808 
809  // iterate over all children
810  for (child_idx = 0; child_idx < 8; child_idx++)
811  {
812  if (branch_arg->hasChild(buffer_selector_, child_idx))
813  {
814  OctreeNode *child_node = branch_arg->getChildPtr(buffer_selector_,child_idx);
815 
816  switch (child_node->getNodeType ())
817  {
818  case BRANCH_NODE:
819  {
820  // recursively proceed with indexed child branch
821  treeCleanUpRecursive (static_cast<BranchNode*> (child_node));
822  break;
823  }
824  case LEAF_NODE:
825  // leaf level - nothing to do..
826  break;
827  default:
828  break;
829  }
830  }
831 
832  // check for unused branches in previous buffer
833  if (unused_branches_bit_pattern & (1 << child_idx))
834  {
835  // delete branch, free memory
836  deleteBranchChild (*branch_arg, !buffer_selector_, child_idx);
837  }
838  }
839  }
840  }
841 }
842 
843 #define PCL_INSTANTIATE_Octree2BufBase(T) template class PCL_EXPORTS pcl::octree::Octree2BufBase<T>;
844 
845 #endif
846 
double Log2(double n_arg)
Helper function to calculate the binary logarithm.
Octree2BufBase()
Empty constructor.
OctreeNode * getChildPtr(unsigned char buffer_arg, unsigned char index_arg) const
Get child pointer in current branch node.
void serializeLeafs(std::vector< LeafContainerT *> &leaf_container_vector_arg)
Outputs a vector of all DataT elements that are stored within the octree leaf nodes.
unsigned int createLeafRecursive(const OctreeKey &key_arg, unsigned int depth_mask_arg, BranchNode *branch_arg, LeafNode *&return_leaf_arg, BranchNode *&parent_of_leaf_arg, bool branch_reset_arg=false)
Create a leaf node at octree key.
void setTreeDepth(unsigned int depth_arg)
Set the maximum depth of the octree.
void treeCleanUpRecursive(BranchNode *branch_arg)
Recursively explore the octree and remove unused branch and leaf nodes.
static const unsigned char maxDepth
Definition: octree_key.h:136
unsigned int octree_depth_
Octree depth.
char getBranchBitPattern(const BranchNode &branch_arg) const
Generate bit pattern reflecting the existence of child node pointers for current buffer.
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
LeafNode * createLeafChild(BranchNode &branch_arg, unsigned char child_idx_arg)
Fetch and add a new leaf child to a branch class.
OctreeKey max_key_
key range
void deleteBranch(BranchNode &branch_arg)
Delete branch and all its subchilds from octree (both buffers)
void serializeTreeRecursive(BranchNode *branch_arg, OctreeKey &key_arg, std::vector< char > *binary_tree_out_arg, typename std::vector< LeafContainerT *> *leaf_container_vector_arg, bool do_XOR_encoding_arg=false, bool new_leafs_filter_arg=false)
Recursively explore the octree and output binary octree description together with a vector of leaf no...
void deserializeTree(std::vector< char > &binary_tree_in_arg, bool do_XOR_decoding_arg=false)
Deserialize a binary octree description vector and create a corresponding octree structure.
void deserializeTreeRecursive(BranchNode *branch_arg, unsigned int depth_mask_arg, OctreeKey &key_arg, typename std::vector< char >::const_iterator &binary_tree_in_it_arg, typename std::vector< char >::const_iterator &binary_tree_in_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, bool branch_reset_arg=false, bool do_XOR_decoding_arg=false)
Rebuild an octree based on binary XOR octree description and DataT objects for leaf node initializati...
void deleteTree()
Delete the octree structure and its leaf nodes.
void popBranch()
pop child node from octree key
Definition: octree_key.h:116
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.
void switchBuffers()
Switch buffers and reset current octree structure.
void serializeTree(std::vector< char > &binary_tree_out_arg, bool do_XOR_encoding_arg=false)
Serialize octree into a binary output vector describing its branch node structure.
virtual ~Octree2BufBase()
Empty deconstructor.
void deleteBranchChild(BranchNode &branch_arg, unsigned char buffer_selector_arg, unsigned char child_idx_arg)
Delete child node and all its subchilds from octree in specific buffer.
void setMaxVoxelIndex(unsigned int max_voxel_index_arg)
Set the maximum amount of voxels per dimension.
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).
virtual void deserializeTreeCallback(LeafContainerT &, const OctreeKey &)
Callback executed for every leaf node data during deserialization.
void serializeNewLeafs(std::vector< LeafContainerT *> &leaf_container_vector_arg)
Outputs a vector of all DataT elements from leaf nodes, that do not exist in the previous octree buff...
unsigned char getChildIdxWithDepthMask(unsigned int depthMask) const
get child node index using depthMask
Definition: octree_key.h:128
std::size_t branch_count_
Amount of branch nodes.
bool existLeaf(unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const
Check for the existence of leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
char getBranchXORBitPattern(const BranchNode &branch_arg) const
Generate XOR bit pattern reflecting differences between the two octree buffers.
Octree key class
Definition: octree_key.h:53
Abstract octree leaf class
Definition: octree_nodes.h:100
unsigned char buffer_selector_
Currently active octree buffer.
const ContainerT * getContainerPtr() const
Get const pointer to container.
Definition: octree_nodes.h:181
virtual void serializeTreeCallback(LeafContainerT &, const OctreeKey &)
Callback executed for every leaf node data during serialization.
Octree double buffer class
BranchNode * createBranchChild(BranchNode &branch_arg, unsigned char child_idx_arg)
Fetch and add a new branch child to a branch class in current buffer.
BranchNode * root_node_
Pointer to root branch node of octree.
unsigned int depth_mask_
Depth mask based on octree depth.
void setChildPtr(unsigned char buffer_arg, unsigned char index_arg, OctreeNode *newNode_arg)
Set child pointer in current branch node.
void pushBranch(unsigned char childIndex)
push a child node to the octree key
Definition: octree_key.h:106
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).
Abstract octree node class
Definition: octree_nodes.h:71
std::size_t leaf_count_
Amount of leaf nodes.
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 deleteLeafRecursive(const OctreeKey &key_arg, unsigned int depth_mask_arg, BranchNode *branch_arg)
Recursively search and delete leaf node.
bool hasChild(unsigned char buffer_arg, unsigned char index_arg) const
Check if branch is pointing to a particular child node.