Point Cloud Library (PCL)  1.10.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 <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 (std::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
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::octree::OctreeKey::pushBranch
void pushBranch(unsigned char childIndex)
push a child node to the octree key
Definition: octree_key.h:114
pcl::octree::OctreeBreadthFirstIterator::OctreeBreadthFirstIterator
OctreeBreadthFirstIterator(unsigned int max_depth_arg=0)
Empty constructor.
Definition: octree_iterator.hpp:174
pcl::octree::OctreeLeafNodeBreadthFirstIterator::reset
void reset()
Reset the iterator to the first leaf in the breadth first way.
Definition: octree_iterator.hpp:353
pcl::octree::OctreeNode::getNodeType
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
pcl::octree::OctreeKey::popBranch
void popBranch()
pop child node from octree key
Definition: octree_key.h:124
pcl::octree::OctreeKey
Octree key class
Definition: octree_key.h:50
pcl::octree::OctreeLeafNodeBreadthFirstIterator::operator++
OctreeLeafNodeBreadthFirstIterator & operator++()
Preincrement operator.
Definition: octree_iterator.hpp:362
pcl::octree::OctreeLeafNodeBreadthFirstIterator::OctreeLeafNodeBreadthFirstIterator
OctreeLeafNodeBreadthFirstIterator(unsigned int max_depth_arg=0)
Empty constructor.
Definition: octree_iterator.hpp:325
pcl::octree::OctreeBreadthFirstIterator::reset
void reset()
Reset the iterator to the root node of the octree.
Definition: octree_iterator.hpp:196
pcl::octree::LEAF_NODE
Definition: octree_nodes.h:59
pcl::octree::BRANCH_NODE
Definition: octree_nodes.h:59
pcl::octree::OctreeDepthFirstIterator::skipChildVoxels
void skipChildVoxels()
Skip all child voxels of current node and return to parent node.
Definition: octree_iterator.hpp:95
pcl::octree::IteratorState
Definition: octree_iterator.h:62
pcl::octree::OctreeIteratorBase
Abstract octree iterator class
Definition: octree_iterator.h:75
pcl::octree::OctreeBreadthFirstIterator
Octree iterator class
Definition: octree_iterator.h:475
pcl::int8_t
std::int8_t int8_t
Definition: pcl_macros.h:93
pcl::octree::OctreeKey::z
std::uint32_t z
Definition: octree_key.h:154
pcl::octree::OctreeDepthFirstIterator::reset
virtual void reset()
Reset the iterator to the root node of the octree.
Definition: octree_iterator.hpp:68
pcl::octree::OctreeDepthFirstIterator::operator++
OctreeDepthFirstIterator & operator++()
Preincrement operator.
Definition: octree_iterator.hpp:121
pcl::octree::OctreeDepthFirstIterator
Octree iterator class
Definition: octree_iterator.h:367
pcl::octree::OctreeFixedDepthIterator::OctreeFixedDepthIterator
OctreeFixedDepthIterator()
Empty constructor.
Definition: octree_iterator.hpp:273
pcl::octree::OctreeLeafNodeBreadthFirstIterator
Octree leaf node iterator class.
Definition: octree_iterator.h:767
pcl::octree::OctreeBreadthFirstIterator::operator++
OctreeBreadthFirstIterator & operator++()
Preincrement operator.
Definition: octree_iterator.hpp:220
pcl::octree::IteratorState::depth_
unsigned int depth_
Definition: octree_iterator.h:65
pcl::octree::OctreeIteratorBase::BranchNode
typename OctreeT::BranchNode BranchNode
Definition: octree_iterator.h:81
pcl::octree::OctreeFixedDepthIterator::reset
void reset()
Reset the iterator to the first node at the current depth.
Definition: octree_iterator.h:644
pcl::octree::IteratorState::key_
OctreeKey key_
Definition: octree_iterator.h:64
pcl::octree::IteratorState::node_
OctreeNode * node_
Definition: octree_iterator.h:63
pcl::octree::OctreeKey::x
std::uint32_t x
Definition: octree_key.h:152
pcl::octree::OctreeDepthFirstIterator::OctreeDepthFirstIterator
OctreeDepthFirstIterator(unsigned int max_depth_arg=0)
Empty constructor.
Definition: octree_iterator.hpp:50
pcl::octree::OctreeKey::y
std::uint32_t y
Definition: octree_key.h:153
pcl::octree::OctreeIteratorBase::reset
void reset()
Reset iterator.
Definition: octree_iterator.h:155