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