Point Cloud Library (PCL)  1.14.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  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
OctreeBreadthFirstIterator & operator++()
Preincrement operator.
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.
OctreeDepthFirstIterator & operator++()
Preincrement operator.
virtual void reset()
Reset the iterator to the root node of the octree.
OctreeDepthFirstIterator(uindex_t max_depth_arg=0)
Empty constructor.
void reset()
Reset the iterator to the first node at the current depth.
Abstract octree iterator class
typename OctreeT::BranchNode BranchNode
Octree key class
Definition: octree_key.h:54
void popBranch()
pop child node from octree key
Definition: octree_key.h:122
void pushBranch(unsigned char childIndex)
push a child node to the octree key
Definition: octree_key.h:112
OctreeLeafNodeBreadthFirstIterator(uindex_t max_depth_arg=0)
Empty constructor.
void reset()
Reset the iterator to the first leaf in the breadth first way.
OctreeLeafNodeBreadthFirstIterator & operator++()
Preincrement operator.
virtual node_type_t getNodeType() const =0
Pure virtual method for retrieving the type of octree node (branch or leaf)
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition: types.h:120