Point Cloud Library (PCL)  1.8.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  }
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 = static_cast<unsigned long> (key.x) << (depth * 2)
344  | static_cast<unsigned long> (key.y) << (depth * 1)
345  | static_cast<unsigned long> (key.z) << (depth * 0);
346  }
347 
348  return id;
349  }
350 
351  protected:
352  /** \brief Reference to octree class. */
353  OctreeT* octree_;
354 
355  /** \brief Pointer to current iterator state. */
357 
358  /** \brief Maximum octree depth */
359  unsigned int max_octree_depth_;
360  };
361 
362  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
363  /** \brief @b Octree iterator class
364  * \note This class implements a forward iterator for traversing octrees in a depth-first manner.
365  * \ingroup octree
366  * \author Julius Kammerl (julius@kammerl.de)
367  */
368  template<typename OctreeT>
370  {
371 
372  public:
373 
376 
377  /** \brief Empty constructor.
378  * \param[in] max_depth_arg Depth limitation during traversal
379  */
380  explicit
381  OctreeDepthFirstIterator (unsigned int max_depth_arg = 0);
382 
383  /** \brief Constructor.
384  * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
385  * \param[in] max_depth_arg Depth limitation during traversal
386  */
387  explicit
388  OctreeDepthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0);
389 
390  /** \brief Empty deconstructor. */
391  virtual
393 
394  /** \brief Copy operator.
395  * \param[in] src the iterator to copy into this
396  */
398  operator = (const OctreeDepthFirstIterator& src)
399  {
400 
402 
403  stack_ = src.stack_;
404 
405  if (stack_.size())
406  {
407  this->current_state_ = &stack_.back();
408  } else
409  {
410  this->current_state_ = 0;
411  }
412 
413  return (*this);
414  }
415 
416  /** \brief Reset the iterator to the root node of the octree
417  */
418  virtual void
419  reset ();
420 
421  /** \brief Preincrement operator.
422  * \note recursively step to next octree node
423  */
425  operator++ ();
426 
427  /** \brief postincrement operator.
428  * \note recursively step to next octree node
429  */
431  operator++ (int)
432  {
433  OctreeDepthFirstIterator _Tmp = *this;
434  ++*this;
435  return (_Tmp);
436  }
437 
438  /** \brief Skip all child voxels of current node and return to parent node.
439  */
440  void
441  skipChildVoxels ();
442 
443  protected:
444  /** Stack structure. */
445  std::vector<IteratorState> stack_;
446  };
447 
448  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
449  /** \brief @b Octree iterator class
450  * \note This class implements a forward iterator for traversing octrees in a breadth-first manner.
451  * \ingroup octree
452  * \author Julius Kammerl (julius@kammerl.de)
453  */
454  template<typename OctreeT>
456  {
457  // public typedefs
460 
461 
462  public:
463  /** \brief Empty constructor.
464  * \param[in] max_depth_arg Depth limitation during traversal
465  */
466  explicit
467  OctreeBreadthFirstIterator (unsigned int max_depth_arg = 0);
468 
469  /** \brief Constructor.
470  * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
471  * \param[in] max_depth_arg Depth limitation during traversal
472  */
473  explicit
474  OctreeBreadthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0);
475 
476  /** \brief Empty deconstructor. */
477  virtual
479 
480  /** \brief Copy operator.
481  * \param[in] src the iterator to copy into this
482  */
484  operator = (const OctreeBreadthFirstIterator& src)
485  {
486 
488 
489  FIFO_ = src.FIFO_;
490 
491  if (FIFO_.size())
492  {
493  this->current_state_ = &FIFO_.front();
494  } else
495  {
496  this->current_state_ = 0;
497  }
498 
499  return (*this);
500  }
501 
502  /** \brief Reset the iterator to the root node of the octree
503  */
504  void
505  reset ();
506 
507  /** \brief Preincrement operator.
508  * \note step to next octree node
509  */
511  operator++ ();
512 
513  /** \brief postincrement operator.
514  * \note step to next octree node
515  */
517  operator++ (int)
518  {
519  OctreeBreadthFirstIterator _Tmp = *this;
520  ++*this;
521  return (_Tmp);
522  }
523 
524  protected:
525  /** FIFO list */
526  std::deque<IteratorState> FIFO_;
527  };
528 
529  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
530  /** \brief Octree leaf node iterator class
531  * \note This class implements a forward iterator for traversing the leaf nodes of an octree data structure.
532  * \ingroup octree
533  * \author Julius Kammerl (julius@kammerl.de)
534  */
535  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
536  template<typename OctreeT>
538  {
541 
542  public:
543  /** \brief Empty constructor.
544  * \param[in] max_depth_arg Depth limitation during traversal
545  */
546  explicit
547  OctreeLeafNodeIterator (unsigned int max_depth_arg = 0) :
548  OctreeDepthFirstIterator<OctreeT> (max_depth_arg)
549  {
550  reset ();
551  }
552 
553  /** \brief Constructor.
554  * \param[in] octree_arg Octree to be iterated. Initially the iterator is set to its root node.
555  * \param[in] max_depth_arg Depth limitation during traversal
556  */
557  explicit
558  OctreeLeafNodeIterator (OctreeT* octree_arg, unsigned int max_depth_arg = 0) :
559  OctreeDepthFirstIterator<OctreeT> (octree_arg, max_depth_arg)
560  {
561  reset ();
562  }
563 
564  /** \brief Empty deconstructor. */
565  virtual
567  {
568  }
569 
570  /** \brief Reset the iterator to the root node of the octree
571  */
572  inline void
573  reset ()
574  {
576  this->operator++ ();
577  }
578 
579  /** \brief Preincrement operator.
580  * \note recursively step to next octree leaf node
581  */
582  inline OctreeLeafNodeIterator&
583  operator++ ()
584  {
585  do
586  {
588  } while ((this->current_state_) && (this->current_state_->node_->getNodeType () != LEAF_NODE));
589 
590  return (*this);
591  }
592 
593  /** \brief postincrement operator.
594  * \note step to next octree node
595  */
597  operator++ (int)
598  {
599  OctreeLeafNodeIterator _Tmp = *this;
600  ++*this;
601  return (_Tmp);
602  }
603 
604  /** \brief *operator.
605  * \return pointer to the current octree leaf node
606  */
607  OctreeNode*
608  operator* () const
609  {
610  // return designated object
611  OctreeNode* ret = 0;
612 
613  if (this->current_state_ && (this->current_state_->node_->getNodeType () == LEAF_NODE))
614  ret = this->current_state_->node_;
615  return (ret);
616  }
617  };
618 
619  }
620 }
621 
622 #endif
623 
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.