Point Cloud Library (PCL)  1.8.0
octree_iterator.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, 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_HPP_
40 #define PCL_OCTREE_ITERATOR_HPP_
41 
42 #include <vector>
43 #include <assert.h>
44 
45 #include <pcl/common/common.h>
46 
47 namespace pcl
48 {
49  namespace octree
50  {
51  //////////////////////////////////////////////////////////////////////////////////////////////
52  template<typename OctreeT>
54  OctreeIteratorBase<OctreeT> (max_depth_arg), stack_ ()
55  {
56  // initialize iterator
57  this->reset ();
58  }
59 
60  //////////////////////////////////////////////////////////////////////////////////////////////
61  template<typename OctreeT>
62  OctreeDepthFirstIterator<OctreeT>::OctreeDepthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg) :
63  OctreeIteratorBase<OctreeT> (octree_arg, max_depth_arg), stack_ ()
64  {
65  // initialize iterator
66  this->reset ();
67  }
68 
69  //////////////////////////////////////////////////////////////////////////////////////////////
70  template<typename OctreeT>
72  {
73  }
74 
75  //////////////////////////////////////////////////////////////////////////////////////////////
76  template<typename OctreeT>
78  {
80 
81  if (this->octree_)
82  {
83  // allocate stack
84  stack_.reserve (this->max_octree_depth_);
85 
86  // empty stack
87  stack_.clear ();
88 
89  // pushing root node to stack
90  IteratorState stack_entry;
91  stack_entry.node_ = this->octree_->getRootNode ();
92  stack_entry.depth_ = 0;
93  stack_entry.key_.x = stack_entry.key_.y = stack_entry.key_.z = 0;
94 
95  stack_.push_back(stack_entry);
96 
97  this->current_state_ = &stack_.back();
98  }
99 
100  }
101 
102  //////////////////////////////////////////////////////////////////////////////////////////////
103  template<typename OctreeT>
105  {
106 
107  if (stack_.size ())
108  {
109  // current depth
110  unsigned char current_depth = stack_.back ().depth_;
111 
112  // pop from stack as long as we find stack elements with depth >= current depth
113  while (stack_.size () && (stack_.back ().depth_ >= current_depth))
114  stack_.pop_back ();
115 
116  if (stack_.size ())
117  {
118  this->current_state_ = &stack_.back();
119  } else
120  {
121  this->current_state_ = 0;
122  }
123  }
124 
125  }
126 
127  //////////////////////////////////////////////////////////////////////////////////////////////
128  template<typename OctreeT>
131  {
132 
133  if (stack_.size ())
134  {
135  // get stack element
136  IteratorState stack_entry = stack_.back ();
137  stack_.pop_back ();
138 
139  stack_entry.depth_ ++;
140  OctreeKey& current_key = stack_entry.key_;
141 
142  if ( (this->max_octree_depth_>=stack_entry.depth_) &&
143  (stack_entry.node_->getNodeType () == BRANCH_NODE) )
144  {
145  unsigned char child_idx;
146 
147  // current node is a branch node
148  BranchNode* current_branch =
149  static_cast<BranchNode*> (stack_entry.node_);
150 
151  // add all children to stack
152  for (child_idx = 0; child_idx < 8; ++child_idx)
153  {
154 
155  // if child exist
156 
157  if (this->octree_->branchHasChild(*current_branch, child_idx))
158  {
159  // add child to stack
160  current_key.pushBranch (child_idx);
161 
162  stack_entry.node_ = this->octree_->getBranchChildPtr(*current_branch, child_idx);
163 
164  stack_.push_back(stack_entry);
165 
166  current_key.popBranch();
167  }
168  }
169  }
170 
171  if (stack_.size ())
172  {
173  this->current_state_ = &stack_.back();
174  } else
175  {
176  this->current_state_ = 0;
177  }
178  }
179 
180  return (*this);
181  }
182 
183  //////////////////////////////////////////////////////////////////////////////////////////////
184  template<typename OctreeT>
186  OctreeIteratorBase<OctreeT> (max_depth_arg), FIFO_ ()
187  {
189 
190  // initialize iterator
191  this->reset ();
192  }
193 
194  //////////////////////////////////////////////////////////////////////////////////////////////
195  template<typename OctreeT>
196  OctreeBreadthFirstIterator<OctreeT>::OctreeBreadthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg) :
197  OctreeIteratorBase<OctreeT> (octree_arg, max_depth_arg), FIFO_ ()
198  {
200 
201  // initialize iterator
202  this->reset ();
203  }
204 
205  //////////////////////////////////////////////////////////////////////////////////////////////
206  template<typename OctreeT>
208  {
209  }
210 
211  //////////////////////////////////////////////////////////////////////////////////////////////
212  template<typename OctreeT>
214  {
216 
217  // init FIFO
218  FIFO_.clear ();
219 
220  if (this->octree_)
221  {
222  // pushing root node to stack
223  IteratorState FIFO_entry;
224  FIFO_entry.node_ = this->octree_->getRootNode ();
225  FIFO_entry.depth_ = 0;
226  FIFO_entry.key_.x = FIFO_entry.key_.y = FIFO_entry.key_.z = 0;
227 
228  FIFO_.push_back(FIFO_entry);
229 
230  this->current_state_ = &FIFO_.front();
231  }
232  }
233 
234  //////////////////////////////////////////////////////////////////////////////////////////////
235  template<typename OctreeT>
238  {
239 
240  if (FIFO_.size ())
241  {
242  // get stack element
243  IteratorState FIFO_entry = FIFO_.front ();
244  FIFO_.pop_front ();
245 
246  FIFO_entry.depth_ ++;
247  OctreeKey& current_key = FIFO_entry.key_;
248 
249  if ( (this->max_octree_depth_>=FIFO_entry.depth_) &&
250  (FIFO_entry.node_->getNodeType () == BRANCH_NODE) )
251  {
252  unsigned char child_idx;
253 
254  // current node is a branch node
255  BranchNode* current_branch =
256  static_cast<BranchNode*> (FIFO_entry.node_);
257 
258  // iterate over all children
259  for (child_idx = 0; child_idx < 8 ; ++child_idx)
260  {
261 
262  // if child exist
263  if (this->octree_->branchHasChild(*current_branch, child_idx))
264  {
265  // add child to stack
266  current_key.pushBranch (static_cast<unsigned char> (child_idx));
267 
268  FIFO_entry.node_ = this->octree_->getBranchChildPtr(*current_branch, child_idx);
269 
270  FIFO_.push_back(FIFO_entry);
271 
272  current_key.popBranch();
273  }
274  }
275  }
276 
277  if (FIFO_.size ())
278  {
279  this->current_state_ = &FIFO_.front();
280  } else
281  {
282  this->current_state_ = 0;
283  }
284 
285  }
286 
287  return (*this);
288  }
289  }
290 }
291 
292 #endif
void reset()
Reset iterator.
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
std::deque< IteratorState > FIFO_
FIFO list.
std::vector< IteratorState > stack_
Stack structure.
IteratorState * current_state_
Pointer to current iterator state.
OctreeT::BranchNode BranchNode
void popBranch()
pop child node from octree key
Definition: octree_key.h:116
unsigned int max_octree_depth_
Maximum octree depth.
Define standard C methods and C++ classes that are common to all methods.
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.
OctreeDepthFirstIterator(unsigned int max_depth_arg=0)
Empty constructor.
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
OctreeBreadthFirstIterator & operator++()
Preincrement operator.
OctreeT * octree_
Reference to octree class.
OctreeDepthFirstIterator & operator++()
Preincrement operator.
Abstract octree iterator class
void pushBranch(unsigned char childIndex)
push a child node to the octree key
Definition: octree_key.h:106
virtual ~OctreeDepthFirstIterator()
Empty deconstructor.
OctreeBreadthFirstIterator(unsigned int max_depth_arg=0)
Empty constructor.