Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
octree2buf_base.h
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.h 6201 2012-07-06 11:53:17Z jkammerl $
37  */
38 
39 #ifndef OCTREE_TREE_2BUF_BASE_H
40 #define OCTREE_TREE_2BUF_BASE_H
41 
42 #include <vector>
43 
44 #include "octree_nodes.h"
45 #include "octree_container.h"
46 #include "octree_key.h"
47 #include "octree_iterator.h"
48 #include "octree_node_pool.h"
49 
50 #include <stdio.h>
51 #include <string.h>
52 
53 namespace pcl
54 {
55  namespace octree
56  {
57 
58  template<typename ContainerT>
59  class BufferedBranchNode : public OctreeNode, ContainerT
60  {
61  using ContainerT::getSize;
62  using ContainerT::getData;
63  using ContainerT::setData;
64 
65  public:
67  BufferedBranchNode () : OctreeNode(), ContainerT(), preBuf(0xFFFFFF), postBuf(0xFFFFFF)
68  {
69  reset ();
70  }
71 
73  BufferedBranchNode (const BufferedBranchNode& source) : ContainerT(source)
74  {
75  *this = source;
76  }
77 
79  inline BufferedBranchNode&
80  operator = (const BufferedBranchNode &source_arg)
81  {
82 
83  unsigned char i, b;
84 
85  memset (childNodeArray_, 0, sizeof(childNodeArray_));
86 
87  for (b = 0; b < 2; ++b)
88  for (i = 0; i < 8; ++i)
89  if (source_arg.childNodeArray_[b][i])
90  childNodeArray_[b][i] = source_arg.childNodeArray_[b][i]->deepCopy ();
91 
92  return (*this);
93 
94  }
95 
97  virtual ~BufferedBranchNode ()
98  {
99  }
100 
102  virtual BufferedBranchNode*
103  deepCopy () const
104  {
105  return new BufferedBranchNode (*this);
106  }
107 
113  inline OctreeNode*
114  getChildPtr (unsigned char buffer_arg, unsigned char index_arg) const
115  {
116  assert( (buffer_arg<2) && (index_arg<8));
117  return childNodeArray_[buffer_arg][index_arg];
118  }
119 
125  inline void setChildPtr (unsigned char buffer_arg, unsigned char index_arg,
126  OctreeNode* newNode_arg)
127  {
128  assert( (buffer_arg<2) && (index_arg<8));
129  childNodeArray_[buffer_arg][index_arg] = newNode_arg;
130  }
131 
137  inline bool hasChild (unsigned char buffer_arg, unsigned char index_arg) const
138  {
139  assert( (buffer_arg<2) && (index_arg<8));
140  return (childNodeArray_[buffer_arg][index_arg] != 0);
141  }
142 
144  virtual node_type_t getNodeType () const
145  {
146  return BRANCH_NODE;
147  }
148 
150  inline void reset ()
151  {
152  memset (&childNodeArray_[0][0], 0, sizeof(OctreeNode*) * 8 * 2);
153  ContainerT::reset ();
154  }
155 
156  protected:
157  int preBuf;
158  OctreeNode* childNodeArray_[2][8];
159  int postBuf;
160 
161  };
162 
176  template<typename DataT, typename LeafT = OctreeContainerDataT<DataT>,
177  typename BranchT = OctreeContainerEmpty<DataT> >
178  class Octree2BufBase
179  {
180 
181  public:
182 
183  typedef Octree2BufBase<DataT, LeafT, BranchT> OctreeT;
184 
185  // iterators are friends
186  friend class OctreeIteratorBase<DataT, OctreeT> ;
187  friend class OctreeDepthFirstIterator<DataT, OctreeT> ;
188  friend class OctreeBreadthFirstIterator<DataT, OctreeT> ;
189  friend class OctreeLeafNodeIterator<DataT, OctreeT> ;
190 
191  typedef BufferedBranchNode<BranchT> BranchNode;
192  typedef OctreeLeafNode<LeafT> LeafNode;
193 
194  // Octree iterators
195  typedef OctreeDepthFirstIterator<DataT, OctreeT> Iterator;
196  typedef const OctreeDepthFirstIterator<DataT, OctreeT> ConstIterator;
197 
198  typedef OctreeLeafNodeIterator<DataT, OctreeT> LeafNodeIterator;
199  typedef const OctreeLeafNodeIterator<DataT, OctreeT> ConstLeafNodeIterator;
200 
201  typedef OctreeDepthFirstIterator<DataT, OctreeT> DepthFirstIterator;
202  typedef const OctreeDepthFirstIterator<DataT, OctreeT> ConstDepthFirstIterator;
203  typedef OctreeBreadthFirstIterator<DataT, OctreeT> BreadthFirstIterator;
204  typedef const OctreeBreadthFirstIterator<DataT, OctreeT> ConstBreadthFirstIterator;
205 
207  Octree2BufBase ();
208 
210  virtual
211  ~Octree2BufBase ();
212 
214  Octree2BufBase (const Octree2BufBase& source) :
215  leafCount_ (source.leafCount_), branchCount_ (source.branchCount_), objectCount_ (
216  source.objectCount_), rootNode_ (
217  new (BranchNode) (* (source.rootNode_))), depthMask_ (
218  source.depthMask_), maxKey_ (source.maxKey_), branchNodePool_ (), leafNodePool_ (), bufferSelector_ (
219  source.bufferSelector_), treeDirtyFlag_ (source.treeDirtyFlag_), octreeDepth_ (
220  source.octreeDepth_)
221  {
222  }
223 
225  inline Octree2BufBase&
226  operator = (const Octree2BufBase& source)
227  {
228  leafCount_ = source.leafCount_;
229  branchCount_ = source.branchCount_;
230  objectCount_ = source.objectCount_;
231  rootNode_ = new (BranchNode) (* (source.rootNode_));
232  depthMask_ = source.depthMask_;
233  maxKey_ = source.maxKey_;
234  bufferSelector_ = source.bufferSelector_;
235  treeDirtyFlag_ = source.treeDirtyFlag_;
236  octreeDepth_ = source.octreeDepth_;
237  return (*this);
238  }
239 
243  void
244  setMaxVoxelIndex (unsigned int maxVoxelIndex_arg);
245 
249  void
250  setTreeDepth (unsigned int depth_arg);
251 
255  inline unsigned int getTreeDepth () const
256  {
257  return this->octreeDepth_;
258  }
259 
266  void
267  addData (unsigned int idxX_arg, unsigned int idxY_arg,
268  unsigned int idxZ_arg, const DataT& data_arg);
269 
277  bool
278  getData (unsigned int idxX_arg, unsigned int idxY_arg,
279  unsigned int idxZ_arg, DataT& data_arg) const;
280 
287  bool
288  existLeaf (unsigned int idxX_arg, unsigned int idxY_arg,
289  unsigned int idxZ_arg) const;
290 
296  void
297  removeLeaf (unsigned int idxX_arg, unsigned int idxY_arg,
298  unsigned int idxZ_arg);
299 
303  inline unsigned int getLeafCount () const
304  {
305  return (static_cast<unsigned int> (leafCount_));
306  }
307 
311  inline unsigned int getBranchCount () const
312  {
313  return (static_cast<unsigned int> (branchCount_));
314  }
315 
319  void
320  deleteTree (bool freeMemory_arg = false);
321 
323  inline void deletePreviousBuffer ()
324  {
325  treeCleanUpRecursive (rootNode_);
326  }
327 
329  inline void deleteCurrentBuffer ()
330  {
331  bufferSelector_ = !bufferSelector_;
332  treeCleanUpRecursive (rootNode_);
333  leafCount_ = 0;
334  }
335 
337  void
338  switchBuffers ();
339 
344  void
345  serializeTree (std::vector<char>& binaryTreeOut_arg,
346  bool doXOREncoding_arg = false);
347 
353  void
354  serializeTree (std::vector<char>& binaryTreeOut_arg,
355  std::vector<DataT>& dataVector_arg, bool doXOREncoding_arg = false);
356 
360  void
361  serializeLeafs (std::vector<DataT>& dataVector_arg);
362 
367  void
368  serializeNewLeafs (std::vector<DataT>& dataVector_arg,
369  const int minPointsPerLeaf_arg = 0);
370 
375  void
376  deserializeTree (std::vector<char>& binaryTreeIn_arg,
377  bool doXORDecoding_arg = false);
378 
384  void
385  deserializeTree (std::vector<char>& binaryTreeIn_arg,
386  std::vector<DataT>& dataVector_arg, bool doXORDecoding_arg = false);
387 
388  protected:
389 
391  // Protected octree methods based on octree keys
393 
395  OctreeNode*
396  getRootNode () const
397  {
398  return (this->rootNode_);
399  }
400 
406  virtual bool genOctreeKeyForDataT (const DataT &, OctreeKey &) const
407  {
408  // this class cannot relate DataT objects to octree keys
409  return (false);
410  }
411 
417  virtual bool genDataTByOctreeKey (const OctreeKey &, DataT &) const
418  {
419  // this class cannot relate DataT objects to octree keys
420  return (false);
421  }
422 
427  inline void addData (const OctreeKey& key_arg, const DataT& data_arg)
428  {
429  // request a (new) leaf from tree
430  LeafT* leaf = createLeaf (key_arg);
431 
432  // assign data to leaf
433  if (leaf)
434  {
435  leaf->setData (data_arg);
436  objectCount_++;
437  }
438  }
439 
444  inline LeafT*
445  findLeaf (const OctreeKey& key_arg) const
446  {
447  return findLeafRecursive (key_arg, depthMask_, rootNode_);
448  }
449 
455  inline LeafT*
456  createLeaf (const OctreeKey& key_arg)
457  {
458  LeafT* result;
459 
460  result = createLeafRecursive (key_arg, depthMask_, rootNode_, false);
461 
462  // getLeafRecursive has changed the octree -> clean-up/tree-reset might be required
463  treeDirtyFlag_ = true;
464 
465  return result;
466  }
467 
472  inline bool existLeaf (const OctreeKey& key_arg) const
473  {
474  return ( (key_arg <= maxKey_)
475  && (findLeafRecursive (key_arg, depthMask_, rootNode_) != 0));
476  }
477 
481  inline void removeLeaf (const OctreeKey& key_arg)
482  {
483  if (key_arg <= maxKey_)
484  {
485  deleteLeafRecursive (key_arg, depthMask_, rootNode_);
486 
487  // we changed the octree structure -> dirty
488  treeDirtyFlag_ = true;
489  }
490  }
491 
493  // Branch node accessor inline functions
495 
501  inline bool
502  branchHasChild (const BranchNode& branch_arg, unsigned char childIdx_arg) const
503  {
504  // test occupancyByte for child existence
505  return (branch_arg.getChildPtr(bufferSelector_, childIdx_arg) != 0);
506  }
507 
513  inline OctreeNode*
514  getBranchChildPtr (const BranchNode& branch_arg,
515  unsigned char childIdx_arg) const
516  {
517  return branch_arg.getChildPtr(bufferSelector_, childIdx_arg);
518  }
519 
525  inline void setBranchChildPtr (BranchNode& branch_arg,
526  unsigned char childIdx_arg, OctreeNode* newChild_arg)
527  {
528  branch_arg.setChildPtr(bufferSelector_, childIdx_arg, newChild_arg);
529  }
530 
535  inline void getDataFromOctreeNode (const OctreeNode* node_arg,
536  DataT& data_arg)
537  {
538  if (node_arg->getNodeType () == LEAF_NODE)
539  {
540  const LeafT* leafContainer = dynamic_cast<const LeafT*> (node_arg);
541  leafContainer->getData (data_arg);
542  }
543  else
544  {
545  const BranchT* branchContainer =
546  dynamic_cast<const BranchT*> (node_arg);
547  branchContainer->getData (data_arg);
548  }
549  }
550 
555  inline void getDataFromOctreeNode (const OctreeNode* node_arg,
556  std::vector<DataT>& data_arg)
557  {
558  if (node_arg->getNodeType () == LEAF_NODE)
559  {
560  const LeafT* leafContainer = dynamic_cast<const LeafT*> (node_arg);
561  leafContainer->getData (data_arg);
562  }
563  else
564  {
565  const BranchT* branchContainer =
566  dynamic_cast<const BranchT*> (node_arg);
567  branchContainer->getData (data_arg);
568  }
569  }
570 
575  inline size_t getDataSizeFromOctreeNode (const OctreeNode* node_arg)
576  {
577  size_t nodeSize;
578  if (node_arg->getNodeType () == LEAF_NODE)
579  {
580  const LeafT* leafContainer = dynamic_cast<const LeafT*> (node_arg);
581  nodeSize = leafContainer->getSize ();
582  }
583  else
584  {
585  const BranchT* branchContainer =
586  dynamic_cast<const BranchT*> (node_arg);
587  nodeSize = branchContainer->getSize ();
588  }
589  return nodeSize;
590  }
591 
596  inline char getBranchBitPattern (const BranchNode& branch_arg) const
597  {
598  unsigned char i;
599  char nodeBits;
600 
601  // create bit pattern
602  nodeBits = 0;
603  for (i = 0; i < 8; i++)
604  {
605  nodeBits |= static_cast<char> ( (!!branch_arg.getChildPtr (
606  bufferSelector_, i)) << i);
607  }
608 
609  return (nodeBits);
610  }
611 
617  inline char getBranchBitPattern (const BranchNode& branch_arg,
618  unsigned char bufferSelector_arg) const
619  {
620  unsigned char i;
621  char nodeBits;
622 
623  // create bit pattern
624  nodeBits = 0;
625  for (i = 0; i < 8; i++)
626  {
627  nodeBits |= static_cast<char> ( (!!branch_arg.getChildPtr (
628  bufferSelector_arg, i)) << i);
629  }
630 
631  return (nodeBits);
632  }
633 
638  inline char getBranchXORBitPattern (
639  const BranchNode& branch_arg) const
640  {
641  unsigned char i;
642  char nodeBits[2];
643 
644  // create bit pattern for both buffers
645  nodeBits[0] = nodeBits[1] = 0;
646 
647  for (i = 0; i < 8; i++)
648  {
649  nodeBits[0] |= static_cast<char> ( (!!branch_arg.getChildPtr (0, i))
650  << i);
651  nodeBits[1] |= static_cast<char> ( (!!branch_arg.getChildPtr (1, i))
652  << i);
653  }
654 
655  return nodeBits[0] ^ nodeBits[1];
656  }
657 
662  inline bool hasBranchChanges (const BranchNode& branch_arg) const
663  {
664  return (getBranchXORBitPattern (branch_arg) > 0);
665  }
666 
672  inline void deleteBranchChild (BranchNode& branch_arg,
673  unsigned char bufferSelector_arg,
674  unsigned char childIdx_arg)
675  {
676  if (branch_arg.hasChild(bufferSelector_arg, childIdx_arg))
677  {
678  OctreeNode* branchChild = branch_arg.getChildPtr(bufferSelector_arg, childIdx_arg);
679 
680  switch (branchChild->getNodeType ())
681  {
682  case BRANCH_NODE:
683  {
684  // free child branch recursively
685  deleteBranch (*static_cast<BranchNode*> (branchChild));
686 
687  // push unused branch to branch pool
688  branchNodePool_.pushNode (
689  static_cast<BranchNode*> (branchChild));
690  break;
691  }
692 
693  case LEAF_NODE:
694  {
695  // push unused leaf to branch pool
696  leafNodePool_.pushNode(
697  static_cast<LeafNode*> (branchChild));
698  break;
699  }
700  default:
701  break;
702  }
703 
704  // set branch child pointer to 0
705  branch_arg.setChildPtr(bufferSelector_arg, childIdx_arg, 0);
706  }
707  }
708 
712  inline void deleteBranch (BranchNode& branch_arg)
713  {
714  char i;
715 
716  // delete all branch node children
717  for (i = 0; i < 8; i++)
718  {
719 
720  if (branch_arg.getChildPtr(0, i) == branch_arg.getChildPtr(1, i))
721  {
722  // reference was copied - there is only one child instance to be deleted
723  deleteBranchChild (branch_arg, 0, i);
724 
725  // remove pointers from both buffers
726  branch_arg.setChildPtr(0, i, 0);
727  branch_arg.setChildPtr(1, i, 0);
728  }
729  else
730  {
731  deleteBranchChild (branch_arg, 0, i);
732  deleteBranchChild (branch_arg, 1, i);
733  }
734  }
735  }
736 
742  inline void createBranchChild (BranchNode& branch_arg,
743  unsigned char childIdx_arg, BranchNode*& newBranchChild_arg)
744  {
745 
746  newBranchChild_arg = branchNodePool_.popNode();
747 
748  branch_arg.setChildPtr (bufferSelector_, childIdx_arg,
749  static_cast<OctreeNode*> (newBranchChild_arg));
750  }
751 
757  inline void createLeafChild (BranchNode& branch_arg,
758  unsigned char childIdx_arg, LeafNode*& newLeafChild_arg)
759  {
760  newLeafChild_arg = leafNodePool_.popNode();
761 
762  branch_arg.setChildPtr(bufferSelector_, childIdx_arg, newLeafChild_arg);
763  }
764 
767  inline void poolCleanUp ()
768  {
769  branchNodePool_.deletePool();
770  leafNodePool_.deletePool();
771  }
772 
774  // Recursive octree methods
776 
784  LeafT*
785  createLeafRecursive (const OctreeKey& key_arg,
786  unsigned int depthMask_arg, BranchNode* branch_arg,
787  bool branchReset_arg);
788 
796  LeafT*
797  findLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg,
798  BranchNode* branch_arg) const;
799 
806  bool
807  deleteLeafRecursive (const OctreeKey& key_arg,
808  unsigned int depthMask_arg, BranchNode* branch_arg);
809 
819  void
820  serializeTreeRecursive (BranchNode* branch_arg, OctreeKey& key_arg,
821  std::vector<char>* binaryTreeOut_arg,
822  typename std::vector<DataT>* dataVector_arg, bool doXOREncoding_arg = false,
823  bool newLeafsFilter_arg = false, std::size_t minPointsPerLeaf_arg = 0);
824 
835  void
836  deserializeTreeRecursive (BranchNode* branch_arg,
837  unsigned int depthMask_arg, OctreeKey& key_arg,
838  typename std::vector<char>::const_iterator& binaryTreeIT_arg,
839  typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
840  typename std::vector<DataT>::const_iterator* dataVectorIterator_arg,
841  typename std::vector<DataT>::const_iterator* dataVectorEndIterator_arg,
842  bool branchReset_arg = false, bool doXORDecoding_arg = false);
843 
844 
846  // Serialization callbacks
848 
851  virtual void serializeTreeCallback (LeafNode &, const OctreeKey &)
852  {
853 
854  }
855 
858  virtual void deserializeTreeCallback (LeafNode&, const OctreeKey&)
859  {
860 
861  }
862 
864  // Helpers
866 
870  void
871  treeCleanUpRecursive (BranchNode* branch_arg);
872 
877  inline double Log2 (double n_arg)
878  {
879  return log (n_arg) / log (2.0);
880  }
881 
885  inline bool octreeCanResize ()
886  {
887  return (false);
888  }
889 
893  inline void printBinary (char data_arg)
894  {
895  unsigned char mask = 1; // Bit mask
896 
897  // Extract the bits
898  for (int i = 0; i < 8; i++)
899  {
900  // Mask each bit in the byte and print it
901  std::cout << ((data_arg & (mask << i)) ? "1" : "0");
902  }
903  std::cout << std::endl;
904  }
905 
907  // Globals
909 
911  std::size_t leafCount_;
912 
914  std::size_t branchCount_;
915 
917  std::size_t objectCount_;
918 
920  BranchNode* rootNode_;
921 
923  unsigned int depthMask_;
924 
926  OctreeKey maxKey_;
927 
929  OctreeNodePool<BranchNode> branchNodePool_;
930 
932  OctreeNodePool<LeafNode> leafNodePool_;
933 
935  unsigned char bufferSelector_;
936 
937  // flags indicating if unused branches and leafs might exist in previous buffer
938  bool treeDirtyFlag_;
939 
941  unsigned int octreeDepth_;
942  };
943  }
944 }
945 
946 //#include "impl/octree2buf_base.hpp"
947 
948 #endif
949 
Generated on Sat Aug 24 2013 17:28:34 for Point Cloud Library (PCL) by   doxygen 1.8.4