Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
octree_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: octree_base.h 6201 2012-07-06 11:53:17Z jkammerl $
37  */
38 
39 #ifndef OCTREE_TREE_BASE_H
40 #define OCTREE_TREE_BASE_H
41 
42 #include <cstddef>
43 #include <vector>
44 
45 #include "octree_nodes.h"
46 #include "octree_container.h"
47 #include "octree_key.h"
48 #include "octree_iterator.h"
49 #include "octree_node_pool.h"
50 
51 namespace pcl
52 {
53  namespace octree
54  {
62  template<typename DataT, typename LeafT = OctreeContainerDataT<DataT>,
63  typename BranchT = OctreeContainerEmpty<DataT> >
64  class OctreeBase
65  {
66 
67  public:
68 
69  typedef OctreeBase<DataT, OctreeContainerDataT<DataT>, OctreeContainerEmpty<DataT> > SingleObjLeafContainer;
70  typedef OctreeBase<DataT, OctreeContainerDataTVector<DataT>, OctreeContainerEmpty<DataT> > MultipleObjsLeafContainer;
71 
72  typedef OctreeBase<DataT, LeafT, BranchT> OctreeT;
73 
74  // iterators are friends
75  friend class OctreeIteratorBase<DataT, OctreeT> ;
76  friend class OctreeDepthFirstIterator<DataT, OctreeT> ;
77  friend class OctreeBreadthFirstIterator<DataT, OctreeT> ;
78  friend class OctreeLeafNodeIterator<DataT, OctreeT> ;
79 
80  typedef OctreeBranchNode<BranchT> BranchNode;
81  typedef OctreeLeafNode<LeafT> LeafNode;
82 
83  // Octree iterators
84  typedef OctreeDepthFirstIterator<DataT, OctreeT> Iterator;
85  typedef const OctreeDepthFirstIterator<DataT, OctreeT> ConstIterator;
86 
87  typedef OctreeLeafNodeIterator<DataT, OctreeT> LeafNodeIterator;
88  typedef const OctreeLeafNodeIterator<DataT, OctreeT> ConstLeafNodeIterator;
89 
90  typedef OctreeDepthFirstIterator<DataT, OctreeT> DepthFirstIterator;
91  typedef const OctreeDepthFirstIterator<DataT, OctreeT> ConstDepthFirstIterator;
92  typedef OctreeBreadthFirstIterator<DataT, OctreeT> BreadthFirstIterator;
93  typedef const OctreeBreadthFirstIterator<DataT, OctreeT> ConstBreadthFirstIterator;
94 
95 
97  OctreeBase ();
98 
100  virtual
101  ~OctreeBase ();
102 
104  OctreeBase (const OctreeBase& source) :
105  leafCount_ (source.leafCount_),
106  branchCount_ (source.branchCount_),
107  objectCount_ (source.objectCount_),
108  rootNode_ (new (BranchNode) (*(source.rootNode_))),
109  depthMask_ (source.depthMask_),
110  octreeDepth_ (source.octreeDepth_),
111  maxKey_ (source.maxKey_),
112  branchNodePool_ (),
113  leafNodePool_ ()
114  {
115  }
116 
118  inline OctreeBase&
119  operator = (const OctreeBase &source)
120  {
121  leafCount_ = source.leafCount_;
122  branchCount_ = source.branchCount_;
123  objectCount_ = source.objectCount_;
124  rootNode_ = new (BranchNode) (*(source.rootNode_));
125  depthMask_ = source.depthMask_;
126  maxKey_ = source.maxKey_;
127  octreeDepth_ = source.octreeDepth_;
128  return (*this);
129  }
130 
134  void
135  setMaxVoxelIndex (unsigned int maxVoxelIndex_arg);
136 
140  void
141  setTreeDepth (unsigned int depth_arg);
142 
146  inline unsigned int
147  getTreeDepth () const
148  {
149  return this->octreeDepth_;
150  }
151 
156  inline void
157  enableDynamicDepth ( size_t maxObjsPerLeaf )
158  {
159  assert(leafCount_==0);
160  maxObjsPerLeaf_ = maxObjsPerLeaf;
161  }
162 
169  void
170  addData (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg,
171  const DataT& data_arg);
172 
180  bool
181  getData (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg, DataT& data_arg) const ;
182 
189  bool
190  existLeaf (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg) const ;
191 
197  void
198  removeLeaf (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg);
199 
203  inline std::size_t
204  getLeafCount () const
205  {
206  return leafCount_;
207  }
208 
212  inline std::size_t
213  getBranchCount () const
214  {
215  return branchCount_;
216  }
217 
221  void
222  deleteTree ( bool freeMemory_arg = true );
223 
227  void
228  serializeTree (std::vector<char>& binaryTreeOut_arg);
229 
234  void
235  serializeTree (std::vector<char>& binaryTreeOut_arg, std::vector<DataT>& dataVector_arg);
236 
240  void
241  serializeLeafs (std::vector<DataT>& dataVector_arg);
242 
246  void
247  deserializeTree (std::vector<char>& binaryTreeIn_arg);
248 
253  void
254  deserializeTree (std::vector<char>& binaryTreeIn_arg, std::vector<DataT>& dataVector_arg);
255 
256  protected:
257 
259  // Protected octree methods based on octree keys
261 
267  virtual bool
268  genOctreeKeyForDataT (const DataT &, OctreeKey &) const
269  {
270  // this class cannot relate DataT objects to octree keys
271  return (false);
272  }
273 
279  virtual bool
280  genDataTByOctreeKey (const OctreeKey &, DataT &) const
281  {
282  // this class cannot relate DataT objects to octree keys
283  return (false);
284  }
285 
290  inline void
291  addData (const OctreeKey& key_arg, const DataT& data_arg)
292  {
293  addDataToLeafRecursive (key_arg, depthMask_,data_arg, rootNode_);
294  }
295 
300  inline LeafNode*
301  findLeaf (const OctreeKey& key_arg) const
302  {
303  LeafNode* result = 0;
304  findLeafRecursive (key_arg, depthMask_, rootNode_, result);
305  return result;
306  }
307 
312  inline bool
313  existLeaf (const OctreeKey& key_arg) const
314  {
315  LeafNode* leafNode = 0;
316  if (key_arg <= maxKey_)
317  findLeafRecursive (key_arg, depthMask_, rootNode_, leafNode);
318  return (leafNode != 0);
319  }
320 
324  inline void
325  removeLeaf (const OctreeKey& key_arg)
326  {
327  if (key_arg <= maxKey_)
328  deleteLeafRecursive (key_arg, depthMask_, rootNode_);
329  }
330 
332  // Branch node accessor inline functions
334 
336  OctreeNode*
337  getRootNode () const
338  {
339  return this->rootNode_;
340  }
341 
347  inline bool
348  branchHasChild (const BranchNode& branch_arg, unsigned char childIdx_arg) const
349  {
350  // test occupancyByte for child existence
351  return (branch_arg.getChildPtr(childIdx_arg) != 0);
352  }
353 
359  inline OctreeNode*
360  getBranchChildPtr (const BranchNode& branch_arg,
361  unsigned char childIdx_arg) const
362  {
363  return branch_arg.getChildPtr(childIdx_arg);
364  }
365 
371  inline void setBranchChildPtr (BranchNode& branch_arg,
372  unsigned char childIdx_arg, OctreeNode* newChild_arg)
373  {
374  branch_arg[childIdx_arg] = newChild_arg;
375  }
376 
381  inline void getDataFromOctreeNode (const OctreeNode* node_arg,
382  DataT& data_arg)
383  {
384  if (node_arg->getNodeType () == LEAF_NODE)
385  {
386  const LeafT* leafContainer = dynamic_cast<const LeafT*> (node_arg);
387  leafContainer->getData (data_arg);
388  }
389  else
390  {
391  const BranchT* branchContainer =
392  dynamic_cast<const BranchT*> (node_arg);
393  branchContainer->getData (data_arg);
394  }
395  }
396 
401  inline void getDataFromOctreeNode (const OctreeNode* node_arg,
402  std::vector<DataT>& data_arg)
403  {
404  if (node_arg->getNodeType () == LEAF_NODE)
405  {
406  const LeafT* leafContainer = dynamic_cast<const LeafT*> (node_arg);
407  leafContainer->getData (data_arg);
408  }
409  else
410  {
411  const BranchT* branchContainer =
412  dynamic_cast<const BranchT*> (node_arg);
413  branchContainer->getData (data_arg);
414  }
415  }
416 
421  inline size_t getDataSizeFromOctreeNode (const OctreeNode* node_arg)
422  {
423  size_t nodeSize;
424  if (node_arg->getNodeType () == LEAF_NODE)
425  {
426  const LeafT* leafContainer = dynamic_cast<const LeafT*> (node_arg);
427  nodeSize = leafContainer->getSize ();
428  }
429  else
430  {
431  const BranchT* branchContainer =
432  dynamic_cast<const BranchT*> (node_arg);
433  nodeSize = branchContainer->getSize ();
434  }
435  return nodeSize;
436  }
441  inline char
442  getBranchBitPattern (const BranchNode& branch_arg) const
443  {
444  unsigned char i;
445  char nodeBits;
446 
447  // create bit pattern
448  nodeBits = 0;
449  for (i = 0; i < 8; i++) {
450  const OctreeNode* child = branch_arg.getChildPtr(i);
451  nodeBits |= static_cast<char> ((!!child) << i);
452  }
453 
454  return (nodeBits);
455  }
456 
461  inline void
462  deleteBranchChild (BranchNode& branch_arg, unsigned char childIdx_arg)
463  {
464  if (branch_arg.hasChild(childIdx_arg))
465  {
466  OctreeNode* branchChild = branch_arg[childIdx_arg];
467 
468  switch (branchChild->getNodeType ())
469  {
470  case BRANCH_NODE:
471  {
472  // free child branch recursively
473  deleteBranch (*static_cast<BranchNode*> (branchChild));
474  // push unused branch to branch pool
475  branchNodePool_.pushNode (
476  static_cast<BranchNode*> (branchChild));
477  }
478  break;
479 
480  case LEAF_NODE:
481  {
482  // push unused leaf to branch pool
483  leafNodePool_.pushNode (static_cast<LeafNode*> (branchChild));
484  break;
485  }
486  default:
487  break;
488  }
489 
490  // set branch child pointer to 0
491  branch_arg[childIdx_arg] = 0;
492  }
493  }
494 
498  inline void
499  deleteBranch (BranchNode& branch_arg)
500  {
501  char i;
502 
503  // delete all branch node children
504  for (i = 0; i < 8; i++)
505  deleteBranchChild (branch_arg, i);
506  }
507 
513  inline void createBranchChild (BranchNode& branch_arg,
514  unsigned char childIdx_arg, BranchNode*& newBranchChild_arg)
515  {
516  newBranchChild_arg = branchNodePool_.popNode ();
517  branch_arg[childIdx_arg] =
518  static_cast<OctreeNode*> (newBranchChild_arg);
519  }
520 
526  inline void
527  createLeafChild (BranchNode& branch_arg, unsigned char childIdx_arg, LeafNode*& newLeafChild_arg)
528  {
529  newLeafChild_arg = leafNodePool_.popNode();
530 
531  branch_arg[childIdx_arg] = static_cast<OctreeNode*> (newLeafChild_arg);
532  }
533 
537  inline void
538  branchReset (BranchNode& branch_arg)
539  {
540  branch_arg.reset ();
541  }
542 
545  inline void
546  poolCleanUp ()
547  {
548  branchNodePool_.deletePool();
549  leafNodePool_.deletePool();
550  }
551 
553  // Recursive octree methods
555 
562  void
563  addDataToLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg, const DataT& data_arg, BranchNode* branch_arg);
564 
572  void
573  findLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg, BranchNode* branch_arg, LeafNode*& result_arg) const;
574 
581  bool
582  deleteLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg, BranchNode* branch_arg);
583 
590  void
591  serializeTreeRecursive (const BranchNode* branch_arg, OctreeKey& key_arg,
592  std::vector<char>* binaryTreeOut_arg,
593  typename std::vector<DataT>* dataVector_arg) const;
594 
604  void
605  deserializeTreeRecursive (BranchNode* branch_arg,
606  unsigned int depthMask_arg, OctreeKey& key_arg,
607  typename std::vector<char>::const_iterator& binaryTreeIT_arg,
608  typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
609  typename std::vector<DataT>::const_iterator* dataVectorIterator_arg,
610  typename std::vector<DataT>::const_iterator* dataVectorEndIterator_arg);
611 
612 
614  // Serialization callbacks
616 
619  virtual void serializeTreeCallback (const LeafNode &,
620  const OctreeKey &) const
621  {
622 
623  }
624 
627  virtual void deserializeTreeCallback (LeafNode&, const OctreeKey&)
628  {
629 
630  }
631 
633  // Helpers
635 
640  inline double
641  Log2 (double n_arg)
642  {
643  return log( n_arg ) / log( 2.0 );
644  }
645 
649  inline bool
650  octreeCanResize ()
651  {
652  return (true);
653  }
654 
656  // Globals
658 
660  std::size_t leafCount_;
661 
663  std::size_t branchCount_;
664 
666  std::size_t objectCount_;
667 
669  BranchNode* rootNode_;
670 
674  std::size_t maxObjsPerLeaf_;
675 
677  unsigned int depthMask_;
678 
680  unsigned int octreeDepth_;
681 
683  OctreeKey maxKey_;
684 
686  OctreeNodePool<BranchNode> branchNodePool_;
687 
689  OctreeNodePool<LeafNode> leafNodePool_;
690  };
691  }
692 }
693 
694 //#include "impl/octree_base.hpp"
695 
696 #endif
697 
Generated on Sat Aug 24 2013 17:28:34 for Point Cloud Library (PCL) by   doxygen 1.8.4