Point Cloud Library (PCL) 1.13.0
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
44namespace pcl {
45namespace octree {
46//////////////////////////////////////////////////////////////////////////////////////////////
47template <typename OctreeT>
49: OctreeIteratorBase<OctreeT>(max_depth_arg), stack_()
50{
51 // initialize iterator
52 this->reset();
53}
54
55//////////////////////////////////////////////////////////////////////////////////////////////
56template <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//////////////////////////////////////////////////////////////////////////////////////////////
66template <typename OctreeT>
67void
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//////////////////////////////////////////////////////////////////////////////////////////////
92template <typename OctreeT>
93void
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//////////////////////////////////////////////////////////////////////////////////////////////
115template <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//////////////////////////////////////////////////////////////////////////////////////////////
165template <typename OctreeT>
167: OctreeIteratorBase<OctreeT>(max_depth_arg), FIFO_()
168{
170
171 // initialize iterator
172 this->reset();
173}
174
175//////////////////////////////////////////////////////////////////////////////////////////////
176template <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//////////////////////////////////////////////////////////////////////////////////////////////
188template <typename OctreeT>
189void
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//////////////////////////////////////////////////////////////////////////////////////////////
211template <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//////////////////////////////////////////////////////////////////////////////////////////////
259template <typename OctreeT>
261: OctreeBreadthFirstIterator<OctreeT>(nullptr, 0), fixed_depth_(0u)
262{}
263
264//////////////////////////////////////////////////////////////////////////////////////////////
265template <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//////////////////////////////////////////////////////////////////////////////////////////////
275template <typename OctreeT>
276void
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//////////////////////////////////////////////////////////////////////////////////////////////
315template <typename OctreeT>
317 uindex_t max_depth_arg)
318: OctreeBreadthFirstIterator<OctreeT>(max_depth_arg)
319{
320 reset();
321}
322
323//////////////////////////////////////////////////////////////////////////////////////////////
324template <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//////////////////////////////////////////////////////////////////////////////////////////////
333template <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//////////////////////////////////////////////////////////////////////////////////////////////
343template <typename OctreeT>
344void
346{
348 ++*this;
349}
350
351//////////////////////////////////////////////////////////////////////////////////////////////
352template <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//////////////////////////////////////////////////////////////////////////////////////////////
365template <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