octomap  1.9.7
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 
61 namespace octomap {
62 
63  typedef uint16_t key_type;
64 
70  class OcTreeKey {
71 
72  public:
73  OcTreeKey () {}
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 
94  OcTreeKey& operator=(const OcTreeKey& other){
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 
142  KeyRay () {
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
octomap::key_type
uint16_t key_type
Definition: OcTreeKey.h:63
octomap::KeyRay::const_iterator
std::vector< OcTreeKey >::const_iterator const_iterator
Definition: OcTreeKey.h:167
octomap::KeyRay::iterator
std::vector< OcTreeKey >::iterator iterator
Definition: OcTreeKey.h:166
octomap::KeyRay::reverse_iterator
std::vector< OcTreeKey >::reverse_iterator reverse_iterator
Definition: OcTreeKey.h:168
octomap::OcTreeKey::operator!=
bool operator!=(const OcTreeKey &other) const
Definition: OcTreeKey.h:90
octomap::KeyRay::end
const_iterator end() const
Definition: OcTreeKey.h:173
octomap::OcTreeKey::OcTreeKey
OcTreeKey()
Definition: OcTreeKey.h:73
octomap::OcTreeKey::operator[]
const key_type & operator[](unsigned int i) const
Definition: OcTreeKey.h:99
octomap::OcTreeKey::operator=
OcTreeKey & operator=(const OcTreeKey &other)
Definition: OcTreeKey.h:94
octomap::KeyRay
Definition: OcTreeKey.h:139
octomap::KeyBoolMap
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
octomap::KeyRay::begin
const_iterator begin() const
Definition: OcTreeKey.h:172
octomap::KeyRay::reset
void reset()
Definition: OcTreeKey.h:153
octomap::KeySet
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
octomap::KeyRay::sizeMax
size_t sizeMax() const
Definition: OcTreeKey.h:164
octomap::OcTreeKey::k
key_type k[3]
Definition: OcTreeKey.h:107
octomap::KeyRay::KeyRay
KeyRay()
Definition: OcTreeKey.h:142
octomap::OcTreeKey::KeyHash::operator()
size_t operator()(const OcTreeKey &key) const
Definition: OcTreeKey.h:111
octomap::KeyRay::end
iterator end()
Definition: OcTreeKey.h:171
octomap::OcTreeKey
OcTreeKey is a container class for internal key addressing.
Definition: OcTreeKey.h:70
octomap::KeyRay::rbegin
reverse_iterator rbegin()
Definition: OcTreeKey.h:175
octomap::OcTreeKey::KeyHash
Provides a hash function on Keys.
Definition: OcTreeKey.h:110
octomap::computeChildIdx
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
octomap::OcTreeKey::operator==
bool operator==(const OcTreeKey &other) const
Definition: OcTreeKey.h:86
octomap::KeyRay::rend
reverse_iterator rend()
Definition: OcTreeKey.h:176
octomap
Tables used by the Marching Cubes Algorithm The tables are from Paul Bourke's web page http://paulbou...
octomap::KeyRay::begin
iterator begin()
Definition: OcTreeKey.h:170
octomap::computeChildKey
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
octomap::computeIndexKey
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
octomap::OcTreeKey::OcTreeKey
OcTreeKey(key_type a, key_type b, key_type c)
Definition: OcTreeKey.h:74
octomap::KeyRay::KeyRay
KeyRay(const KeyRay &other)
Definition: OcTreeKey.h:147
octomap::KeyRay::addKey
void addKey(const OcTreeKey &k)
Definition: OcTreeKey.h:157
octomap::KeyRay::size
size_t size() const
Definition: OcTreeKey.h:163
octomap::OcTreeKey::OcTreeKey
OcTreeKey(const OcTreeKey &other)
Definition: OcTreeKey.h:80