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