Point Cloud Library (PCL)  1.7.2
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  }
117 
118  /** \brief Copy operator.
119  * \param[in] src the iterator to copy into this
120  */
121  inline OctreeIteratorBase&
123  {
124  octree_ = src.octree_;
127  return (*this);
128  }
129 
130  /** \brief Empty deconstructor. */
131  virtual
133  {
134  }
135 
136  /** \brief Equal comparison operator
137  * \param[in] other OctreeIteratorBase to compare with
138  */
139  bool operator==(const OctreeIteratorBase& other) const
140  {
141  return (( octree_ ==other.octree_) &&
142  ( current_state_ == other.current_state_) &&
143  ( max_octree_depth_ == other.max_octree_depth_) );
144  }
145 
146  /** \brief Inequal comparison operator
147  * \param[in] other OctreeIteratorBase to compare with
148  */
149  bool operator!=(const OctreeIteratorBase& other) const
150  {
151  return (( octree_ !=other.octree_) &&
152  ( current_state_ != other.current_state_) &&
153  ( max_octree_depth_ != other.max_octree_depth_) );
154  }
155 
156  /** \brief Reset iterator */
157  inline void reset ()
158  {
159  current_state_ = 0;
160  if (octree_ && (!max_octree_depth_))
161  {
162  max_octree_depth_ = octree_->getTreeDepth();
163  }
164  }
165 
166  /** \brief Get octree key for the current iterator octree node
167  * \return octree key of current node
168  */
169  inline const OctreeKey&
171  {
172  assert(octree_!=0);
173  assert(current_state_!=0);
174 
175  return (current_state_->key_);
176  }
177 
178  /** \brief Get the current depth level of octree
179  * \return depth level
180  */
181  inline unsigned int
183  {
184  assert(octree_!=0);
185  assert(current_state_!=0);
186 
187  return (current_state_->depth_);
188  }
189 
190  /** \brief Get the current octree node
191  * \return pointer to current octree node
192  */
193  inline OctreeNode*
195  {
196  assert(octree_!=0);
197  assert(current_state_!=0);
198 
199  return (current_state_->node_);
200  }
201 
202 
203  /** \brief check if current node is a branch node
204  * \return true if current node is a branch node, false otherwise
205  */
206  inline bool
207  isBranchNode () const
208  {
209  assert(octree_!=0);
210  assert(current_state_!=0);
211 
212  return (current_state_->node_->getNodeType () == BRANCH_NODE);
213  }
214 
215  /** \brief check if current node is a branch node
216  * \return true if current node is a branch node, false otherwise
217  */
218  inline bool
219  isLeafNode () const
220  {
221  assert(octree_!=0);
222  assert(current_state_!=0);
223 
224  return (current_state_->node_->getNodeType () == LEAF_NODE);
225  }
226 
227  /** \brief *operator.
228  * \return pointer to the current octree node
229  */
230  inline OctreeNode*
231  operator* () const
232  { // return designated object
233  if (octree_ && current_state_)
234  {
235  return (current_state_->node_);
236  } else
237  {
238  return 0;
239  }
240  }
241 
242  /** \brief Get bit pattern of children configuration of current node
243  * \return bit pattern (byte) describing the existence of 8 children of the current node
244  */
245  inline char
247  {
248  char ret = 0;
249 
250  assert(octree_!=0);
251  assert(current_state_!=0);
252 
253  if (isBranchNode ())
254  {
255 
256  // current node is a branch node
257  const BranchNode* current_branch = static_cast<const BranchNode*> (current_state_->node_);
258 
259  // get child configuration bit pattern
260  ret = octree_->getBranchBitPattern (*current_branch);
261 
262  }
263 
264  return (ret);
265  }
266 
267  /** \brief Method for retrieving a single leaf container from the octree leaf node
268  * \return Reference to container class of leaf node.
269  */
270  const LeafContainer&
272  {
273  assert(octree_!=0);
274  assert(current_state_!=0);
275  assert(this->isLeafNode());
276 
277  LeafNode* leaf_node = static_cast<LeafNode*>(current_state_->node_);
278 
279  return leaf_node->getContainer();
280  }
281 
282  /** \brief Method for retrieving a single leaf container from the octree leaf node
283  * \return Reference to container class of leaf node.
284  */
285  LeafContainer&
287  {
288  assert(octree_!=0);
289  assert(current_state_!=0);
290  assert(this->isLeafNode());
291 
292  LeafNode* leaf_node = static_cast<LeafNode*>(current_state_->node_);
293 
294  return leaf_node->getContainer();
295  }
296 
297  /** \brief Method for retrieving the container from an octree branch node
298  * \return BranchContainer.
299  */
300  const BranchContainer&
302  {
303  assert(octree_!=0);
304  assert(current_state_!=0);
305  assert(this->isBranchNode());
306 
307  BranchNode* branch_node = static_cast<BranchNode*>(current_state_->node_);
308 
309  return branch_node->getContainer();
310  }
311 
312  /** \brief Method for retrieving the container from an octree branch node
313  * \return BranchContainer.
314  */
315  BranchContainer&
317  {
318  assert(octree_!=0);
319  assert(current_state_!=0);
320  assert(this->isBranchNode());
321 
322  BranchNode* branch_node = static_cast<BranchNode*>(current_state_->node_);
323 
324  return branch_node->getContainer();
325  }
326 
327  /** \brief get a integer identifier for current node (note: identifier depends on tree depth).
328  * \return node id.
329  */
330  virtual unsigned long
331  getNodeID () const
332  {
333  unsigned long id = 0;
334 
335  assert(octree_!=0);
336  assert(current_state_!=0);
337 
338  if (current_state_)
339  {
340  const OctreeKey& key = getCurrentOctreeKey();
341  // calculate integer id with respect to octree key
342  unsigned int depth = octree_->getTreeDepth ();
343  id = key.x << (depth * 2) | key.y << (depth * 1) | key.z << (depth * 0);
344  }
345 
346  return id;
347  }
348 
349  protected:
350  /** \brief Reference to octree class. */
351  OctreeT* octree_;
352 
353  /** \brief Pointer to current iterator state. */
355 
356  /** \brief Maximum octree depth */
357  unsigned int max_octree_depth_;
358  };
359 
360  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
361  /** \brief @b Octree iterator class
362  * \note This class implements a forward iterator for traversing octrees in a depth-first manner.
363  * \ingroup octree
364  * \author Julius Kammerl (julius@kammerl.de)
365  */
366  template<typename OctreeT>
368  {
369 
370  public:
371 
374 
375  /** \brief Empty constructor.
376  * \param[in] max_depth_arg Depth limitation during traversal
377  */
378  explicit
379  OctreeDepthFirstIterator (unsigned int max_depth_arg = 0);
380 
381  /** \brief Constructor.
382  * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
383  * \param[in] max_depth_arg Depth limitation during traversal
384  */
385  explicit
386  OctreeDepthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0);
387 
388  /** \brief Empty deconstructor. */
389  virtual
391 
392  /** \brief Copy operator.
393  * \param[in] src the iterator to copy into this
394  */
397  {
398 
400 
401  stack_ = src.stack_;
402 
403  if (stack_.size())
404  {
405  this->current_state_ = &stack_.back();
406  } else
407  {
408  this->current_state_ = 0;
409  }
410 
411  return (*this);
412  }
413 
414  /** \brief Reset the iterator to the root node of the octree
415  */
416  virtual void
417  reset ();
418 
419  /** \brief Preincrement operator.
420  * \note recursively step to next octree node
421  */
423  operator++ ();
424 
425  /** \brief postincrement operator.
426  * \note recursively step to next octree node
427  */
430  {
431  OctreeDepthFirstIterator _Tmp = *this;
432  ++*this;
433  return (_Tmp);
434  }
435 
436  /** \brief Skip all child voxels of current node and return to parent node.
437  */
438  void
439  skipChildVoxels ();
440 
441  protected:
442  /** Stack structure. */
443  std::vector<IteratorState> stack_;
444  };
445 
446  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
447  /** \brief @b Octree iterator class
448  * \note This class implements a forward iterator for traversing octrees in a breadth-first manner.
449  * \ingroup octree
450  * \author Julius Kammerl (julius@kammerl.de)
451  */
452  template<typename OctreeT>
454  {
455  // public typedefs
458 
459 
460  public:
461  /** \brief Empty constructor.
462  * \param[in] max_depth_arg Depth limitation during traversal
463  */
464  explicit
465  OctreeBreadthFirstIterator (unsigned int max_depth_arg = 0);
466 
467  /** \brief Constructor.
468  * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
469  * \param[in] max_depth_arg Depth limitation during traversal
470  */
471  explicit
472  OctreeBreadthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0);
473 
474  /** \brief Empty deconstructor. */
475  virtual
477 
478  /** \brief Copy operator.
479  * \param[in] src the iterator to copy into this
480  */
483  {
484 
486 
487  FIFO_ = src.FIFO_;
488 
489  if (FIFO_.size())
490  {
491  this->current_state_ = &FIFO_.front();
492  } else
493  {
494  this->current_state_ = 0;
495  }
496 
497  return (*this);
498  }
499 
500  /** \brief Reset the iterator to the root node of the octree
501  */
502  void
503  reset ();
504 
505  /** \brief Preincrement operator.
506  * \note step to next octree node
507  */
509  operator++ ();
510 
511  /** \brief postincrement operator.
512  * \note step to next octree node
513  */
516  {
517  OctreeBreadthFirstIterator _Tmp = *this;
518  ++*this;
519  return (_Tmp);
520  }
521 
522  protected:
523  /** FIFO list */
524  std::deque<IteratorState> FIFO_;
525  };
526 
527  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
528  /** \brief Octree leaf node iterator class
529  * \note This class implements a forward iterator for traversing the leaf nodes of an octree data structure.
530  * \ingroup octree
531  * \author Julius Kammerl (julius@kammerl.de)
532  */
533  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
534  template<typename OctreeT>
536  {
539 
540  public:
541  /** \brief Empty constructor.
542  * \param[in] max_depth_arg Depth limitation during traversal
543  */
544  explicit
545  OctreeLeafNodeIterator (unsigned int max_depth_arg = 0) :
546  OctreeDepthFirstIterator<OctreeT> (max_depth_arg)
547  {
548  reset ();
549  }
550 
551  /** \brief Constructor.
552  * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
553  * \param[in] max_depth_arg Depth limitation during traversal
554  */
555  explicit
556  OctreeLeafNodeIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0) :
557  OctreeDepthFirstIterator<OctreeT> (octree_arg, max_depth_arg)
558  {
559  reset ();
560  }
561 
562  /** \brief Empty deconstructor. */
563  virtual
565  {
566  }
567 
568  /** \brief Reset the iterator to the root node of the octree
569  */
570  inline void
571  reset ()
572  {
574  this->operator++ ();
575  }
576 
577  /** \brief Preincrement operator.
578  * \note recursively step to next octree leaf node
579  */
580  inline OctreeLeafNodeIterator&
582  {
583  do
584  {
586  } while ((this->current_state_) && (this->current_state_->node_->getNodeType () != LEAF_NODE));
587 
588  return (*this);
589  }
590 
591  /** \brief postincrement operator.
592  * \note step to next octree node
593  */
596  {
597  OctreeLeafNodeIterator _Tmp = *this;
598  ++*this;
599  return (_Tmp);
600  }
601 
602  /** \brief *operator.
603  * \return pointer to the current octree leaf node
604  */
605  OctreeNode*
606  operator* () const
607  {
608  // return designated object
609  OctreeNode* ret = 0;
610 
611  if (this->current_state_ && (this->current_state_->node_->getNodeType () == LEAF_NODE))
612  ret = this->current_state_->node_;
613  return (ret);
614  }
615  };
616 
617  }
618 }
619 
620 #endif
621 
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:71
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.