Point Cloud Library (PCL)  1.14.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 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  uindex_t 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 : OctreeIteratorBase<OctreeT>(max_depth_arg), FIFO_()
168 {
170 
171  // initialize iterator
172  this->reset();
173 }
174 
175 //////////////////////////////////////////////////////////////////////////////////////////////
176 template <typename OctreeT>
178  uindex_t max_depth_arg)
179 : OctreeIteratorBase<OctreeT>(octree_arg, max_depth_arg), FIFO_()
180 {
182 
183  // initialize iterator
184  this->reset();
185 }
186 
187 //////////////////////////////////////////////////////////////////////////////////////////////
188 template <typename OctreeT>
189 void
191 {
193 
194  // init FIFO
195  FIFO_.clear();
196 
197  if (this->octree_) {
198  // pushing root node to stack
199  IteratorState FIFO_entry;
200  FIFO_entry.node_ = this->octree_->getRootNode();
201  FIFO_entry.depth_ = 0;
202  FIFO_entry.key_.x = FIFO_entry.key_.y = FIFO_entry.key_.z = 0;
203 
204  FIFO_.push_back(FIFO_entry);
205 
206  this->current_state_ = &FIFO_.front();
207  }
208 }
209 
210 //////////////////////////////////////////////////////////////////////////////////////////////
211 template <typename OctreeT>
214 {
215 
216  if (FIFO_.size()) {
217  // get stack element
218  IteratorState FIFO_entry = FIFO_.front();
219  FIFO_.pop_front();
220 
221  FIFO_entry.depth_++;
222 
223  if ((this->max_octree_depth_ >= FIFO_entry.depth_) &&
224  (FIFO_entry.node_->getNodeType() == BRANCH_NODE)) {
225  // current node is a branch node
226  BranchNode* current_branch = static_cast<BranchNode*>(FIFO_entry.node_);
227 
228  // iterate over all children
229  for (unsigned char child_idx = 0; child_idx < 8; ++child_idx) {
230 
231  // if child exist
232  if (this->octree_->branchHasChild(*current_branch, child_idx)) {
233  // add child to stack
234  OctreeKey& current_key = FIFO_entry.key_;
235  current_key.pushBranch(static_cast<unsigned char>(child_idx));
236 
237  FIFO_entry.node_ =
238  this->octree_->getBranchChildPtr(*current_branch, child_idx);
239 
240  FIFO_.push_back(FIFO_entry);
241 
242  current_key.popBranch();
243  }
244  }
245  }
246 
247  if (FIFO_.size()) {
248  this->current_state_ = &FIFO_.front();
249  }
250  else {
251  this->current_state_ = 0;
252  }
253  }
254 
255  return (*this);
256 }
257 
258 //////////////////////////////////////////////////////////////////////////////////////////////
259 template <typename OctreeT>
261 : OctreeBreadthFirstIterator<OctreeT>(nullptr, 0), fixed_depth_(0u)
262 {}
263 
264 //////////////////////////////////////////////////////////////////////////////////////////////
265 template <typename OctreeT>
267  uindex_t fixed_depth_arg)
268 : OctreeBreadthFirstIterator<OctreeT>(octree_arg, fixed_depth_arg)
269 , fixed_depth_(fixed_depth_arg)
270 {
271  this->reset(fixed_depth_arg);
272 }
273 
274 //////////////////////////////////////////////////////////////////////////////////////////////
275 template <typename OctreeT>
276 void
278 {
279  // Set the desired depth to walk through
280  fixed_depth_ = fixed_depth_arg;
281 
282  if (!this->octree_) {
283  return;
284  }
285 
286  // If I'm nowhere, reset
287  // If I'm somewhere and I can't guarantee I'm before the first node, reset
288  if ((!this->current_state_) || (fixed_depth_ <= this->getCurrentOctreeDepth()))
290 
291  if (this->octree_->getTreeDepth() < fixed_depth_) {
292  PCL_WARN("[pcl::octree::FixedDepthIterator] The requested fixed depth was bigger "
293  "than the octree's depth.\n");
294  PCL_WARN("[pcl::octree::FixedDepthIterator] fixed_depth = %d (instead of %d)\n",
295  this->octree_->getTreeDepth(),
296  fixed_depth_);
297  }
298 
299  // By default for the parent class OctreeBreadthFirstIterator, if the
300  // depth argument is equal to 0, the iterator would run over every node.
301  // For the OctreeFixedDepthIterator, whatever the depth ask, set the
302  // max_octree_depth_ accordingly
303  this->max_octree_depth_ = std::min(fixed_depth_, this->octree_->getTreeDepth());
304 
305  // Restore previous state in case breath first iterator had child nodes already set up
306  if (FIFO_.size())
307  this->current_state_ = &FIFO_.front();
308 
309  // Iterate all the way to the desired level
310  while (this->current_state_ && (this->getCurrentOctreeDepth() != fixed_depth_))
312 }
313 
314 //////////////////////////////////////////////////////////////////////////////////////////////
315 template <typename OctreeT>
317  uindex_t max_depth_arg)
318 : OctreeBreadthFirstIterator<OctreeT>(max_depth_arg)
319 {
320  reset();
321 }
322 
323 //////////////////////////////////////////////////////////////////////////////////////////////
324 template <typename OctreeT>
326  OctreeT* octree_arg, uindex_t max_depth_arg)
327 : OctreeBreadthFirstIterator<OctreeT>(octree_arg, max_depth_arg)
328 {
329  reset();
330 }
331 
332 //////////////////////////////////////////////////////////////////////////////////////////////
333 template <typename OctreeT>
335  OctreeT* octree_arg,
336  uindex_t max_depth_arg,
337  IteratorState* current_state,
338  const std::deque<IteratorState>& fifo)
339 : OctreeBreadthFirstIterator<OctreeT>(octree_arg, max_depth_arg, current_state, fifo)
340 {}
341 
342 //////////////////////////////////////////////////////////////////////////////////////////////
343 template <typename OctreeT>
344 void
346 {
348  ++*this;
349 }
350 
351 //////////////////////////////////////////////////////////////////////////////////////////////
352 template <typename OctreeT>
355 {
356  do {
358  } while ((this->current_state_) &&
359  (this->current_state_->node_->getNodeType() != LEAF_NODE));
360 
361  return (*this);
362 }
363 
364 //////////////////////////////////////////////////////////////////////////////////////////////
365 template <typename OctreeT>
368 {
370  ++*this;
371  return (_Tmp);
372 }
373 } // namespace octree
374 } // namespace pcl
375 
376 #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 retrieving the type of octree node (branch or leaf)
OctreeLeafNodeBreadthFirstIterator(uindex_t max_depth_arg=0)
Empty constructor.
void popBranch()
pop child node from octree key
Definition: octree_key.h:122
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.
OctreeBreadthFirstIterator(uindex_t 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.
OctreeDepthFirstIterator(uindex_t max_depth_arg=0)
Empty constructor.
Octree key class
Definition: octree_key.h:54
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:112
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition: types.h:120
OctreeLeafNodeBreadthFirstIterator & operator++()
Preincrement operator.