Point Cloud Library (PCL)  1.7.1
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