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&
122  operator = (const OctreeIteratorBase& src)
123  {
124  octree_ = src.octree_;
125  current_state_ = src.current_state_;
126  max_octree_depth_ = src.max_octree_depth_;
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  */
396  operator = (const OctreeDepthFirstIterator& src)
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  */
429  operator++ (int)
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  */
482  operator = (const OctreeBreadthFirstIterator& src)
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  */
515  operator++ (int)
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&
581  operator++ ()
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  */
595  operator++ (int)
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 
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.
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.
OctreeT::LeafContainer LeafContainer
std::deque< IteratorState > FIFO_
FIFO list.
std::vector< IteratorState > stack_
Stack structure.
IteratorState * current_state_
Pointer to current iterator state.
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.
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.
bool isLeafNode() const
check if current node is a branch node
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.
OctreeT * octree_
Reference to octree class.
OctreeDepthFirstIterator & operator++()
Preincrement operator.
Abstract octree iterator class
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.
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.