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)
OctreeT::BranchNode BranchNode
void popBranch()
pop child node from octree key
Definition: octree_key.h:114
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.
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.