Point Cloud Library (PCL)  1.7.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 <vector>
43 #include <assert.h>
44 
45 #include <pcl/common/common.h>
46 
47 namespace pcl
48 {
49  namespace octree
50  {
51  //////////////////////////////////////////////////////////////////////////////////////////////
52  template<typename OctreeT>
54  OctreeIteratorBase<OctreeT> (max_depth_arg), stack_ ()
55  {
56  // initialize iterator
57  this->reset ();
58  }
59 
60  //////////////////////////////////////////////////////////////////////////////////////////////
61  template<typename OctreeT>
62  OctreeDepthFirstIterator<OctreeT>::OctreeDepthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg) :
63  OctreeIteratorBase<OctreeT> (octree_arg, max_depth_arg), stack_ ()
64  {
65  // initialize iterator
66  this->reset ();
67  }
68 
69  //////////////////////////////////////////////////////////////////////////////////////////////
70  template<typename OctreeT>
72  {
73  }
74 
75  //////////////////////////////////////////////////////////////////////////////////////////////
76  template<typename OctreeT>
78  {
80 
81  if (this->octree_)
82  {
83  // allocate stack
84  stack_.reserve (this->max_octree_depth_);
85 
86  // empty stack
87  stack_.clear ();
88 
89  // pushing root node to stack
90  IteratorState stack_entry;
91  stack_entry.node_ = this->octree_->getRootNode ();
92  stack_entry.depth_ = 0;
93  stack_entry.key_.x = stack_entry.key_.y = stack_entry.key_.z = 0;
94 
95  stack_.push_back(stack_entry);
96 
97  this->current_state_ = &stack_.back();
98  }
99 
100  }
101 
102  //////////////////////////////////////////////////////////////////////////////////////////////
103  template<typename OctreeT>
105  {
106 
107  if (stack_.size ())
108  {
109  // current depth
110  unsigned char current_depth = stack_.back ().depth_;
111 
112  // pop from stack as long as we find stack elements with depth >= current depth
113  while (stack_.size () && (stack_.back ().depth_ >= current_depth))
114  stack_.pop_back ();
115 
116  if (stack_.size ())
117  {
118  this->current_state_ = &stack_.back();
119  } else
120  {
121  this->current_state_ = 0;
122  }
123  }
124 
125  }
126 
127  //////////////////////////////////////////////////////////////////////////////////////////////
128  template<typename OctreeT>
131  {
132 
133  if (stack_.size ())
134  {
135  // get stack element
136  IteratorState stack_entry = stack_.back ();
137  stack_.pop_back ();
138 
139  stack_entry.depth_ ++;
140  OctreeKey& current_key = stack_entry.key_;
141 
142  if ( (this->max_octree_depth_>=stack_entry.depth_) &&
143  (stack_entry.node_->getNodeType () == BRANCH_NODE) )
144  {
145  unsigned char child_idx;
146 
147  // current node is a branch node
148  BranchNode* current_branch =
149  static_cast<BranchNode*> (stack_entry.node_);
150 
151  // add all children to stack
152  for (child_idx = 0; child_idx < 8; ++child_idx)
153  {
154 
155  // if child exist
156 
157  if (this->octree_->branchHasChild(*current_branch, child_idx))
158  {
159  // add child to stack
160  current_key.pushBranch (child_idx);
161 
162  stack_entry.node_ = this->octree_->getBranchChildPtr(*current_branch, child_idx);
163 
164  stack_.push_back(stack_entry);
165 
166  current_key.popBranch();
167  }
168  }
169  }
170 
171  if (stack_.size ())
172  {
173  this->current_state_ = &stack_.back();
174  } else
175  {
176  this->current_state_ = 0;
177  }
178  }
179 
180  return (*this);
181  }
182 
183  //////////////////////////////////////////////////////////////////////////////////////////////
184  template<typename OctreeT>
186  OctreeIteratorBase<OctreeT> (max_depth_arg), FIFO_ ()
187  {
189 
190  // initialize iterator
191  this->reset ();
192  }
193 
194  //////////////////////////////////////////////////////////////////////////////////////////////
195  template<typename OctreeT>
196  OctreeBreadthFirstIterator<OctreeT>::OctreeBreadthFirstIterator (OctreeT* octree_arg, unsigned int max_depth_arg) :
197  OctreeIteratorBase<OctreeT> (octree_arg, max_depth_arg), FIFO_ ()
198  {
200 
201  // initialize iterator
202  this->reset ();
203  }
204 
205  //////////////////////////////////////////////////////////////////////////////////////////////
206  template<typename OctreeT>
208  {
209  }
210 
211  //////////////////////////////////////////////////////////////////////////////////////////////
212  template<typename OctreeT>
214  {
216 
217  // init FIFO
218  FIFO_.clear ();
219 
220  if (this->octree_)
221  {
222  // pushing root node to stack
223  IteratorState FIFO_entry;
224  FIFO_entry.node_ = this->octree_->getRootNode ();
225  FIFO_entry.depth_ = 0;
226  FIFO_entry.key_.x = FIFO_entry.key_.y = FIFO_entry.key_.z = 0;
227 
228  FIFO_.push_back(FIFO_entry);
229 
230  this->current_state_ = &FIFO_.front();
231  }
232  }
233 
234  //////////////////////////////////////////////////////////////////////////////////////////////
235  template<typename OctreeT>
238  {
239 
240  if (FIFO_.size ())
241  {
242  // get stack element
243  IteratorState FIFO_entry = FIFO_.front ();
244  FIFO_.pop_front ();
245 
246  FIFO_entry.depth_ ++;
247  OctreeKey& current_key = FIFO_entry.key_;
248 
249  if ( (this->max_octree_depth_>=FIFO_entry.depth_) &&
250  (FIFO_entry.node_->getNodeType () == BRANCH_NODE) )
251  {
252  unsigned char child_idx;
253 
254  // current node is a branch node
255  BranchNode* current_branch =
256  static_cast<BranchNode*> (FIFO_entry.node_);
257 
258  // iterate over all children
259  for (child_idx = 0; child_idx < 8 ; ++child_idx)
260  {
261 
262  // if child exist
263  if (this->octree_->branchHasChild(*current_branch, child_idx))
264  {
265  // add child to stack
266  current_key.pushBranch (static_cast<unsigned char> (child_idx));
267 
268  FIFO_entry.node_ = this->octree_->getBranchChildPtr(*current_branch, child_idx);
269 
270  FIFO_.push_back(FIFO_entry);
271 
272  current_key.popBranch();
273  }
274  }
275  }
276 
277  if (FIFO_.size ())
278  {
279  this->current_state_ = &FIFO_.front();
280  } else
281  {
282  this->current_state_ = 0;
283  }
284 
285  }
286 
287  return (*this);
288  }
289  }
290 }
291 
292 #endif