Point Cloud Library (PCL)  1.9.1-dev
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 <pcl/console/print.h>
43 
44 namespace pcl
45 {
46  namespace octree
47  {
48  //////////////////////////////////////////////////////////////////////////////////////////////
49  template<typename OctreeT>
51  OctreeIteratorBase<OctreeT> (max_depth_arg), stack_ ()
52  {
53  // initialize iterator
54  this->reset ();
55  }
56 
57  //////////////////////////////////////////////////////////////////////////////////////////////
58  template<typename OctreeT>
59  OctreeDepthFirstIterator<OctreeT>::OctreeDepthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg) :
60  OctreeIteratorBase<OctreeT> (octree_arg, max_depth_arg), stack_ ()
61  {
62  // initialize iterator
63  this->reset ();
64  }
65 
66  //////////////////////////////////////////////////////////////////////////////////////////////
67  template<typename OctreeT>
69  {
71 
72  if (this->octree_)
73  {
74  // allocate stack
75  stack_.reserve (this->max_octree_depth_);
76 
77  // empty stack
78  stack_.clear ();
79 
80  // pushing root node to stack
81  IteratorState stack_entry;
82  stack_entry.node_ = this->octree_->getRootNode ();
83  stack_entry.depth_ = 0;
84  stack_entry.key_.x = stack_entry.key_.y = stack_entry.key_.z = 0;
85 
86  stack_.push_back(stack_entry);
87 
88  this->current_state_ = &stack_.back();
89  }
90 
91  }
92 
93  //////////////////////////////////////////////////////////////////////////////////////////////
94  template<typename OctreeT>
96  {
97 
98  if (stack_.size ())
99  {
100  // current depth
101  unsigned char current_depth = stack_.back ().depth_;
102 
103  // pop from stack as long as we find stack elements with depth >= current depth
104  while (stack_.size () && (stack_.back ().depth_ >= current_depth))
105  stack_.pop_back ();
106 
107  if (stack_.size ())
108  {
109  this->current_state_ = &stack_.back();
110  } else
111  {
112  this->current_state_ = 0;
113  }
114  }
115 
116  }
117 
118  //////////////////////////////////////////////////////////////////////////////////////////////
119  template<typename OctreeT>
122  {
123 
124  if (stack_.size ())
125  {
126  // get stack element
127  IteratorState stack_entry = stack_.back ();
128  stack_.pop_back ();
129 
130  stack_entry.depth_ ++;
131  OctreeKey& current_key = stack_entry.key_;
132 
133  if ( (this->max_octree_depth_>=stack_entry.depth_) &&
134  (stack_entry.node_->getNodeType () == BRANCH_NODE) )
135  {
136  // current node is a branch node
137  BranchNode* current_branch =
138  static_cast<BranchNode*> (stack_entry.node_);
139 
140  // add all children to stack
141  for (int8_t i = 7; i >= 0; --i)
142  {
143  const unsigned char child_idx = (unsigned char) i;
144 
145  // if child exist
146  if (this->octree_->branchHasChild(*current_branch, child_idx))
147  {
148  // add child to stack
149  current_key.pushBranch (child_idx);
150 
151  stack_entry.node_ = this->octree_->getBranchChildPtr(*current_branch, child_idx);
152 
153  stack_.push_back(stack_entry);
154 
155  current_key.popBranch();
156  }
157  }
158  }
159 
160  if (stack_.size ())
161  {
162  this->current_state_ = &stack_.back();
163  } else
164  {
165  this->current_state_ = 0;
166  }
167  }
168 
169  return (*this);
170  }
171 
172  //////////////////////////////////////////////////////////////////////////////////////////////
173  template<typename OctreeT>
175  OctreeIteratorBase<OctreeT> (max_depth_arg), FIFO_ ()
176  {
178 
179  // initialize iterator
180  this->reset ();
181  }
182 
183  //////////////////////////////////////////////////////////////////////////////////////////////
184  template<typename OctreeT>
185  OctreeBreadthFirstIterator<OctreeT>::OctreeBreadthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg) :
186  OctreeIteratorBase<OctreeT> (octree_arg, max_depth_arg), FIFO_ ()
187  {
189 
190  // initialize iterator
191  this->reset ();
192  }
193 
194  //////////////////////////////////////////////////////////////////////////////////////////////
195  template<typename OctreeT>
197  {
199 
200  // init FIFO
201  FIFO_.clear ();
202 
203  if (this->octree_)
204  {
205  // pushing root node to stack
206  IteratorState FIFO_entry;
207  FIFO_entry.node_ = this->octree_->getRootNode ();
208  FIFO_entry.depth_ = 0;
209  FIFO_entry.key_.x = FIFO_entry.key_.y = FIFO_entry.key_.z = 0;
210 
211  FIFO_.push_back(FIFO_entry);
212 
213  this->current_state_ = &FIFO_.front();
214  }
215  }
216 
217  //////////////////////////////////////////////////////////////////////////////////////////////
218  template<typename OctreeT>
221  {
222 
223  if (FIFO_.size ())
224  {
225  // get stack element
226  IteratorState FIFO_entry = FIFO_.front ();
227  FIFO_.pop_front ();
228 
229  FIFO_entry.depth_ ++;
230  OctreeKey& current_key = FIFO_entry.key_;
231 
232  if ( (this->max_octree_depth_>=FIFO_entry.depth_) &&
233  (FIFO_entry.node_->getNodeType () == BRANCH_NODE) )
234  {
235  // current node is a branch node
236  BranchNode* current_branch =
237  static_cast<BranchNode*> (FIFO_entry.node_);
238 
239  // iterate over all children
240  for (unsigned char child_idx = 0; child_idx < 8 ; ++child_idx)
241  {
242 
243  // if child exist
244  if (this->octree_->branchHasChild(*current_branch, child_idx))
245  {
246  // add child to stack
247  current_key.pushBranch (static_cast<unsigned char> (child_idx));
248 
249  FIFO_entry.node_ = this->octree_->getBranchChildPtr(*current_branch, child_idx);
250 
251  FIFO_.push_back(FIFO_entry);
252 
253  current_key.popBranch();
254  }
255  }
256  }
257 
258  if (FIFO_.size ())
259  {
260  this->current_state_ = &FIFO_.front();
261  } else
262  {
263  this->current_state_ = 0;
264  }
265 
266  }
267 
268  return (*this);
269  }
270 
271  //////////////////////////////////////////////////////////////////////////////////////////////
272  template<typename OctreeT>
274  OctreeBreadthFirstIterator<OctreeT> (0u), fixed_depth_ (0u)
275  {}
276 
277  //////////////////////////////////////////////////////////////////////////////////////////////
278  template<typename OctreeT>
279  OctreeFixedDepthIterator<OctreeT>::OctreeFixedDepthIterator (OctreeT* octree_arg, unsigned int fixed_depth_arg) :
280  OctreeBreadthFirstIterator<OctreeT> (octree_arg, fixed_depth_arg), fixed_depth_ (fixed_depth_arg)
281  {
282  this->reset (fixed_depth_arg);
283  }
284 
285  //////////////////////////////////////////////////////////////////////////////////////////////
286  template<typename OctreeT>
287  void OctreeFixedDepthIterator<OctreeT>::reset (unsigned int fixed_depth_arg)
288  {
289  // Set the desired depth to walk through
290  fixed_depth_ = fixed_depth_arg;
291 
292  if (!this->octree_)
293  {
294  return;
295  }
296 
297  // If I'm nowhere, reset
298  // If I'm somewhere and I can't guarantee I'm before the first node, reset
299  if ((!this->current_state_) || (fixed_depth_ <= this->getCurrentOctreeDepth ()))
301 
302  if (this->octree_->getTreeDepth () < fixed_depth_)
303  {
304  PCL_WARN ("[pcl::octree::FixedDepthIterator] The requested fixed depth was bigger than the octree's depth.\n");
305  PCL_WARN ("[pcl::octree::FixedDepthIterator] fixed_depth = %d (instead of %d)\n", this->octree_->getTreeDepth (), fixed_depth_);
306  }
307 
308  // By default for the parent class OctreeBreadthFirstIterator, if the
309  // depth argument is equal to 0, the iterator would run over every node.
310  // For the OctreeFixedDepthIterator, whatever the depth ask, set the
311  // max_octree_depth_ accordingly
312  this->max_octree_depth_ = std::min (fixed_depth_, this->octree_->getTreeDepth ());
313 
314  // Restore previous state in case breath first iterator had child nodes already set up
315  if (FIFO_.size ())
316  this->current_state_ = &FIFO_.front ();
317 
318  // Iterate all the way to the desired level
319  while (this->current_state_ && (this->getCurrentOctreeDepth () != fixed_depth_))
321  }
322 
323  //////////////////////////////////////////////////////////////////////////////////////////////
324  template<typename OctreeT>
326  OctreeBreadthFirstIterator<OctreeT> (max_depth_arg)
327  {
328  reset ();
329  }
330 
331  //////////////////////////////////////////////////////////////////////////////////////////////
332  template<typename OctreeT>
334  OctreeBreadthFirstIterator<OctreeT> (octree_arg, max_depth_arg)
335  {
336  reset ();
337  }
338 
339  //////////////////////////////////////////////////////////////////////////////////////////////
340  template<typename OctreeT>
342  unsigned int max_depth_arg,
343  IteratorState* current_state,
344  const std::deque<IteratorState>& fifo)
345  : OctreeBreadthFirstIterator<OctreeT> (octree_arg,
346  max_depth_arg,
347  current_state,
348  fifo)
349  {}
350 
351  //////////////////////////////////////////////////////////////////////////////////////////////
352  template<typename OctreeT>
354  {
356  ++*this;
357  }
358 
359  //////////////////////////////////////////////////////////////////////////////////////////////
360  template<typename OctreeT>
363  {
364  do
365  {
367  } while ((this->current_state_) && (this->current_state_->node_->getNodeType () != LEAF_NODE));
368 
369  return (*this);
370  }
371 
372  //////////////////////////////////////////////////////////////////////////////////////////////
373  template<typename OctreeT>
376  {
378  ++*this;
379  return (_Tmp);
380  }
381  }
382 }
383 
384 #endif
void reset()
Reset iterator.
void reset()
Reset the iterator to the first node at the current depth.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
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.
OctreeLeafNodeBreadthFirstIterator(unsigned int max_depth_arg=0)
Empty constructor.
std::vector< IteratorState > stack_
Stack structure.
IteratorState * current_state_
Pointer to current iterator state.
void popBranch()
pop child node from octree key
Definition: octree_key.h:122
unsigned int max_octree_depth_
Maximum octree depth.
unsigned int fixed_depth_
Given level of the node to be iterated.
void reset()
Reset the iterator to the root node of the octree.
typename OctreeT::BranchNode BranchNode
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.
void reset()
Reset the iterator to the first leaf in the breadth first way.
Octree key class
Definition: octree_key.h:50
unsigned int getCurrentOctreeDepth() const
Get the current depth level of octree.
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:112
OctreeLeafNodeBreadthFirstIterator & operator++()
Preincrement operator.
OctreeBreadthFirstIterator(unsigned int max_depth_arg=0)
Empty constructor.