octomap 1.9.8
OcTreeIterator.hxx
Go to the documentation of this file.
1/*
2 * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3 * https://octomap.github.io/
4 *
5 * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
6 * All rights reserved.
7 * License: New BSD
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above copyright
15 * notice, this list of conditions and the following disclaimer in the
16 * documentation and/or other materials provided with the distribution.
17 * * Neither the name of the University of Freiburg nor the names of its
18 * contributors may be used to endorse or promote products derived from
19 * this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 * POSSIBILITY OF SUCH DAMAGE.
32 */
33
34#ifndef OCTOMAP_OCTREEITERATOR_HXX_
35#define OCTOMAP_OCTREEITERATOR_HXX_
36
42 class iterator_base : public std::iterator<std::forward_iterator_tag, NodeType>{
43 public:
44 struct StackElement;
46 iterator_base() : tree(NULL), maxDepth(0){}
47
55 iterator_base(OcTreeBaseImpl<NodeType,INTERFACE> const* ptree, uint8_t depth=0)
56 : tree((ptree && ptree->root) ? ptree : NULL), maxDepth(depth)
57 {
58 if (ptree && maxDepth == 0)
59 maxDepth = ptree->getTreeDepth();
60
61 if (tree && tree->root){ // tree is not empty
63 s.node = tree->root;
64 s.depth = 0;
65 s.key[0] = s.key[1] = s.key[2] = tree->tree_max_val;
66 stack.push(s);
67 } else{ // construct the same as "end"
68 tree = NULL;
69 this->maxDepth = 0;
70 }
71 }
72
75 : tree(other.tree), maxDepth(other.maxDepth), stack(other.stack) {}
76
78 bool operator==(const iterator_base& other) const {
79 return (tree ==other.tree && stack.size() == other.stack.size()
80 && (stack.size()==0 || (stack.size() > 0 && (stack.top().node == other.stack.top().node
81 && stack.top().depth == other.stack.top().depth
82 && stack.top().key == other.stack.top().key ))));
83 }
84
86 bool operator!=(const iterator_base& other) const {
87 return (tree !=other.tree || stack.size() != other.stack.size()
88 || (stack.size() > 0 && ((stack.top().node != other.stack.top().node
89 || stack.top().depth != other.stack.top().depth
90 || stack.top().key != other.stack.top().key ))));
91 }
92
94 tree = other.tree;
95 maxDepth = other.maxDepth;
96 stack = other.stack;
97 return *this;
98 };
99
102 NodeType const* operator->() const { return stack.top().node;}
103
106 NodeType* operator->() { return stack.top().node;}
107
110 const NodeType& operator*() const { return *(stack.top().node);}
111
114 NodeType& operator*() { return *(stack.top().node);}
115
118 return tree->keyToCoord(stack.top().key, stack.top().depth);
119 }
120
122 double getX() const{
123 return tree->keyToCoord(stack.top().key[0], stack.top().depth);
124 }
126 double getY() const{
127 return tree->keyToCoord(stack.top().key[1], stack.top().depth);
128 }
130 double getZ() const{
131 return tree->keyToCoord(stack.top().key[2], stack.top().depth);
132 }
133
135 double getSize() const {return tree->getNodeSize(stack.top().depth); }
136
138 unsigned getDepth() const {return unsigned(stack.top().depth); }
139
141 const OcTreeKey& getKey() const {return stack.top().key;}
142
144 OcTreeKey getIndexKey() const {
145 return computeIndexKey(tree->getTreeDepth() - stack.top().depth, stack.top().key);
146 }
147
148
151 NodeType* node;
152 OcTreeKey key;
153 uint8_t depth;
154 };
155
156
157 protected:
158 OcTreeBaseImpl<NodeType,INTERFACE> const* tree;
159 uint8_t maxDepth;
160
162 std::stack<StackElement,std::vector<StackElement> > stack;
163
167 StackElement top = stack.top();
168 stack.pop();
169 if (top.depth == maxDepth)
170 return;
171
172 StackElement s;
173 s.depth = top.depth +1;
174
175 key_type center_offset_key = tree->tree_max_val >> s.depth;
176 // push on stack in reverse order
177 for (int i=7; i>=0; --i) {
178 if (tree->nodeChildExists(top.node,i)) {
179 computeChildKey(i, center_offset_key, top.key, s.key);
180 s.node = tree->getNodeChild(top.node, i);
181 //OCTOMAP_DEBUG_STR("Current depth: " << int(top.depth) << " new: "<< int(s.depth) << " child#" << i <<" ptr: "<<s.node);
182 stack.push(s);
183 assert(s.depth <= maxDepth);
184 }
185 }
186 }
187
188 };
189
208 public:
216 tree_iterator(OcTreeBaseImpl<NodeType,INTERFACE> const* ptree, uint8_t depth=0) : iterator_base(ptree, depth) {};
217
220 tree_iterator result = *this;
221 ++(*this);
222 return result;
223 }
224
227
228 if (!this->stack.empty()){
229 this->singleIncrement();
230 }
231
232 if (this->stack.empty()){
233 this->tree = NULL;
234 }
235
236 return *this;
237 }
238
240 bool isLeaf() const{
241 return (!this->tree->nodeHasChildren(this->stack.top().node) || this->stack.top().depth == this->maxDepth);
242 }
243 };
244
264 public:
266
273 leaf_iterator(OcTreeBaseImpl<NodeType, INTERFACE> const* ptree, uint8_t depth=0) : iterator_base(ptree, depth) {
274 // tree could be empty (= no stack)
275 if (this->stack.size() > 0){
276 // skip forward to next valid leaf node:
277 // add root another time (one will be removed) and ++
278 this->stack.push(this->stack.top());
279 operator ++();
280 }
281 }
282
283 leaf_iterator(const leaf_iterator& other) : iterator_base(other) {};
284
287 leaf_iterator result = *this;
288 ++(*this);
289 return result;
290 }
291
294 if (this->stack.empty()){
295 this->tree = NULL; // TODO check?
296
297 } else {
298 this->stack.pop();
299
300 // skip forward to next leaf
301 while(!this->stack.empty()
302 && this->stack.top().depth < this->maxDepth
303 && this->tree->nodeHasChildren(this->stack.top().node))
304 {
305 this->singleIncrement();
306 }
307 // done: either stack is empty (== end iterator) or a next leaf node is reached!
308 if (this->stack.empty())
309 this->tree = NULL;
310 }
311
312
313 return *this;
314 }
315
316 };
317
336 public:
353 leaf_bbx_iterator(OcTreeBaseImpl<NodeType,INTERFACE> const* ptree, const point3d& min, const point3d& max, uint8_t depth=0)
354 : iterator_base(ptree, depth)
355 {
356 if (this->stack.size() > 0){
357 assert(ptree);
358 if (!this->tree->coordToKeyChecked(min, minKey) || !this->tree->coordToKeyChecked(max, maxKey)){
359 // coordinates invalid, set to end iterator
360 this->tree = NULL;
361 this->maxDepth = 0;
362 } else{ // else: keys are generated and stored
363
364 // advance from root to next valid leaf in bbx:
365 this->stack.push(this->stack.top());
366 this->operator ++();
367 }
368 }
369
370 }
371
381 leaf_bbx_iterator(OcTreeBaseImpl<NodeType,INTERFACE> const* ptree, const OcTreeKey& min, const OcTreeKey& max, uint8_t depth=0)
382 : iterator_base(ptree, depth), minKey(min), maxKey(max)
383 {
384 // tree could be empty (= no stack)
385 if (this->stack.size() > 0){
386 // advance from root to next valid leaf in bbx:
387 this->stack.push(this->stack.top());
388 this->operator ++();
389 }
390 }
391
393 minKey = other.minKey;
394 maxKey = other.maxKey;
395 }
396
397
398
401 leaf_bbx_iterator result = *this;
402 ++(*this);
403 return result;
404 }
405
408 if (this->stack.empty()){
409 this->tree = NULL; // TODO check?
410
411 } else {
412 this->stack.pop();
413
414 // skip forward to next leaf
415 while(!this->stack.empty()
416 && this->stack.top().depth < this->maxDepth
417 && this->tree->nodeHasChildren(this->stack.top().node))
418 {
419 this->singleIncrement();
420 }
421 // done: either stack is empty (== end iterator) or a next leaf node is reached!
422 if (this->stack.empty())
423 this->tree = NULL;
424 }
425
426
427 return *this;
428 };
429
430 protected:
431
433 typename iterator_base::StackElement top = this->stack.top();
434 this->stack.pop();
435
437 s.depth = top.depth +1;
438 key_type center_offset_key = this->tree->tree_max_val >> s.depth;
439 // push on stack in reverse order
440 for (int i=7; i>=0; --i) {
441 if (this->tree->nodeChildExists(top.node, i)) {
442 computeChildKey(i, center_offset_key, top.key, s.key);
443
444 // overlap of query bbx and child bbx?
445 if ((minKey[0] <= (s.key[0] + center_offset_key)) && (maxKey[0] >= (s.key[0] - center_offset_key))
446 && (minKey[1] <= (s.key[1] + center_offset_key)) && (maxKey[1] >= (s.key[1] - center_offset_key))
447 && (minKey[2] <= (s.key[2] + center_offset_key)) && (maxKey[2] >= (s.key[2] - center_offset_key)))
448 {
449 s.node = this->tree->getNodeChild(top.node, i);
450 this->stack.push(s);
451 assert(s.depth <= this->maxDepth);
452 }
453 }
454 }
455 }
456
457
458 OcTreeKey minKey;
459 OcTreeKey maxKey;
460 };
461
462
463#endif /* OCTREEITERATOR_HXX_ */
Base class for OcTree iterators.
Definition: OcTreeIterator.hxx:42
bool operator==(const iterator_base &other) const
Comparison between iterators. First compares the tree, then stack size and top element of stack.
Definition: OcTreeIterator.hxx:78
bool operator!=(const iterator_base &other) const
Comparison between iterators. First compares the tree, then stack size and top element of stack.
Definition: OcTreeIterator.hxx:86
double getZ() const
Definition: OcTreeIterator.hxx:130
const OcTreeKey & getKey() const
Definition: OcTreeIterator.hxx:141
double getY() const
Definition: OcTreeIterator.hxx:126
double getX() const
Definition: OcTreeIterator.hxx:122
NodeType & operator*()
Return the current node in the octree which the iterator is referring to.
Definition: OcTreeIterator.hxx:114
iterator_base & operator=(const iterator_base &other)
Definition: OcTreeIterator.hxx:93
iterator_base()
Default ctor, only used for the end-iterator.
Definition: OcTreeIterator.hxx:46
unsigned getDepth() const
return depth of the current node
Definition: OcTreeIterator.hxx:138
OcTreeBaseImpl< NodeType, INTERFACE > const * tree
Octree this iterator is working on.
Definition: OcTreeIterator.hxx:158
void singleIncrement()
One step of depth-first tree traversal. How this is used depends on the actual iterator.
Definition: OcTreeIterator.hxx:166
point3d getCoordinate() const
return the center coordinate of the current node
Definition: OcTreeIterator.hxx:117
OcTreeKey getIndexKey() const
Definition: OcTreeIterator.hxx:144
NodeType * operator->()
Ptr operator will return the current node in the octree which the iterator is referring to.
Definition: OcTreeIterator.hxx:106
iterator_base(const iterator_base &other)
Copy constructor of the iterator.
Definition: OcTreeIterator.hxx:74
double getSize() const
Definition: OcTreeIterator.hxx:135
uint8_t maxDepth
Maximum depth for depth-limited queries.
Definition: OcTreeIterator.hxx:159
NodeType const * operator->() const
Ptr operator will return the current node in the octree which the iterator is referring to.
Definition: OcTreeIterator.hxx:102
const NodeType & operator*() const
Return the current node in the octree which the iterator is referring to.
Definition: OcTreeIterator.hxx:110
std::stack< StackElement, std::vector< StackElement > > stack
Internal recursion stack. Apparently a stack of vector works fastest here.
Definition: OcTreeIterator.hxx:162
iterator_base(OcTreeBaseImpl< NodeType, INTERFACE > const *ptree, uint8_t depth=0)
Constructor of the iterator.
Definition: OcTreeIterator.hxx:55
Bounding-box leaf iterator.
Definition: OcTreeIterator.hxx:335
leaf_bbx_iterator(OcTreeBaseImpl< NodeType, INTERFACE > const *ptree, const OcTreeKey &min, const OcTreeKey &max, uint8_t depth=0)
Constructor of the iterator.
Definition: OcTreeIterator.hxx:381
leaf_bbx_iterator()
Definition: OcTreeIterator.hxx:337
leaf_bbx_iterator & operator++()
prefix increment operator of iterator (++it)
Definition: OcTreeIterator.hxx:407
leaf_bbx_iterator(OcTreeBaseImpl< NodeType, INTERFACE > const *ptree, const point3d &min, const point3d &max, uint8_t depth=0)
Constructor of the iterator.
Definition: OcTreeIterator.hxx:353
void singleIncrement()
Definition: OcTreeIterator.hxx:432
OcTreeKey minKey
Definition: OcTreeIterator.hxx:458
leaf_bbx_iterator operator++(int)
postfix increment operator of iterator (it++)
Definition: OcTreeIterator.hxx:400
leaf_bbx_iterator(const leaf_bbx_iterator &other)
Definition: OcTreeIterator.hxx:392
OcTreeKey maxKey
Definition: OcTreeIterator.hxx:459
Iterator to iterate over all leafs of the tree.
Definition: OcTreeIterator.hxx:263
leaf_iterator & operator++()
prefix increment operator of iterator (++it)
Definition: OcTreeIterator.hxx:293
leaf_iterator(const leaf_iterator &other)
Definition: OcTreeIterator.hxx:283
leaf_iterator(OcTreeBaseImpl< NodeType, INTERFACE > const *ptree, uint8_t depth=0)
Constructor of the iterator.
Definition: OcTreeIterator.hxx:273
leaf_iterator operator++(int)
postfix increment operator of iterator (it++)
Definition: OcTreeIterator.hxx:286
leaf_iterator()
Definition: OcTreeIterator.hxx:265
Iterator over the complete tree (inner nodes and leafs).
Definition: OcTreeIterator.hxx:207
tree_iterator operator++(int)
postfix increment operator of iterator (it++)
Definition: OcTreeIterator.hxx:219
tree_iterator()
Definition: OcTreeIterator.hxx:209
tree_iterator & operator++()
Prefix increment operator to advance the iterator.
Definition: OcTreeIterator.hxx:226
bool isLeaf() const
Definition: OcTreeIterator.hxx:240
tree_iterator(OcTreeBaseImpl< NodeType, INTERFACE > const *ptree, uint8_t depth=0)
Constructor of the iterator.
Definition: OcTreeIterator.hxx:216
uint16_t key_type
Definition: OcTreeKey.h:63
octomath::Vector3 point3d
Use Vector3 (float precision) as a point3d in octomap.
Definition: octomap_types.h:49
void computeChildKey(unsigned int pos, key_type center_offset_key, const OcTreeKey &parent_key, OcTreeKey &child_key)
Computes the key of a child node while traversing the octree, given child index and current key.
Definition: OcTreeKey.h:193
OcTreeKey computeIndexKey(key_type level, const OcTreeKey &key)
Generates a unique key for all keys on a certain level of the tree.
Definition: OcTreeKey.h:228
Element on the internal recursion stack of the iterator.
Definition: OcTreeIterator.hxx:150
OcTreeKey key
Definition: OcTreeIterator.hxx:152
NodeType * node
Definition: OcTreeIterator.hxx:151
uint8_t depth
Definition: OcTreeIterator.hxx:153