Point Cloud Library (PCL)  1.7.0
octree_iterator.h
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_ITERATOR_H
40 #define PCL_OCTREE_ITERATOR_H
41 
42 #include <cstddef>
43 #include <vector>
44 #include <deque>
45 
46 #include "octree_nodes.h"
47 #include "octree_key.h"
48 
49 #include <pcl/point_cloud.h>
50 #include <pcl/point_types.h>
51 
52 #include <iterator>
53 
54 // Ignore warnings in the above headers
55 #ifdef __GNUC__
56 #pragma GCC system_header
57 #endif
58 
59 namespace pcl
60 {
61  namespace octree
62  {
63 
64  // Octree iterator state pushed on stack/list
65  struct IteratorState{
68  unsigned char depth_;
69  };
70 
71 
72  /** \brief @b Abstract octree iterator class
73  * \note Octree iterator base class
74  * \ingroup octree
75  * \author Julius Kammerl (julius@kammerl.de)
76  */
77  template<typename OctreeT>
78  class OctreeIteratorBase : public std::iterator<std::forward_iterator_tag, const OctreeNode, void,
79  const OctreeNode*, const OctreeNode&>
80  {
81  public:
82 
83  typedef typename OctreeT::LeafNode LeafNode;
84  typedef typename OctreeT::BranchNode BranchNode;
85 
86  typedef typename OctreeT::LeafContainer LeafContainer;
87  typedef typename OctreeT::BranchContainer BranchContainer;
88 
89  /** \brief Empty constructor.
90  */
91  explicit
92  OctreeIteratorBase (unsigned int max_depth_arg = 0) :
93  octree_ (0), current_state_(0), max_octree_depth_(max_depth_arg)
94  {
95  this->reset ();
96  }
97 
98  /** \brief Constructor.
99  * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
100  * \param[in] max_depth_arg Depth limitation during traversal
101  */
102  explicit
103  OctreeIteratorBase (OctreeT* octree_arg, unsigned int max_depth_arg = 0) :
104  octree_ (octree_arg), current_state_(0), max_octree_depth_(max_depth_arg)
105  {
106  this->reset ();
107  }
108 
109  /** \brief Copy constructor.
110  * \param[in] src the iterator to copy into this
111  * \param[in] max_depth_arg Depth limitation during traversal
112  */
113  OctreeIteratorBase (const OctreeIteratorBase& src, unsigned int max_depth_arg = 0) :
114  octree_ (src.octree_), current_state_(0), max_octree_depth_(max_depth_arg)
115  {
116  this->reset ();
117  }
118 
119  /** \brief Copy operator.
120  * \param[in] src the iterator to copy into this
121  */
122  inline OctreeIteratorBase&
124  {
125  octree_ = src.octree_;
128  return (*this);
129  }
130 
131  /** \brief Empty deconstructor. */
132  virtual
134  {
135  }
136 
137  /** \brief Equal comparison operator
138  * \param[in] OctreeIteratorBase to compare with
139  */
140  bool operator==(const OctreeIteratorBase& other) const
141  {
142  return (( octree_ ==other.octree_) &&
143  ( current_state_ == other.current_state_) &&
144  ( max_octree_depth_ == other.max_octree_depth_) );
145  }
146 
147  /** \brief Inequal comparison operator
148  * \param[in] OctreeIteratorBase to compare with
149  */
150  bool operator!=(const OctreeIteratorBase& other) const
151  {
152  return (( octree_ !=other.octree_) &&
153  ( current_state_ != other.current_state_) &&
154  ( max_octree_depth_ != other.max_octree_depth_) );
155  }
156 
157  /** \brief Reset iterator */
158  inline void reset ()
159  {
160  current_state_ = 0;
161  if (octree_ && (!max_octree_depth_))
162  {
163  max_octree_depth_ = octree_->getTreeDepth();
164  }
165  }
166 
167  /** \brief Get octree key for the current iterator octree node
168  * \return octree key of current node
169  */
170  inline const OctreeKey&
172  {
173  assert(octree_!=0);
174  assert(current_state_!=0);
175 
176  return (current_state_->key_);
177  }
178 
179  /** \brief Get the current depth level of octree
180  * \return depth level
181  */
182  inline unsigned int
184  {
185  assert(octree_!=0);
186  assert(current_state_!=0);
187 
188  return (current_state_->depth_);
189  }
190 
191  /** \brief Get the current octree node
192  * \return pointer to current octree node
193  */
194  inline OctreeNode*
196  {
197  assert(octree_!=0);
198  assert(current_state_!=0);
199 
200  return (current_state_->node_);
201  }
202 
203 
204  /** \brief check if current node is a branch node
205  * \return true if current node is a branch node, false otherwise
206  */
207  inline bool
208  isBranchNode () const
209  {
210  assert(octree_!=0);
211  assert(current_state_!=0);
212 
213  return (current_state_->node_->getNodeType () == BRANCH_NODE);
214  }
215 
216  /** \brief check if current node is a branch node
217  * \return true if current node is a branch node, false otherwise
218  */
219  inline bool
220  isLeafNode () const
221  {
222  assert(octree_!=0);
223  assert(current_state_!=0);
224 
225  return (current_state_->node_->getNodeType () == LEAF_NODE);
226  }
227 
228  /** \brief *operator.
229  * \return pointer to the current octree node
230  */
231  inline OctreeNode*
232  operator* () const
233  { // return designated object
234  if (octree_ && current_state_)
235  {
236  return (current_state_->node_);
237  } else
238  {
239  return 0;
240  }
241  }
242 
243  /** \brief Get bit pattern of children configuration of current node
244  * \return bit pattern (byte) describing the existence of 8 children of the current node
245  */
246  inline char
248  {
249  char ret = 0;
250 
251  assert(octree_!=0);
252  assert(current_state_!=0);
253 
254  if (isBranchNode ())
255  {
256 
257  // current node is a branch node
258  const BranchNode* current_branch = static_cast<const BranchNode*> (current_state_->node_);
259 
260  // get child configuration bit pattern
261  ret = octree_->getBranchBitPattern (*current_branch);
262 
263  }
264 
265  return (ret);
266  }
267 
268  /** \brief Method for retrieving a single leaf container from the octree leaf node
269  * \return Reference to container class of leaf node.
270  */
271  const LeafContainer&
273  {
274  assert(octree_!=0);
275  assert(current_state_!=0);
276  assert(this->isLeafNode());
277 
278  LeafNode* leaf_node = static_cast<LeafNode*>(current_state_->node_);
279 
280  return leaf_node->getContainer();
281  }
282 
283  /** \brief Method for retrieving a single leaf container from the octree leaf node
284  * \return Reference to container class of leaf node.
285  */
288  {
289  assert(octree_!=0);
290  assert(current_state_!=0);
291  assert(this->isLeafNode());
292 
293  LeafNode* leaf_node = static_cast<LeafNode*>(current_state_->node_);
294 
295  return leaf_node->getContainer();
296  }
297 
298  /** \brief Method for retrieving the container from an octree branch node
299  * \return BranchContainer.
300  */
301  const BranchContainer&
303  {
304  assert(octree_!=0);
305  assert(current_state_!=0);
306  assert(this->isBranchNode());
307 
308  BranchNode* branch_node = static_cast<BranchNode*>(current_state_->node_);
309 
310  return branch_node->getContainer();
311  }
312 
313  /** \brief Method for retrieving the container from an octree branch node
314  * \return BranchContainer.
315  */
318  {
319  assert(octree_!=0);
320  assert(current_state_!=0);
321  assert(this->isBranchNode());
322 
323  BranchNode* branch_node = static_cast<BranchNode*>(current_state_->node_);
324 
325  return branch_node->getContainer();
326  }
327 
328  /** \brief get a integer identifier for current node (note: identifier depends on tree depth).
329  * \return node id.
330  */
331  virtual unsigned long
332  getNodeID () const
333  {
334  unsigned long id = 0;
335 
336  assert(octree_!=0);
337  assert(current_state_!=0);
338 
339  if (current_state_)
340  {
341  const OctreeKey& key = getCurrentOctreeKey();
342  // calculate integer id with respect to octree key
343  unsigned int depth = octree_->getTreeDepth ();
344  id = key.x << (depth * 2) | key.y << (depth * 1) | key.z << (depth * 0);
345  }
346 
347  return id;
348  }
349 
350  protected:
351  /** \brief Reference to octree class. */
352  OctreeT* octree_;
353 
354  /** \brief Pointer to current iterator state. */
356 
357  /** \brief Maximum octree depth */
358  unsigned int max_octree_depth_;
359  };
360 
361  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
362  /** \brief @b Octree iterator class
363  * \note This class implements a forward iterator for traversing octrees in a depth-first manner.
364  * \ingroup octree
365  * \author Julius Kammerl (julius@kammerl.de)
366  */
367  template<typename OctreeT>
369  {
370 
371  public:
372 
375 
376  /** \brief Empty constructor.
377  * \param[in] max_depth_arg Depth limitation during traversal
378  */
379  explicit
380  OctreeDepthFirstIterator (unsigned int max_depth_arg = 0);
381 
382  /** \brief Constructor.
383  * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
384  * \param[in] max_depth_arg Depth limitation during traversal
385  */
386  explicit
387  OctreeDepthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0);
388 
389  /** \brief Empty deconstructor. */
390  virtual
392 
393  /** \brief Copy operator.
394  * \param[in] src the iterator to copy into this
395  */
398  {
399 
401 
402  stack_ = src.stack_;
403 
404  if (stack_.size())
405  {
406  this->current_state_ = &stack_.back();
407  } else
408  {
409  this->current_state_ = 0;
410  }
411 
412  return (*this);
413  }
414 
415  /** \brief Reset the iterator to the root node of the octree
416  */
417  virtual void
418  reset ();
419 
420  /** \brief Preincrement operator.
421  * \note recursively step to next octree node
422  */
424  operator++ ();
425 
426  /** \brief postincrement operator.
427  * \note recursively step to next octree node
428  */
431  {
432  OctreeDepthFirstIterator _Tmp = *this;
433  ++*this;
434  return (_Tmp);
435  }
436 
437  /** \brief Skip all child voxels of current node and return to parent node.
438  */
439  void
440  skipChildVoxels ();
441 
442  protected:
443  /** Stack structure. */
444  std::vector<IteratorState> stack_;
445  };
446 
447  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
448  /** \brief @b Octree iterator class
449  * \note This class implements a forward iterator for traversing octrees in a breadth-first manner.
450  * \ingroup octree
451  * \author Julius Kammerl (julius@kammerl.de)
452  */
453  template<typename OctreeT>
455  {
456  // public typedefs
459 
460 
461  public:
462  /** \brief Empty constructor.
463  * \param[in] max_depth_arg Depth limitation during traversal
464  */
465  explicit
466  OctreeBreadthFirstIterator (unsigned int max_depth_arg = 0);
467 
468  /** \brief Constructor.
469  * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
470  * \param[in] max_depth_arg Depth limitation during traversal
471  */
472  explicit
473  OctreeBreadthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0);
474 
475  /** \brief Empty deconstructor. */
476  virtual
478 
479  /** \brief Copy operator.
480  * \param[in] src the iterator to copy into this
481  */
484  {
485 
487 
488  FIFO_ = src.FIFO_;
489 
490  if (FIFO_.size())
491  {
492  this->current_state_ = &FIFO_.front();
493  } else
494  {
495  this->current_state_ = 0;
496  }
497 
498  return (*this);
499  }
500 
501  /** \brief Reset the iterator to the root node of the octree
502  */
503  void
504  reset ();
505 
506  /** \brief Preincrement operator.
507  * \note step to next octree node
508  */
510  operator++ ();
511 
512  /** \brief postincrement operator.
513  * \note step to next octree node
514  */
517  {
518  OctreeBreadthFirstIterator _Tmp = *this;
519  ++*this;
520  return (_Tmp);
521  }
522 
523  protected:
524  /** FIFO list */
525  std::deque<IteratorState> FIFO_;
526  };
527 
528  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
529  /** \brief Octree leaf node iterator class
530  * \note This class implements a forward iterator for traversing the leaf nodes of an octree data structure.
531  * \ingroup octree
532  * \author Julius Kammerl (julius@kammerl.de)
533  */
534  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
535  template<typename OctreeT>
537  {
540 
541  public:
542  /** \brief Empty constructor.
543  * \param[in] max_depth_arg Depth limitation during traversal
544  */
545  explicit
546  OctreeLeafNodeIterator (unsigned int max_depth_arg = 0) :
547  OctreeDepthFirstIterator<OctreeT> (max_depth_arg)
548  {
549  reset ();
550  }
551 
552  /** \brief Constructor.
553  * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
554  * \param[in] max_depth_arg Depth limitation during traversal
555  */
556  explicit
557  OctreeLeafNodeIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0) :
558  OctreeDepthFirstIterator<OctreeT> (octree_arg, max_depth_arg)
559  {
560  reset ();
561  }
562 
563  /** \brief Empty deconstructor. */
564  virtual
566  {
567  }
568 
569  /** \brief Reset the iterator to the root node of the octree
570  */
571  inline void
572  reset ()
573  {
575  this->operator++ ();
576  }
577 
578  /** \brief Preincrement operator.
579  * \note recursively step to next octree leaf node
580  */
581  inline OctreeLeafNodeIterator&
583  {
584  do
585  {
587  } while ((this->current_state_) && (this->current_state_->node_->getNodeType () != LEAF_NODE));
588 
589  return (*this);
590  }
591 
592  /** \brief postincrement operator.
593  * \note step to next octree node
594  */
597  {
598  OctreeLeafNodeIterator _Tmp = *this;
599  ++*this;
600  return (_Tmp);
601  }
602 
603  /** \brief *operator.
604  * \return pointer to the current octree leaf node
605  */
606  OctreeNode*
607  operator* () const
608  {
609  // return designated object
610  OctreeNode* ret = 0;
611 
612  if (this->current_state_ && (this->current_state_->node_->getNodeType () == LEAF_NODE))
613  ret = this->current_state_->node_;
614  return (ret);
615  }
616  };
617 
618  }
619 }
620 
621 #endif
622 
OctreeNode * operator*() const
*operator.
OctreeBreadthFirstIterator & operator=(const OctreeBreadthFirstIterator &src)
Copy operator.
void reset()
Reset iterator.
bool operator==(const OctreeIteratorBase &other) const
Equal comparison operator.
OctreeNode * getCurrentOctreeNode() const
Get the current octree node.
const BranchContainer & getBranchContainer() const
Method for retrieving the container from an octree branch node.
OctreeLeafNodeIterator & operator++()
Preincrement operator.
const LeafContainer & getLeafContainer() const
Method for retrieving a single leaf container from the octree leaf node.
OctreeIteratorBase(const OctreeIteratorBase &src, unsigned int max_depth_arg=0)
Copy constructor.
OctreeIteratorBase & operator=(const OctreeIteratorBase &src)
Copy operator.
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
OctreeT::LeafContainer LeafContainer
std::deque< IteratorState > FIFO_
FIFO list.
std::vector< IteratorState > stack_
Stack structure.
IteratorState * current_state_
Pointer to current iterator state.
OctreeNode * operator*() const
*operator.
Octree leaf node iterator class.
OctreeT::BranchNode BranchNode
LeafContainer & getLeafContainer()
Method for retrieving a single leaf container from the octree leaf node.
unsigned int max_octree_depth_
Maximum octree depth.
void reset()
Reset the iterator to the root node of the octree.
virtual void reset()
Reset the iterator to the root node of the octree.
const OctreeKey & getCurrentOctreeKey() const
Get octree key for the current iterator octree node.
virtual unsigned long getNodeID() const
get a integer identifier for current node (note: identifier depends on tree depth).
OctreeT::BranchContainer BranchContainer
OctreeIteratorBase< OctreeT >::LeafNode LeafNode
OctreeIteratorBase< OctreeT >::BranchNode BranchNode
OctreeIteratorBase(unsigned int max_depth_arg=0)
Empty constructor.
OctreeLeafNodeIterator(unsigned int max_depth_arg=0)
Empty constructor.
OctreeDepthFirstIterator(unsigned int max_depth_arg=0)
Empty constructor.
bool isLeafNode() const
check if current node is a branch node
void skipChildVoxels()
Skip all child voxels of current node and return to parent node.
virtual ~OctreeBreadthFirstIterator()
Empty deconstructor.
Octree key class
Definition: octree_key.h:53
OctreeLeafNodeIterator(OctreeT *octree_arg, unsigned int max_depth_arg=0)
Constructor.
virtual ~OctreeLeafNodeIterator()
Empty deconstructor.
unsigned int getCurrentOctreeDepth() const
Get the current depth level of octree.
void reset()
Reset the iterator to the root node of the octree.
OctreeBreadthFirstIterator & operator++()
Preincrement operator.
OctreeT * octree_
Reference to octree class.
OctreeDepthFirstIterator & operator++()
Preincrement operator.
Abstract octree iterator class
OctreeDepthFirstIterator & operator=(const OctreeDepthFirstIterator &src)
Copy operator.
char getNodeConfiguration() const
Get bit pattern of children configuration of current node.
virtual ~OctreeIteratorBase()
Empty deconstructor.
bool operator!=(const OctreeIteratorBase &other) const
Inequal comparison operator.
virtual ~OctreeDepthFirstIterator()
Empty deconstructor.
Abstract octree node class
Definition: octree_nodes.h:69
OctreeIteratorBase(OctreeT *octree_arg, unsigned int max_depth_arg=0)
Constructor.
bool isBranchNode() const
check if current node is a branch node
BranchContainer & getBranchContainer()
Method for retrieving the container from an octree branch node.
OctreeBreadthFirstIterator(unsigned int max_depth_arg=0)
Empty constructor.