Point Cloud Library (PCL)  1.8.1
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 namespace pcl
43 {
44  namespace octree
45  {
46  //////////////////////////////////////////////////////////////////////////////////////////////
47  template<typename OctreeT>
49  OctreeIteratorBase<OctreeT> (max_depth_arg), stack_ ()
50  {
51  // initialize iterator
52  this->reset ();
53  }
54 
55  //////////////////////////////////////////////////////////////////////////////////////////////
56  template<typename OctreeT>
57  OctreeDepthFirstIterator<OctreeT>::OctreeDepthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg) :
58  OctreeIteratorBase<OctreeT> (octree_arg, max_depth_arg), stack_ ()
59  {
60  // initialize iterator
61  this->reset ();
62  }
63 
64  //////////////////////////////////////////////////////////////////////////////////////////////
65  template<typename OctreeT>
67  {
68  }
69 
70  //////////////////////////////////////////////////////////////////////////////////////////////
71  template<typename OctreeT>
73  {
75 
76  if (this->octree_)
77  {
78  // allocate stack
79  stack_.reserve (this->max_octree_depth_);
80 
81  // empty stack
82  stack_.clear ();
83 
84  // pushing root node to stack
85  IteratorState stack_entry;
86  stack_entry.node_ = this->octree_->getRootNode ();
87  stack_entry.depth_ = 0;
88  stack_entry.key_.x = stack_entry.key_.y = stack_entry.key_.z = 0;
89 
90  stack_.push_back(stack_entry);
91 
92  this->current_state_ = &stack_.back();
93  }
94 
95  }
96 
97  //////////////////////////////////////////////////////////////////////////////////////////////
98  template<typename OctreeT>
100  {
101 
102  if (stack_.size ())
103  {
104  // current depth
105  unsigned char current_depth = stack_.back ().depth_;
106 
107  // pop from stack as long as we find stack elements with depth >= current depth
108  while (stack_.size () && (stack_.back ().depth_ >= current_depth))
109  stack_.pop_back ();
110 
111  if (stack_.size ())
112  {
113  this->current_state_ = &stack_.back();
114  } else
115  {
116  this->current_state_ = 0;
117  }
118  }
119 
120  }
121 
122  //////////////////////////////////////////////////////////////////////////////////////////////
123  template<typename OctreeT>
126  {
127 
128  if (stack_.size ())
129  {
130  // get stack element
131  IteratorState stack_entry = stack_.back ();
132  stack_.pop_back ();
133 
134  stack_entry.depth_ ++;
135  OctreeKey& current_key = stack_entry.key_;
136 
137  if ( (this->max_octree_depth_>=stack_entry.depth_) &&
138  (stack_entry.node_->getNodeType () == BRANCH_NODE) )
139  {
140  unsigned char child_idx;
141 
142  // current node is a branch node
143  BranchNode* current_branch =
144  static_cast<BranchNode*> (stack_entry.node_);
145 
146  // add all children to stack
147  for (child_idx = 0; child_idx < 8; ++child_idx)
148  {
149 
150  // if child exist
151 
152  if (this->octree_->branchHasChild(*current_branch, child_idx))
153  {
154  // add child to stack
155  current_key.pushBranch (child_idx);
156 
157  stack_entry.node_ = this->octree_->getBranchChildPtr(*current_branch, child_idx);
158 
159  stack_.push_back(stack_entry);
160 
161  current_key.popBranch();
162  }
163  }
164  }
165 
166  if (stack_.size ())
167  {
168  this->current_state_ = &stack_.back();
169  } else
170  {
171  this->current_state_ = 0;
172  }
173  }
174 
175  return (*this);
176  }
177 
178  //////////////////////////////////////////////////////////////////////////////////////////////
179  template<typename OctreeT>
181  OctreeIteratorBase<OctreeT> (max_depth_arg), FIFO_ ()
182  {
184 
185  // initialize iterator
186  this->reset ();
187  }
188 
189  //////////////////////////////////////////////////////////////////////////////////////////////
190  template<typename OctreeT>
191  OctreeBreadthFirstIterator<OctreeT>::OctreeBreadthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg) :
192  OctreeIteratorBase<OctreeT> (octree_arg, max_depth_arg), FIFO_ ()
193  {
195 
196  // initialize iterator
197  this->reset ();
198  }
199 
200  //////////////////////////////////////////////////////////////////////////////////////////////
201  template<typename OctreeT>
203  {
204  }
205 
206  //////////////////////////////////////////////////////////////////////////////////////////////
207  template<typename OctreeT>
209  {
211 
212  // init FIFO
213  FIFO_.clear ();
214 
215  if (this->octree_)
216  {
217  // pushing root node to stack
218  IteratorState FIFO_entry;
219  FIFO_entry.node_ = this->octree_->getRootNode ();
220  FIFO_entry.depth_ = 0;
221  FIFO_entry.key_.x = FIFO_entry.key_.y = FIFO_entry.key_.z = 0;
222 
223  FIFO_.push_back(FIFO_entry);
224 
225  this->current_state_ = &FIFO_.front();
226  }
227  }
228 
229  //////////////////////////////////////////////////////////////////////////////////////////////
230  template<typename OctreeT>
233  {
234 
235  if (FIFO_.size ())
236  {
237  // get stack element
238  IteratorState FIFO_entry = FIFO_.front ();
239  FIFO_.pop_front ();
240 
241  FIFO_entry.depth_ ++;
242  OctreeKey& current_key = FIFO_entry.key_;
243 
244  if ( (this->max_octree_depth_>=FIFO_entry.depth_) &&
245  (FIFO_entry.node_->getNodeType () == BRANCH_NODE) )
246  {
247  unsigned char child_idx;
248 
249  // current node is a branch node
250  BranchNode* current_branch =
251  static_cast<BranchNode*> (FIFO_entry.node_);
252 
253  // iterate over all children
254  for (child_idx = 0; child_idx < 8 ; ++child_idx)
255  {
256 
257  // if child exist
258  if (this->octree_->branchHasChild(*current_branch, child_idx))
259  {
260  // add child to stack
261  current_key.pushBranch (static_cast<unsigned char> (child_idx));
262 
263  FIFO_entry.node_ = this->octree_->getBranchChildPtr(*current_branch, child_idx);
264 
265  FIFO_.push_back(FIFO_entry);
266 
267  current_key.popBranch();
268  }
269  }
270  }
271 
272  if (FIFO_.size ())
273  {
274  this->current_state_ = &FIFO_.front();
275  } else
276  {
277  this->current_state_ = 0;
278  }
279 
280  }
281 
282  return (*this);
283  }
284  }
285 }
286 
287 #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:114
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.
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:51
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:104
virtual ~OctreeDepthFirstIterator()
Empty deconstructor.
OctreeBreadthFirstIterator(unsigned int max_depth_arg=0)
Empty constructor.