Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
octree2buf_base.hpp
Go to the documentation of this file.
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: octree2buf_base.hpp 6119 2012-07-03 18:50:04Z aichim $
37  */
38 
39 #ifndef OCTREE_2BUF_BASE_HPP
40 #define OCTREE_2BUF_BASE_HPP
41 
42 namespace pcl
43 {
44  namespace octree
45  {
47  template<typename DataT, typename LeafT, typename BranchT>
48  Octree2BufBase<DataT, LeafT, BranchT>::Octree2BufBase () :
49  leafCount_ (0),
50  branchCount_ (1),
51  objectCount_ (0),
52  rootNode_ (new BranchNode ()),
53  depthMask_ (0),
54  maxKey_ (),
55  branchNodePool_ (),
56  leafNodePool_ (),
57  bufferSelector_ (0),
58  treeDirtyFlag_ (false),
59  octreeDepth_ (0)
60  {
61  }
62 
64  template<typename DataT, typename LeafT, typename BranchT>
65  Octree2BufBase<DataT, LeafT, BranchT>::~Octree2BufBase ()
66  {
67  // deallocate tree structure
68  deleteTree ();
69  delete (rootNode_);
70  poolCleanUp ();
71  }
72 
74  template<typename DataT, typename LeafT, typename BranchT> void
75  Octree2BufBase<DataT, LeafT, BranchT>::setMaxVoxelIndex (unsigned int maxVoxelIndex_arg)
76  {
77  unsigned int treeDepth;
78 
79  assert (maxVoxelIndex_arg > 0);
80 
81  // tree depth == amount of bits of maxVoxels
82  treeDepth = std::max ((std::min (static_cast<unsigned int> (OCT_MAXTREEDEPTH),
83  static_cast<unsigned int> (std::ceil (Log2 (maxVoxelIndex_arg))))),
84  static_cast<unsigned int> (0));
85 
86  // define depthMask_ by setting a single bit to 1 at bit position == tree depth
87  depthMask_ = (1 << (treeDepth - 1));
88  }
89 
91  template<typename DataT, typename LeafT, typename BranchT> void
92  Octree2BufBase<DataT, LeafT, BranchT>::setTreeDepth (unsigned int depth_arg)
93  {
94  assert (depth_arg > 0);
95 
96  // set octree depth
97  octreeDepth_ = depth_arg;
98 
99  // define depthMask_ by setting a single bit to 1 at bit position == tree depth
100  depthMask_ = (1 << (depth_arg - 1));
101 
102  // define max. keys
103  maxKey_.x = maxKey_.y = maxKey_.z = (1 << depth_arg) - 1;
104  }
105 
107  template<typename DataT, typename LeafT, typename BranchT> void
108  Octree2BufBase<DataT, LeafT, BranchT>::addData (unsigned int idxX_arg, unsigned int idxY_arg,
109  unsigned int idxZ_arg, const DataT& data_arg)
110  {
111  // generate key
112  OctreeKey key (idxX_arg, idxY_arg, idxZ_arg);
113 
114  // add data_arg to octree
115  addData (key, data_arg);
116  }
117 
119  template<typename DataT, typename LeafT, typename BranchT> bool
120  Octree2BufBase<DataT, LeafT, BranchT>::getData (unsigned int idxX_arg, unsigned int idxY_arg,
121  unsigned int idxZ_arg, DataT& data_arg) const
122  {
123  // generate key
124  OctreeKey key (idxX_arg, idxY_arg, idxZ_arg);
125 
126  // search for leaf at key
127  LeafT* leaf = findLeaf (key);
128  if (leaf)
129  {
130  // if successful, decode data to data_arg
131  leaf->getData (data_arg);
132  }
133  // returns true on success
134  return (leaf != 0);
135  }
136 
138  template<typename DataT, typename LeafT, typename BranchT> bool
139  Octree2BufBase<DataT, LeafT, BranchT>::existLeaf (unsigned int idxX_arg, unsigned int idxY_arg,
140  unsigned int idxZ_arg) const
141  {
142  // generate key
143  OctreeKey key (idxX_arg, idxY_arg, idxZ_arg);
144 
145  // check if key exist in octree
146  return existLeaf (key);
147  }
148 
150  template<typename DataT, typename LeafT, typename BranchT> void
151  Octree2BufBase<DataT, LeafT, BranchT>::removeLeaf (unsigned int idxX_arg, unsigned int idxY_arg,
152  unsigned int idxZ_arg)
153  {
154  // generate key
155  OctreeKey key (idxX_arg, idxY_arg, idxZ_arg);
156 
157  // free voxel at key
158  return (this->removeLeaf (key));
159  }
160 
162  template<typename DataT, typename LeafT, typename BranchT> void
163  Octree2BufBase<DataT, LeafT, BranchT>::deleteTree ( bool freeMemory_arg )
164  {
165  if (rootNode_)
166  {
167  // reset octree
168  deleteBranch (*rootNode_);
169  leafCount_ = 0;
170  branchCount_ = 1;
171  objectCount_ = 0;
172 
173  treeDirtyFlag_ = false;
174  depthMask_ = 0;
175  octreeDepth_ = 0;
176  }
177 
178  // delete node pool
179  if (freeMemory_arg)
180  poolCleanUp ();
181  }
182 
184  template<typename DataT, typename LeafT, typename BranchT> void
185  Octree2BufBase<DataT, LeafT, BranchT>::switchBuffers ()
186  {
187  if (treeDirtyFlag_)
188  {
189  // make sure that all unused branch nodes from previous buffer are deleted
190  treeCleanUpRecursive (rootNode_);
191  }
192 
193  // switch butter selector
194  bufferSelector_ = !bufferSelector_;
195 
196  // reset flags
197  treeDirtyFlag_ = true;
198  leafCount_ = 0;
199  objectCount_ = 0;
200  branchCount_ = 1;
201 
202  unsigned char childIdx;
203  // we can safely remove children references of root node
204  for (childIdx = 0; childIdx < 8; childIdx++)
205  {
206  rootNode_->setChildPtr(bufferSelector_, childIdx, 0);
207  }
208  }
209 
211  template<typename DataT, typename LeafT, typename BranchT> void
212  Octree2BufBase<DataT, LeafT, BranchT>::serializeTree (std::vector<char>& binaryTreeOut_arg, bool doXOREncoding_arg)
213  {
214  OctreeKey newKey;
215 
216  // clear binary vector
217  binaryTreeOut_arg.clear ();
218  binaryTreeOut_arg.reserve (this->branchCount_);
219 
220  serializeTreeRecursive (rootNode_, newKey, &binaryTreeOut_arg, 0,
221  doXOREncoding_arg, false, 0);
222 
223  // serializeTreeRecursive cleans-up unused octree nodes in previous octree
224  treeDirtyFlag_ = false;
225  }
226 
228  template<typename DataT, typename LeafT, typename BranchT> void
229  Octree2BufBase<DataT, LeafT, BranchT>::serializeTree (std::vector<char>& binaryTreeOut_arg,
230  std::vector<DataT>& dataVector_arg, bool doXOREncoding_arg)
231  {
232  OctreeKey newKey;
233 
234  // clear output vectors
235  binaryTreeOut_arg.clear ();
236  dataVector_arg.clear ();
237 
238  dataVector_arg.reserve (objectCount_);
239  binaryTreeOut_arg.reserve (this->branchCount_);
240 
241  serializeTreeRecursive (rootNode_, newKey, &binaryTreeOut_arg, &dataVector_arg, doXOREncoding_arg, false, 0);
242 
243  // serializeTreeRecursive cleans-up unused octree nodes in previous octree
244  treeDirtyFlag_ = false;
245  }
246 
248  template<typename DataT, typename LeafT, typename BranchT> void
249  Octree2BufBase<DataT, LeafT, BranchT>::serializeLeafs (std::vector<DataT>& dataVector_arg)
250  {
251  OctreeKey newKey;
252 
253  // clear output vector
254  dataVector_arg.clear ();
255 
256  dataVector_arg.reserve (objectCount_);
257 
258  serializeTreeRecursive (rootNode_, newKey, 0, &dataVector_arg, false, false, 0);
259 
260  // serializeLeafsRecursive cleans-up unused octree nodes in previous octree
261  treeDirtyFlag_ = false;
262  }
263 
265  template<typename DataT, typename LeafT, typename BranchT> void
266  Octree2BufBase<DataT, LeafT, BranchT>::deserializeTree (std::vector<char>& binaryTreeIn_arg, bool doXORDecoding_arg)
267  {
268  OctreeKey newKey;
269 
270  // we will rebuild an octree -> reset leafCount
271  leafCount_ = 0;
272 
273  // iterator for binary tree structure vector
274  std::vector<char>::const_iterator binaryTreeVectorIterator =
275  binaryTreeIn_arg.begin ();
276  std::vector<char>::const_iterator binaryTreeVectorIteratorEnd =
277  binaryTreeIn_arg.end ();
278 
279  deserializeTreeRecursive (rootNode_, depthMask_, newKey,
280  binaryTreeVectorIterator, binaryTreeVectorIteratorEnd, 0, 0, false,
281  doXORDecoding_arg);
282 
283  // we modified the octree structure -> clean-up/tree-reset might be required
284  treeDirtyFlag_ = false;
285  }
286 
288  template<typename DataT, typename LeafT, typename BranchT> void
289  Octree2BufBase<DataT, LeafT, BranchT>::deserializeTree (std::vector<char>& binaryTreeIn_arg,
290  std::vector<DataT>& dataVector_arg, bool doXORDecoding_arg)
291  {
292  OctreeKey newKey;
293 
294  // set data iterator to first element
295  typename std::vector<DataT>::const_iterator dataVectorIterator = dataVector_arg.begin ();
296 
297  // set data iterator to last element
298  typename std::vector<DataT>::const_iterator dataVectorEndIterator = dataVector_arg.end ();
299 
300  // we will rebuild an octree -> reset leafCount
301  leafCount_ = 0;
302 
303  // iterator for binary tree structure vector
304  std::vector<char>::const_iterator binaryTreeVectorIterator = binaryTreeIn_arg.begin ();
305  std::vector<char>::const_iterator binaryTreeVectorIteratorEnd = binaryTreeIn_arg.end ();
306 
307  deserializeTreeRecursive (rootNode_, depthMask_, newKey, binaryTreeVectorIterator, binaryTreeVectorIteratorEnd, &dataVectorIterator,
308  &dataVectorEndIterator, false, doXORDecoding_arg);
309 
310 
311  // we modified the octree structure -> clean-up/tree-reset might be required
312  treeDirtyFlag_ = false;
313  }
314 
315 
317  template<typename DataT, typename LeafT, typename BranchT> void
318  Octree2BufBase<DataT, LeafT, BranchT>::serializeNewLeafs (std::vector<DataT>& dataVector_arg,
319  const int minPointsPerLeaf_arg)
320  {
321  OctreeKey newKey;
322 
323  // clear output vector
324  dataVector_arg.clear ();
325 
326  dataVector_arg.reserve (leafCount_);
327 
328  serializeTreeRecursive (rootNode_, newKey, 0, &dataVector_arg, false, true, minPointsPerLeaf_arg);
329 
330  // serializeLeafsRecursive cleans-up unused octree nodes in previous octree
331  treeDirtyFlag_ = false;
332  }
333 
335  template<typename DataT, typename LeafT, typename BranchT> LeafT*
336  Octree2BufBase<DataT, LeafT, BranchT>::createLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg,
337  BranchNode* branch_arg, bool branchReset_arg)
338  {
339  // index to branch child
340  unsigned char childIdx;
341  LeafT* result = 0;
342 
343  // branch reset -> this branch has been taken from previous buffer
344  if (branchReset_arg)
345  {
346  // we can safely remove children references
347  for (childIdx = 0; childIdx < 8; childIdx++)
348  {
349  branch_arg->setChildPtr(bufferSelector_, childIdx, 0);
350  }
351  }
352 
353  // find branch child from key
354  childIdx = key_arg.getChildIdxWithDepthMask (depthMask_arg);
355 
356  if (depthMask_arg > 1)
357  {
358  // we have not reached maximum tree depth
359  BranchNode* childBranch;
360  bool doNodeReset;
361 
362  doNodeReset = false;
363 
364  // if required branch does not exist
365  if (!branch_arg->hasChild(bufferSelector_, childIdx))
366  {
367  // check if we find a branch node reference in previous buffer
368 
369  if (branch_arg->hasChild(!bufferSelector_, childIdx))
370  {
371  OctreeNode* childNode = branch_arg->getChildPtr(!bufferSelector_,childIdx);
372 
373  if (childNode->getNodeType()==BRANCH_NODE) {
374  childBranch = static_cast<BranchNode*> (childNode);
375  branch_arg->setChildPtr(bufferSelector_, childIdx, childNode);
376  } else {
377  // depth has changed.. child in preceeding buffer is a leaf node.
378  deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
379  createBranchChild (*branch_arg, childIdx, childBranch);
380  }
381 
382  // take child branch from previous buffer
383  doNodeReset = true; // reset the branch pointer array of stolen child node
384 
385  }
386  else
387  {
388  // if required branch does not exist -> create it
389  createBranchChild (*branch_arg, childIdx, childBranch);
390  }
391 
392  branchCount_++;
393  }
394  // required branch node already exists - use it
395  else
396  childBranch = static_cast<BranchNode*> (branch_arg->getChildPtr(bufferSelector_,childIdx));
397 
398  // recursively proceed with indexed child branch
399  result = createLeafRecursive (key_arg, depthMask_arg / 2, childBranch, doNodeReset);
400  }
401  else
402  {
403  // branch childs are leaf nodes
404  LeafNode* childLeaf;
405  if (!branch_arg->hasChild(bufferSelector_, childIdx))
406  {
407  // leaf node at childIdx does not exist
408 
409  // check if we can take copy a reference from previous buffer
410  if (branch_arg->hasChild(!bufferSelector_, childIdx))
411  {
412 
413  OctreeNode * childNode = branch_arg->getChildPtr(!bufferSelector_,childIdx);
414  if (childNode->getNodeType () == LEAF_NODE)
415  {
416  childLeaf = static_cast<LeafNode*> (childNode);
417  branch_arg->setChildPtr(bufferSelector_, childIdx, childNode);
418  childLeaf->reset ();
419  } else {
420  // depth has changed.. child in preceeding buffer is a leaf node.
421  deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
422  createLeafChild (*branch_arg, childIdx, childLeaf);
423  }
424  leafCount_++;
425  }
426  else
427  {
428  // if required leaf does not exist -> create it
429  createLeafChild (*branch_arg, childIdx, childLeaf);
430  leafCount_++;
431  }
432 
433  // return leaf node
434  result = childLeaf;
435  }
436  else
437  {
438  // leaf node already exist
439  childLeaf = static_cast<LeafNode*> (branch_arg->getChildPtr(bufferSelector_,childIdx));
440 
441  // return leaf node
442  result = childLeaf;
443  }
444  }
445 
446  return (result);
447  }
448 
450  template<typename DataT, typename LeafT, typename BranchT> LeafT*
451  Octree2BufBase<DataT, LeafT, BranchT>::findLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg,
452  BranchNode* branch_arg) const
453  {
454  // return leaf node
455  unsigned char childIdx;
456  LeafT* result = 0;
457 
458  // find branch child from key
459  childIdx = key_arg.getChildIdxWithDepthMask (depthMask_arg);
460 
461  if (depthMask_arg > 1)
462  {
463  // we have not reached maximum tree depth
464  BranchNode* childBranch;
465  childBranch = static_cast<BranchNode*> (branch_arg->getChildPtr(bufferSelector_,childIdx));
466 
467  if (childBranch)
468  // recursively proceed with indexed child branch
469  result = findLeafRecursive (key_arg, depthMask_arg / 2, childBranch);
470  }
471  else
472  {
473  // we reached leaf node level
474  if (branch_arg->hasChild(bufferSelector_, childIdx))
475  {
476  // return existing leaf node
477  LeafNode* childLeaf = static_cast<LeafNode*> (branch_arg->getChildPtr(bufferSelector_,childIdx));
478  result = childLeaf;
479  }
480  }
481  return (result);
482  }
483 
485  template<typename DataT, typename LeafT, typename BranchT> bool
486  Octree2BufBase<DataT, LeafT, BranchT>::deleteLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg,
487  BranchNode* branch_arg)
488  {
489  // index to branch child
490  unsigned char childIdx;
491  // indicates if branch is empty and can be safely removed
492  bool bNoChilds;
493 
494  // find branch child from key
495  childIdx = key_arg.getChildIdxWithDepthMask (depthMask_arg);
496 
497  if (depthMask_arg > 1)
498  {
499  // we have not reached maximum tree depth
500  BranchNode* childBranch;
501  bool bBranchOccupied;
502 
503  // next branch child on our path through the tree
504  childBranch = static_cast<BranchNode*> (branch_arg->getChildPtr(bufferSelector_,childIdx));
505 
506  if (childBranch)
507  {
508  // recursively explore the indexed child branch
509  bBranchOccupied = deleteLeafRecursive (key_arg, depthMask_arg / 2, childBranch);
510 
511  if (!bBranchOccupied)
512  {
513  // child branch does not own any sub-child nodes anymore -> delete child branch
514  deleteBranchChild (*branch_arg, bufferSelector_, childIdx);
515  branchCount_--;
516  }
517  }
518  }
519  else
520  {
521  // our child is a leaf node -> delete it
522  deleteBranchChild (*branch_arg, bufferSelector_, childIdx);
523  leafCount_--;
524  }
525 
526  // check if current branch still owns childs
527  bNoChilds = false;
528  for (childIdx = 0; childIdx < 8; childIdx++)
529  {
530  bNoChilds = branch_arg->hasChild(bufferSelector_, childIdx);
531  if (bNoChilds)
532  break;
533  }
534 
535  // return true if current branch can be deleted
536  return (bNoChilds);
537  }
538 
540  template<typename DataT, typename LeafT, typename BranchT> void Octree2BufBase<
541  DataT, LeafT, BranchT>::serializeTreeRecursive (BranchNode* branch_arg,
542  OctreeKey& key_arg, std::vector<char>* binaryTreeOut_arg,
543  typename std::vector<DataT>* dataVector_arg, bool doXOREncoding_arg,
544  bool newLeafsFilter_arg, std::size_t minPointsPerLeaf_arg)
545  {
546  // child iterator
547  unsigned char childIdx;
548 
549  // bit pattern
550  char branchBitPatternCurrBuffer;
551  char branchBitPatternPrevBuffer;
552  char nodeXORBitPattern;
553 
554  // occupancy bit patterns of branch node (current and previous octree buffer)
555  branchBitPatternCurrBuffer = getBranchBitPattern (*branch_arg, bufferSelector_);
556  branchBitPatternPrevBuffer = getBranchBitPattern (*branch_arg, !bufferSelector_);
557 
558  // XOR of current and previous occupancy bit patterns
559  nodeXORBitPattern = branchBitPatternCurrBuffer ^ branchBitPatternPrevBuffer;
560 
561  if (binaryTreeOut_arg)
562  {
563  if (doXOREncoding_arg)
564  {
565  // write XOR bit pattern to output vector
566  binaryTreeOut_arg->push_back (nodeXORBitPattern);
567  }
568  else
569  {
570  // write bit pattern of current buffer to output vector
571  binaryTreeOut_arg->push_back (branchBitPatternCurrBuffer);
572  }
573  }
574 
575  // iterate over all children
576  for (childIdx = 0; childIdx < 8; childIdx++)
577  {
578  if (branch_arg->hasChild(bufferSelector_, childIdx))
579  {
580  // add current branch voxel to key
581  key_arg.pushBranch(childIdx);
582 
583  OctreeNode *childNode = branch_arg->getChildPtr(bufferSelector_,childIdx);
584 
585  switch (childNode->getNodeType ())
586  {
587  case BRANCH_NODE:
588  {
589  // recursively proceed with indexed child branch
590  serializeTreeRecursive (static_cast<BranchNode*> (childNode),
591  key_arg, binaryTreeOut_arg, dataVector_arg, doXOREncoding_arg,
592  newLeafsFilter_arg, minPointsPerLeaf_arg);
593  break;
594  }
595  case LEAF_NODE:
596  {
597  LeafNode* childLeaf = static_cast<LeafNode*> (childNode);
598 
599 
600  if (childLeaf->getSize () >= minPointsPerLeaf_arg)
601  {
602  if (!newLeafsFilter_arg)
603  {
604  if (dataVector_arg)
605  childLeaf->getData (*dataVector_arg);
606 
607  // we reached a leaf node -> execute serialization callback
608  serializeTreeCallback (*childLeaf, key_arg);
609  }
610  else if (!branch_arg->hasChild (!bufferSelector_, childIdx))
611  {
612  if (dataVector_arg)
613  childLeaf->getData (*dataVector_arg);
614 
615  serializeTreeCallback (*childLeaf, key_arg);
616  }
617  }
618 
619 
620  break;
621  }
622  default:
623  break;
624  }
625 
626  // pop current branch voxel from key
627  key_arg.popBranch();
628  }
629  else if (branch_arg->hasChild (!bufferSelector_, childIdx))
630  {
631 
632  // delete branch, free memory
633  deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
634 
635  }
636 
637  }
638  }
639 
640 
642  template<typename DataT, typename LeafT, typename BranchT> void
643  Octree2BufBase<DataT, LeafT, BranchT>::deserializeTreeRecursive (BranchNode* branch_arg,
644  unsigned int depthMask_arg, 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<DataT>::const_iterator* dataVectorIterator_arg,
648  typename std::vector<DataT>::const_iterator* dataVectorEndIterator_arg,
649  bool branchReset_arg, bool doXORDecoding_arg)
650  {
651  // child iterator
652  unsigned char childIdx;
653 
654  // node bits
655  char nodeBits;
656  char recoveredNodeBits;
657 
658  // branch reset -> this branch has been taken from previous buffer
659  if (branchReset_arg)
660  {
661  // we can safely remove children references
662  for (childIdx = 0; childIdx < 8; childIdx++)
663  {
664  branch_arg->setChildPtr(bufferSelector_, childIdx, 0);
665  }
666  }
667 
668  if (binaryTreeIT_arg!=binaryTreeIT_End_arg) {
669  // read branch occupancy bit pattern from vector
670  nodeBits = *binaryTreeIT_arg++;
671 
672 
673  // recover branch occupancy bit pattern
674  if (doXORDecoding_arg)
675  {
676  recoveredNodeBits = getBranchBitPattern (*branch_arg, !bufferSelector_) ^ nodeBits;
677  }
678  else
679  {
680  recoveredNodeBits = nodeBits;
681  }
682 
683  // iterate over all children
684  for (childIdx = 0; childIdx < 8; childIdx++)
685  {
686  // if occupancy bit for childIdx is set..
687  if (recoveredNodeBits & (1 << childIdx))
688  {
689  // add current branch voxel to key
690  key_arg.pushBranch(childIdx);
691 
692  bool doNodeReset;
693 
694  doNodeReset = false;
695 
696  if (depthMask_arg > 1)
697  {
698  // we have not reached maximum tree depth
699 
700  BranchNode* childBranch;
701 
702  // check if we find a branch node reference in previous buffer
703  if (!branch_arg->hasChild(bufferSelector_, childIdx))
704  {
705 
706  if (branch_arg->hasChild(!bufferSelector_, childIdx))
707  {
708  OctreeNode* childNode = branch_arg->getChildPtr(!bufferSelector_,childIdx);
709 
710  if (childNode->getNodeType()==BRANCH_NODE) {
711  childBranch = static_cast<BranchNode*> (childNode);
712  branch_arg->setChildPtr(bufferSelector_, childIdx, childNode);
713  } else {
714  // depth has changed.. child in preceeding buffer is a leaf node.
715  deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
716  createBranchChild (*branch_arg, childIdx, childBranch);
717  }
718 
719  // take child branch from previous buffer
720  doNodeReset = true; // reset the branch pointer array of stolen child node
721  }
722  else
723  {
724  // if required branch does not exist -> create it
725  createBranchChild (*branch_arg, childIdx, childBranch);
726  }
727 
728  branchCount_++;
729 
730  }
731  else
732  {
733  // required branch node already exists - use it
734  childBranch = static_cast<BranchNode*> (branch_arg->getChildPtr(bufferSelector_,childIdx));
735  }
736 
737  // recursively proceed with indexed child branch
738  deserializeTreeRecursive (childBranch, depthMask_arg / 2, key_arg,
739  binaryTreeIT_arg, binaryTreeIT_End_arg,
740  dataVectorIterator_arg, dataVectorEndIterator_arg,
741  doNodeReset, doXORDecoding_arg);
742 
743  }
744  else
745  {
746  // branch childs are leaf nodes
747  LeafNode* childLeaf;
748 
749  // check if we can take copy a reference pointer from previous buffer
750  if (branch_arg->hasChild(!bufferSelector_, childIdx))
751  {
752  // take child leaf node from previous buffer
753  OctreeNode* childNode = branch_arg->getChildPtr(!bufferSelector_,childIdx);
754  if (childNode->getNodeType()==LEAF_NODE) {
755  childLeaf = static_cast<LeafNode*> (childNode);
756  branch_arg->setChildPtr(bufferSelector_, childIdx, childNode);
757  childLeaf->reset ();
758  } else {
759  // depth has changed.. child in preceeding buffer is a leaf node.
760  deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
761  createLeafChild (*branch_arg, childIdx, childLeaf);
762  }
763  }
764  else
765  {
766  // if required leaf does not exist -> create it
767  createLeafChild (*branch_arg, childIdx, childLeaf);
768  }
769 
770  leafCount_++;
771 
772  OctreeKey dataKey;
773  bool bKeyBasedEncoding = false;
774 
775  if (dataVectorIterator_arg && dataVectorEndIterator_arg)
776  {
777  // add DataT objects to octree leaf as long as their key fit to voxel
778  while ( (*dataVectorIterator_arg != *dataVectorEndIterator_arg)
779  && (this->genOctreeKeyForDataT (**dataVectorIterator_arg,
780  dataKey) && (dataKey == key_arg)))
781  {
782  childLeaf->setData (**dataVectorIterator_arg);
783  (*dataVectorIterator_arg)++;
784  bKeyBasedEncoding = true;
785  objectCount_++;
786  }
787 
788  // add single DataT object to octree if key-based encoding is disabled
789  if (!bKeyBasedEncoding)
790  {
791  childLeaf->setData (**dataVectorIterator_arg);
792  (*dataVectorIterator_arg)++;
793  objectCount_++;
794  }
795  }
796 
797  // execute deserialization callback
798  this->deserializeTreeCallback (*childLeaf, key_arg);
799  }
800 
801  // pop current branch voxel from key
802  key_arg.popBranch();
803  }
804  else if (branch_arg->hasChild (!bufferSelector_, childIdx))
805  {
806  // remove old branch pointer information in current branch
807  branch_arg->setChildPtr(bufferSelector_, childIdx, 0);
808 
809  // remove unused branches in previous buffer
810  deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
811  }
812  }
813  }
814 
815  }
816 
818  template<typename DataT, typename LeafT, typename BranchT> void
819  Octree2BufBase<DataT, LeafT, BranchT>::treeCleanUpRecursive (BranchNode* branch_arg)
820  {
821  // child iterator
822  unsigned char childIdx;
823 
824  // bit pattern
825  char nodeBitPatternLastBuffer;
826  char nodeXORBitPattern;
827  char unusedBranchesBits;
828 
829  // occupancy bit pattern of branch node (previous octree buffer)
830  nodeBitPatternLastBuffer = getBranchBitPattern (*branch_arg, !bufferSelector_);
831 
832  // XOR of current and previous occupancy bit patterns
833  nodeXORBitPattern = getBranchXORBitPattern (*branch_arg);
834 
835  // bit pattern indicating unused octree nodes in previous branch
836  unusedBranchesBits = nodeXORBitPattern & nodeBitPatternLastBuffer;
837 
838  // iterate over all children
839  for (childIdx = 0; childIdx < 8; childIdx++)
840  {
841  if (branch_arg->hasChild(bufferSelector_, childIdx))
842  {
843  OctreeNode *childNode = branch_arg->getChildPtr(bufferSelector_,childIdx);
844 
845  switch (childNode->getNodeType ())
846  {
847  case BRANCH_NODE:
848  {
849  // recursively proceed with indexed child branch
850  treeCleanUpRecursive (static_cast<BranchNode*> (childNode));
851  break;
852  }
853  case LEAF_NODE:
854  // leaf level - nothing to do..
855  break;
856  default:
857  break;
858  }
859  }
860 
861  // check for unused branches in previous buffer
862  if (unusedBranchesBits & (1 << childIdx))
863  {
864  // delete branch, free memory
865  deleteBranchChild (*branch_arg, !bufferSelector_, childIdx);
866  }
867  }
868  }
869  }
870 }
871 
872 #define PCL_INSTANTIATE_Octree2BufBase(T) template class PCL_EXPORTS pcl::octree::Octree2BufBase<T>;
873 
874 #endif
875 
Generated on Sat Aug 24 2013 17:28:34 for Point Cloud Library (PCL) by   doxygen 1.8.4