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
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::octree::OctreeKey::z
uint32_t z
Definition: octree_key.h:153
pcl::octree::OctreeKey::pushBranch
void pushBranch(unsigned char childIndex)
push a child node to the octree key
Definition: octree_key.h:113
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:123
pcl::octree::OctreeKey
Octree key class
Definition: octree_key.h:51
pcl::octree::LEAF_NODE
@ LEAF_NODE
Definition: octree_nodes.h:60
pcl::octree::BRANCH_NODE
@ BRANCH_NODE
Definition: octree_nodes.h:60
pcl::octree::IteratorState
Definition: octree_iterator.h:63
pcl::octree::OctreeIteratorBase
Abstract octree iterator class
Definition: octree_iterator.h:76
pcl::octree::OctreeBreadthFirstIterator
Octree iterator class
Definition: octree_iterator.h:476
pcl::octree::OctreeDepthFirstIterator
Octree iterator class
Definition: octree_iterator.h:368
pcl::octree::OctreeFixedDepthIterator
Octree iterator class
Definition: octree_iterator.h:579
pcl::octree::OctreeLeafNodeBreadthFirstIterator
Octree leaf node iterator class.
Definition: octree_iterator.h:768
pcl::octree::IteratorState::depth_
unsigned int depth_
Definition: octree_iterator.h:66
pcl::octree::OctreeIteratorBase< pcl::octree::Octree2BufBase >::BranchNode
pcl::octree::Octree2BufBase ::BranchNode BranchNode
Definition: octree_iterator.h:82
pcl::octree::IteratorState::key_
OctreeKey key_
Definition: octree_iterator.h:65
pcl::octree::OctreeKey::y
uint32_t y
Definition: octree_key.h:152
pcl::octree::IteratorState::node_
OctreeNode * node_
Definition: octree_iterator.h:64
pcl::octree::OctreeDepthFirstIterator::OctreeDepthFirstIterator
OctreeDepthFirstIterator(unsigned int max_depth_arg=0)
Empty constructor.
Definition: octree_iterator.hpp:50
pcl::octree::OctreeKey::x
uint32_t x
Definition: octree_key.h:151