Point Cloud Library (PCL)  1.10.1-dev
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 namespace octree {
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>
58  unsigned int max_depth_arg)
59 : OctreeIteratorBase<OctreeT>(octree_arg, max_depth_arg), stack_()
60 {
61  // initialize iterator
62  this->reset();
63 }
64 
65 //////////////////////////////////////////////////////////////////////////////////////////////
66 template <typename OctreeT>
67 void
69 {
71 
72  if (this->octree_) {
73  // allocate stack
74  stack_.reserve(this->max_octree_depth_);
75 
76  // empty stack
77  stack_.clear();
78 
79  // pushing root node to stack
80  IteratorState stack_entry;
81  stack_entry.node_ = this->octree_->getRootNode();
82  stack_entry.depth_ = 0;
83  stack_entry.key_.x = stack_entry.key_.y = stack_entry.key_.z = 0;
84 
85  stack_.push_back(stack_entry);
86 
87  this->current_state_ = &stack_.back();
88  }
89 }
90 
91 //////////////////////////////////////////////////////////////////////////////////////////////
92 template <typename OctreeT>
93 void
95 {
96 
97  if (stack_.size()) {
98  // current depth
99  unsigned char current_depth = stack_.back().depth_;
100 
101  // pop from stack as long as we find stack elements with depth >= current depth
102  while (stack_.size() && (stack_.back().depth_ >= current_depth))
103  stack_.pop_back();
104 
105  if (stack_.size()) {
106  this->current_state_ = &stack_.back();
107  }
108  else {
109  this->current_state_ = 0;
110  }
111  }
112 }
113 
114 //////////////////////////////////////////////////////////////////////////////////////////////
115 template <typename OctreeT>
118 {
119 
120  if (stack_.size()) {
121  // get stack element
122  IteratorState stack_entry = stack_.back();
123  stack_.pop_back();
124 
125  stack_entry.depth_++;
126 
127  if ((this->max_octree_depth_ >= stack_entry.depth_) &&
128  (stack_entry.node_->getNodeType() == BRANCH_NODE)) {
129  // current node is a branch node
130  BranchNode* current_branch = static_cast<BranchNode*>(stack_entry.node_);
131 
132  OctreeKey& current_key = stack_entry.key_;
133 
134  // add all children to stack
135  for (std::int8_t i = 7; i >= 0; --i) {
136  const unsigned char child_idx = (unsigned char)i;
137 
138  // if child exist
139  if (this->octree_->branchHasChild(*current_branch, child_idx)) {
140  // add child to stack
141  current_key.pushBranch(child_idx);
142 
143  stack_entry.node_ =
144  this->octree_->getBranchChildPtr(*current_branch, child_idx);
145 
146  stack_.push_back(stack_entry);
147 
148  current_key.popBranch();
149  }
150  }
151  }
152 
153  if (stack_.size()) {
154  this->current_state_ = &stack_.back();
155  }
156  else {
157  this->current_state_ = 0;
158  }
159  }
160 
161  return (*this);
162 }
163 
164 //////////////////////////////////////////////////////////////////////////////////////////////
165 template <typename OctreeT>
167  unsigned int max_depth_arg)
168 : OctreeIteratorBase<OctreeT>(max_depth_arg), FIFO_()
169 {
171 
172  // initialize iterator
173  this->reset();
174 }
175 
176 //////////////////////////////////////////////////////////////////////////////////////////////
177 template <typename OctreeT>
179  OctreeT* octree_arg, unsigned int max_depth_arg)
180 : OctreeIteratorBase<OctreeT>(octree_arg, max_depth_arg), FIFO_()
181 {
183 
184  // initialize iterator
185  this->reset();
186 }
187 
188 //////////////////////////////////////////////////////////////////////////////////////////////
189 template <typename OctreeT>
190 void
192 {
194 
195  // init FIFO
196  FIFO_.clear();
197 
198  if (this->octree_) {
199  // pushing root node to stack
200  IteratorState FIFO_entry;
201  FIFO_entry.node_ = this->octree_->getRootNode();
202  FIFO_entry.depth_ = 0;
203  FIFO_entry.key_.x = FIFO_entry.key_.y = FIFO_entry.key_.z = 0;
204 
205  FIFO_.push_back(FIFO_entry);
206 
207  this->current_state_ = &FIFO_.front();
208  }
209 }
210 
211 //////////////////////////////////////////////////////////////////////////////////////////////
212 template <typename OctreeT>
215 {
216 
217  if (FIFO_.size()) {
218  // get stack element
219  IteratorState FIFO_entry = FIFO_.front();
220  FIFO_.pop_front();
221 
222  FIFO_entry.depth_++;
223 
224  if ((this->max_octree_depth_ >= FIFO_entry.depth_) &&
225  (FIFO_entry.node_->getNodeType() == BRANCH_NODE)) {
226  // current node is a branch node
227  BranchNode* current_branch = static_cast<BranchNode*>(FIFO_entry.node_);
228 
229  // iterate over all children
230  for (unsigned char child_idx = 0; child_idx < 8; ++child_idx) {
231 
232  // if child exist
233  if (this->octree_->branchHasChild(*current_branch, child_idx)) {
234  // add child to stack
235  OctreeKey& current_key = FIFO_entry.key_;
236  current_key.pushBranch(static_cast<unsigned char>(child_idx));
237 
238  FIFO_entry.node_ =
239  this->octree_->getBranchChildPtr(*current_branch, child_idx);
240 
241  FIFO_.push_back(FIFO_entry);
242 
243  current_key.popBranch();
244  }
245  }
246  }
247 
248  if (FIFO_.size()) {
249  this->current_state_ = &FIFO_.front();
250  }
251  else {
252  this->current_state_ = 0;
253  }
254  }
255 
256  return (*this);
257 }
258 
259 //////////////////////////////////////////////////////////////////////////////////////////////
260 template <typename OctreeT>
262 : OctreeBreadthFirstIterator<OctreeT>(0u), fixed_depth_(0u)
263 {}
264 
265 //////////////////////////////////////////////////////////////////////////////////////////////
266 template <typename OctreeT>
268  OctreeT* octree_arg, unsigned int fixed_depth_arg)
269 : OctreeBreadthFirstIterator<OctreeT>(octree_arg, fixed_depth_arg)
270 , fixed_depth_(fixed_depth_arg)
271 {
272  this->reset(fixed_depth_arg);
273 }
274 
275 //////////////////////////////////////////////////////////////////////////////////////////////
276 template <typename OctreeT>
277 void
278 OctreeFixedDepthIterator<OctreeT>::reset(unsigned int fixed_depth_arg)
279 {
280  // Set the desired depth to walk through
281  fixed_depth_ = fixed_depth_arg;
282 
283  if (!this->octree_) {
284  return;
285  }
286 
287  // If I'm nowhere, reset
288  // If I'm somewhere and I can't guarantee I'm before the first node, reset
289  if ((!this->current_state_) || (fixed_depth_ <= this->getCurrentOctreeDepth()))
291 
292  if (this->octree_->getTreeDepth() < fixed_depth_) {
293  PCL_WARN("[pcl::octree::FixedDepthIterator] The requested fixed depth was bigger "
294  "than the octree's depth.\n");
295  PCL_WARN("[pcl::octree::FixedDepthIterator] fixed_depth = %d (instead of %d)\n",
296  this->octree_->getTreeDepth(),
297  fixed_depth_);
298  }
299 
300  // By default for the parent class OctreeBreadthFirstIterator, if the
301  // depth argument is equal to 0, the iterator would run over every node.
302  // For the OctreeFixedDepthIterator, whatever the depth ask, set the
303  // max_octree_depth_ accordingly
304  this->max_octree_depth_ = std::min(fixed_depth_, this->octree_->getTreeDepth());
305 
306  // Restore previous state in case breath first iterator had child nodes already set up
307  if (FIFO_.size())
308  this->current_state_ = &FIFO_.front();
309 
310  // Iterate all the way to the desired level
311  while (this->current_state_ && (this->getCurrentOctreeDepth() != fixed_depth_))
313 }
314 
315 //////////////////////////////////////////////////////////////////////////////////////////////
316 template <typename OctreeT>
318  unsigned int max_depth_arg)
319 : OctreeBreadthFirstIterator<OctreeT>(max_depth_arg)
320 {
321  reset();
322 }
323 
324 //////////////////////////////////////////////////////////////////////////////////////////////
325 template <typename OctreeT>
327  OctreeT* octree_arg, unsigned int max_depth_arg)
328 : OctreeBreadthFirstIterator<OctreeT>(octree_arg, max_depth_arg)
329 {
330  reset();
331 }
332 
333 //////////////////////////////////////////////////////////////////////////////////////////////
334 template <typename OctreeT>
336  OctreeT* octree_arg,
337  unsigned int max_depth_arg,
338  IteratorState* current_state,
339  const std::deque<IteratorState>& fifo)
340 : OctreeBreadthFirstIterator<OctreeT>(octree_arg, max_depth_arg, current_state, fifo)
341 {}
342 
343 //////////////////////////////////////////////////////////////////////////////////////////////
344 template <typename OctreeT>
345 void
347 {
349  ++*this;
350 }
351 
352 //////////////////////////////////////////////////////////////////////////////////////////////
353 template <typename OctreeT>
356 {
357  do {
359  } while ((this->current_state_) &&
360  (this->current_state_->node_->getNodeType() != LEAF_NODE));
361 
362  return (*this);
363 }
364 
365 //////////////////////////////////////////////////////////////////////////////////////////////
366 template <typename OctreeT>
369 {
371  ++*this;
372  return (_Tmp);
373 }
374 } // namespace octree
375 } // namespace pcl
376 
377 #endif
void reset()
Reset iterator.
void reset()
Reset the iterator to the first node at the current depth.
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
std::deque< IteratorState > FIFO_
FIFO list.
OctreeLeafNodeBreadthFirstIterator(unsigned int max_depth_arg=0)
Empty constructor.
std::vector< IteratorState > stack_
Stack structure.
IteratorState * current_state_
Pointer to current iterator state.
void popBranch()
pop child node from octree key
Definition: octree_key.h:122
unsigned int max_octree_depth_
Maximum octree depth.
unsigned int fixed_depth_
Given level of the node to be iterated.
void reset()
Reset the iterator to the root node of the octree.
typename OctreeT::BranchNode BranchNode
virtual void reset()
Reset the iterator to the root node of the octree.
unsigned int getCurrentOctreeDepth() const
Get the current depth level of 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:52
OctreeBreadthFirstIterator & operator++()
Preincrement operator.
OctreeT * octree_
Reference to octree class.
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:112
OctreeLeafNodeBreadthFirstIterator & operator++()
Preincrement operator.
OctreeBreadthFirstIterator(unsigned int max_depth_arg=0)
Empty constructor.