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