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