octomap 1.9.8
OcTreeKey.h
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_OCTREE_KEY_H
35#define OCTOMAP_OCTREE_KEY_H
36
37/* According to c++ standard including this header has no practical effect
38 * but it can be used to determine the c++ standard library implementation.
39 */
40#include <ciso646>
41
42#include <assert.h>
43
44/* Libc++ does not implement the TR1 namespace, all c++11 related functionality
45 * is instead implemented in the std namespace.
46 */
47#if defined(__GNUC__) && ! defined(_LIBCPP_VERSION)
48 #include <tr1/unordered_set>
49 #include <tr1/unordered_map>
50 namespace octomap {
51 namespace unordered_ns = std::tr1;
52 }
53#else
54 #include <unordered_set>
55 #include <unordered_map>
56 namespace octomap {
57 namespace unordered_ns = std;
58 }
59#endif
60
61namespace octomap {
62
63 typedef uint16_t key_type;
64
70 class OcTreeKey {
71
72 public:
75 k[0] = a;
76 k[1] = b;
77 k[2] = c;
78 }
79
80 OcTreeKey(const OcTreeKey& other){
81 k[0] = other.k[0];
82 k[1] = other.k[1];
83 k[2] = other.k[2];
84 }
85
86 bool operator== (const OcTreeKey &other) const {
87 return ((k[0] == other[0]) && (k[1] == other[1]) && (k[2] == other[2]));
88 }
89
90 bool operator!= (const OcTreeKey& other) const {
91 return( (k[0] != other[0]) || (k[1] != other[1]) || (k[2] != other[2]) );
92 }
93
95 k[0] = other.k[0]; k[1] = other.k[1]; k[2] = other.k[2];
96 return *this;
97 }
98
99 const key_type& operator[] (unsigned int i) const {
100 return k[i];
101 }
102
103 key_type& operator[] (unsigned int i) {
104 return k[i];
105 }
106
108
110 struct KeyHash{
111 size_t operator()(const OcTreeKey& key) const{
112 // a simple hashing function
113 // explicit casts to size_t to operate on the complete range
114 // constanst will be promoted according to C++ standard
115 return static_cast<size_t>(key.k[0])
116 + 1447*static_cast<size_t>(key.k[1])
117 + 345637*static_cast<size_t>(key.k[2]);
118 }
119 };
120
121 };
122
129 typedef unordered_ns::unordered_set<OcTreeKey, OcTreeKey::KeyHash> KeySet;
130
136 typedef unordered_ns::unordered_map<OcTreeKey, bool, OcTreeKey::KeyHash> KeyBoolMap;
137
138
139 class KeyRay {
140 public:
141
143 ray.resize(maxSize);
144 reset();
145 }
146
147 KeyRay(const KeyRay& other){
148 ray = other.ray;
149 size_t dSize = other.end() - other.begin();
150 end_of_ray = ray.begin() + dSize;
151 }
152
153 void reset() {
154 end_of_ray = begin();
155 }
156
157 void addKey(const OcTreeKey& k) {
158 assert(end_of_ray != ray.end());
159 *end_of_ray = k;
160 ++end_of_ray;
161 }
162
163 size_t size() const { return end_of_ray - ray.begin(); }
164 size_t sizeMax() const { return maxSize; }
165
166 typedef std::vector<OcTreeKey>::iterator iterator;
167 typedef std::vector<OcTreeKey>::const_iterator const_iterator;
168 typedef std::vector<OcTreeKey>::reverse_iterator reverse_iterator;
169
170 iterator begin() { return ray.begin(); }
171 iterator end() { return end_of_ray; }
172 const_iterator begin() const { return ray.begin(); }
173 const_iterator end() const { return end_of_ray; }
174
175 reverse_iterator rbegin() { return (reverse_iterator) end_of_ray; }
176 reverse_iterator rend() { return ray.rend(); }
177
178 private:
179 std::vector<OcTreeKey> ray;
180 std::vector<OcTreeKey>::iterator end_of_ray;
181 const static size_t maxSize = 100000;
182 };
183
193 inline void computeChildKey (unsigned int pos, key_type center_offset_key,
194 const OcTreeKey& parent_key, OcTreeKey& child_key) {
195 // x-axis
196 if (pos & 1) child_key[0] = parent_key[0] + center_offset_key;
197 else child_key[0] = parent_key[0] - center_offset_key - (center_offset_key ? 0 : 1);
198 // y-axis
199 if (pos & 2) child_key[1] = parent_key[1] + center_offset_key;
200 else child_key[1] = parent_key[1] - center_offset_key - (center_offset_key ? 0 : 1);
201 // z-axis
202 if (pos & 4) child_key[2] = parent_key[2] + center_offset_key;
203 else child_key[2] = parent_key[2] - center_offset_key - (center_offset_key ? 0 : 1);
204 }
205
207 inline uint8_t computeChildIdx(const OcTreeKey& key, int depth){
208 uint8_t pos = 0;
209 if (key.k[0] & (1 << depth))
210 pos += 1;
211
212 if (key.k[1] & (1 << depth))
213 pos += 2;
214
215 if (key.k[2] & (1 << depth))
216 pos += 4;
217
218 return pos;
219 }
220
228 inline OcTreeKey computeIndexKey(key_type level, const OcTreeKey& key) {
229 if (level == 0)
230 return key;
231 else {
232 key_type mask = 65535 << level;
233 OcTreeKey result = key;
234 result[0] &= mask;
235 result[1] &= mask;
236 result[2] &= mask;
237 return result;
238 }
239 }
240
241} // namespace
242
243#endif
Definition: OcTreeKey.h:139
size_t sizeMax() const
Definition: OcTreeKey.h:164
std::vector< OcTreeKey >::const_iterator const_iterator
Definition: OcTreeKey.h:167
void addKey(const OcTreeKey &k)
Definition: OcTreeKey.h:157
iterator begin()
Definition: OcTreeKey.h:170
std::vector< OcTreeKey >::iterator iterator
Definition: OcTreeKey.h:166
const_iterator begin() const
Definition: OcTreeKey.h:172
void reset()
Definition: OcTreeKey.h:153
reverse_iterator rend()
Definition: OcTreeKey.h:176
const_iterator end() const
Definition: OcTreeKey.h:173
size_t size() const
Definition: OcTreeKey.h:163
std::vector< OcTreeKey >::reverse_iterator reverse_iterator
Definition: OcTreeKey.h:168
reverse_iterator rbegin()
Definition: OcTreeKey.h:175
KeyRay(const KeyRay &other)
Definition: OcTreeKey.h:147
KeyRay()
Definition: OcTreeKey.h:142
iterator end()
Definition: OcTreeKey.h:171
OcTreeKey is a container class for internal key addressing.
Definition: OcTreeKey.h:70
OcTreeKey(key_type a, key_type b, key_type c)
Definition: OcTreeKey.h:74
OcTreeKey()
Definition: OcTreeKey.h:73
const key_type & operator[](unsigned int i) const
Definition: OcTreeKey.h:99
OcTreeKey(const OcTreeKey &other)
Definition: OcTreeKey.h:80
OcTreeKey & operator=(const OcTreeKey &other)
Definition: OcTreeKey.h:94
key_type k[3]
Definition: OcTreeKey.h:107
bool operator==(const OcTreeKey &other) const
Definition: OcTreeKey.h:86
bool operator!=(const OcTreeKey &other) const
Definition: OcTreeKey.h:90
Namespace the OctoMap library and visualization tools.
uint16_t key_type
Definition: OcTreeKey.h:63
unordered_ns::unordered_map< OcTreeKey, bool, OcTreeKey::KeyHash > KeyBoolMap
Data structrure to efficiently track changed nodes as a combination of OcTreeKeys and a bool flag (to...
Definition: OcTreeKey.h:136
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
uint8_t computeChildIdx(const OcTreeKey &key, int depth)
generate child index (between 0 and 7) from key at given tree depth
Definition: OcTreeKey.h:207
unordered_ns::unordered_set< OcTreeKey, OcTreeKey::KeyHash > KeySet
Data structure to efficiently compute the nodes to update from a scan insertion using a hash set.
Definition: OcTreeKey.h:129
Provides a hash function on Keys.
Definition: OcTreeKey.h:110
size_t operator()(const OcTreeKey &key) const
Definition: OcTreeKey.h:111