Point Cloud Library (PCL)  1.7.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
154  deleteBranch (*root_node_);
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
171  treeCleanUpRecursive (root_node_);
172  }
173 
174  // switch butter selector
175  buffer_selector_ = !buffer_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 
259  deserializeTreeRecursive (root_node_, depth_mask_, new_key,
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 
288  deserializeTreeRecursive (root_node_,
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