Point Cloud Library (PCL)  1.9.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 #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  unsigned char child_idx;
236 
237  // current node is a branch node
238  BranchNode* current_branch =
239  static_cast<BranchNode*> (FIFO_entry.node_);
240 
241  // iterate over all children
242  for (child_idx = 0; child_idx < 8 ; ++child_idx)
243  {
244 
245  // if child exist
246  if (this->octree_->branchHasChild(*current_branch, child_idx))
247  {
248  // add child to stack
249  current_key.pushBranch (static_cast<unsigned char> (child_idx));
250 
251  FIFO_entry.node_ = this->octree_->getBranchChildPtr(*current_branch, child_idx);
252 
253  FIFO_.push_back(FIFO_entry);
254 
255  current_key.popBranch();
256  }
257  }
258  }
259 
260  if (FIFO_.size ())
261  {
262  this->current_state_ = &FIFO_.front();
263  } else
264  {
265  this->current_state_ = 0;
266  }
267 
268  }
269 
270  return (*this);
271  }
272 
273  //////////////////////////////////////////////////////////////////////////////////////////////
274  template<typename OctreeT>
276  OctreeBreadthFirstIterator<OctreeT> (0u), fixed_depth_ (0u)
277  {}
278 
279  //////////////////////////////////////////////////////////////////////////////////////////////
280  template<typename OctreeT>
281  OctreeFixedDepthIterator<OctreeT>::OctreeFixedDepthIterator (OctreeT* octree_arg, unsigned int fixed_depth_arg) :
282  OctreeBreadthFirstIterator<OctreeT> (octree_arg, fixed_depth_arg), fixed_depth_ (fixed_depth_arg)
283  {
284  this->reset (fixed_depth_arg);
285  }
286 
287  //////////////////////////////////////////////////////////////////////////////////////////////
288  template<typename OctreeT>
289  void OctreeFixedDepthIterator<OctreeT>::reset (unsigned int fixed_depth_arg)
290  {
291  // Set the desired depth to walk through
292  fixed_depth_ = fixed_depth_arg;
293 
294  if (!this->octree_)
295  {
296  return;
297  }
298 
299  // If I'm nowhere, reset
300  // If I'm somewhere and I can't guarantee I'm before the first node, reset
301  if ((!this->current_state_) || (fixed_depth_ <= this->getCurrentOctreeDepth ()))
303 
304  if (this->octree_->getTreeDepth () < fixed_depth_)
305  {
306  PCL_WARN ("[pcl::octree::FixedDepthIterator] The requested fixed depth was bigger than the octree's depth.\n");
307  PCL_WARN ("[pcl::octree::FixedDepthIterator] fixed_depth = %d (instead of %d)\n", this->octree_->getTreeDepth (), fixed_depth_);
308  }
309 
310  // By default for the parent class OctreeBreadthFirstIterator, if the
311  // depth argument is equal to 0, the iterator would run over every node.
312  // For the OctreeFixedDepthIterator, whatever the depth ask, set the
313  // max_octree_depth_ accordingly
314  this->max_octree_depth_ = std::min (fixed_depth_, this->octree_->getTreeDepth ());
315 
316  // Restore previous state in case breath first iterator had child nodes already set up
317  if (FIFO_.size ())
318  this->current_state_ = &FIFO_.front ();
319 
320  // Iterate all the way to the desired level
321  while (this->current_state_ && (this->getCurrentOctreeDepth () != fixed_depth_))
323  }
324 
325  //////////////////////////////////////////////////////////////////////////////////////////////
326  template<typename OctreeT>
328  OctreeBreadthFirstIterator<OctreeT> (max_depth_arg)
329  {
330  reset ();
331  }
332 
333  //////////////////////////////////////////////////////////////////////////////////////////////
334  template<typename OctreeT>
336  OctreeBreadthFirstIterator<OctreeT> (octree_arg, max_depth_arg)
337  {
338  reset ();
339  }
340 
341  //////////////////////////////////////////////////////////////////////////////////////////////
342  template<typename OctreeT>
344  unsigned int max_depth_arg,
345  IteratorState* current_state,
346  const std::deque<IteratorState>& fifo)
347  : OctreeBreadthFirstIterator<OctreeT> (octree_arg,
348  max_depth_arg,
349  current_state,
350  fifo)
351  {}
352 
353  //////////////////////////////////////////////////////////////////////////////////////////////
354  template<typename OctreeT>
356  {
358  ++*this;
359  }
360 
361  //////////////////////////////////////////////////////////////////////////////////////////////
362  template<typename OctreeT>
365  {
366  do
367  {
369  } while ((this->current_state_) && (this->current_state_->node_->getNodeType () != LEAF_NODE));
370 
371  return (*this);
372  }
373 
374  //////////////////////////////////////////////////////////////////////////////////////////////
375  template<typename OctreeT>
378  {
380  ++*this;
381  return (_Tmp);
382  }
383  }
384 }
385 
386 #endif
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)
OctreeLeafNodeBreadthFirstIterator(unsigned int max_depth_arg=0)
Empty constructor.
OctreeT::BranchNode BranchNode
void popBranch()
pop child node from octree key
Definition: octree_key.h:123
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.
void reset()
Reset the iterator to the first leaf in the breadth first way.
Octree key class
Definition: octree_key.h:51
OctreeBreadthFirstIterator & operator++()
Preincrement operator.
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:113
OctreeLeafNodeBreadthFirstIterator & operator++()
Preincrement operator.
OctreeBreadthFirstIterator(unsigned int max_depth_arg=0)
Empty constructor.