Main MRPT website > C++ reference for MRPT 1.3.2
OcTreeKey.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef OCTOMAP_OCTREE_KEY_H
10 #define OCTOMAP_OCTREE_KEY_H
11 
12 // $Id: OcTreeKey.h 429 2012-09-21 10:08:14Z ahornung $
13 
14 /**
15 * OctoMap:
16 * A probabilistic, flexible, and compact 3D mapping library for robotic systems.
17 * @author K. M. Wurm, A. Hornung, University of Freiburg, Copyright (C) 2010.
18 * @see http://octomap.sourceforge.net/
19 * License: New BSD License
20 */
21 
22 /*
23  * Copyright (c) 2010, K. M. Wurm, A. Hornung, University of Freiburg
24  * All rights reserved.
25  *
26  * Redistribution and use in source and binary forms, with or without
27  * modification, are permitted provided that the following conditions are met:
28  *
29  * * Redistributions of source code must retain the above copyright
30  * notice, this list of conditions and the following disclaimer.
31  * * Redistributions in binary form must reproduce the above copyright
32  * notice, this list of conditions and the following disclaimer in the
33  * documentation and/or other materials provided with the distribution.
34  * * Neither the name of the University of Freiburg nor the names of its
35  * contributors may be used to endorse or promote products derived from
36  * this software without specific prior written permission.
37  *
38  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
39  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
40  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
41  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
42  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
43  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
44  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
45  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
46  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
47  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
48  * POSSIBILITY OF SUCH DAMAGE.
49  */
50 
51 #include <assert.h>
52 #if defined( __GNUC__ ) && !(defined( __clang__ ) && defined( __APPLE__ ) )
53  #include <tr1/unordered_set>
54  #include <tr1/unordered_map>
55  #define OCTREE_KEY_USES_TR1
56 #else
57  #include <unordered_set>
58  #include <unordered_map>
59 #endif
60 
61 namespace octomap {
62 
63  /**
64  * OcTreeKey is a container class for internal key addressing. The keys count the
65  * number of cells (voxels) from the origin as discrete address of a voxel.
66  * @see OcTreeBaseImpl::coordToKey() and OcTreeBaseImpl::keyToCoord() for conversions.
67  */
68  class OcTreeKey {
69 
70  public:
71  OcTreeKey () {}
72  OcTreeKey (unsigned short int a, unsigned short int b, unsigned short int c)
73  { k[0] = a; k[1] = b; k[2] = c; }
74  OcTreeKey(const OcTreeKey& other){
75  k[0] = other.k[0]; k[1] = other.k[1]; k[2] = other.k[2];
76  }
77  bool operator== (const OcTreeKey &other) const {
78  return ((k[0] == other[0]) && (k[1] == other[1]) && (k[2] == other[2]));
79  }
80  bool operator!= (const OcTreeKey &other) const {
81  return( (k[0] != other[0]) || (k[1] != other[1]) || (k[2] != other[2]) );
82  }
83  OcTreeKey& operator=(const OcTreeKey& other){
84  k[0] = other.k[0]; k[1] = other.k[1]; k[2] = other.k[2];
85  return *this;
86  }
87  const unsigned short int& operator[] (unsigned int i) const {
88  return k[i];
89  }
90  unsigned short int& operator[] (unsigned int i) {
91  return k[i];
92  }
93 
94  unsigned short int k[3];
95 
96  /// Provides a hash function on Keys
97  struct KeyHash{
98  size_t operator()(const OcTreeKey& key) const{
99  // a hashing function
100  return key.k[0] + 1337*key.k[1] + 345637*key.k[2];
101  }
102  };
103 
104  };
105 
106  /**
107  * Data structure to efficiently compute the nodes to update from a scan
108  * insertion using a hash set.
109  * @note you need to use boost::unordered_set instead if your compiler does not
110  * yet support tr1!
111  */
112 #ifdef OCTREE_KEY_USES_TR1
113  typedef std::tr1::unordered_set<OcTreeKey, OcTreeKey::KeyHash> KeySet;
114 #else
115  typedef std::unordered_set<OcTreeKey, OcTreeKey::KeyHash> KeySet;
116 #endif
117 
118  /**
119  * Data structrure to efficiently track changed nodes as a combination of
120  * OcTreeKeys and a bool flag (to denote newly created nodes)
121  *
122  */
123 #ifdef OCTREE_KEY_USES_TR1
124  typedef std::tr1::unordered_map<OcTreeKey, bool, OcTreeKey::KeyHash> KeyBoolMap;
125 #else
126  typedef std::unordered_map<OcTreeKey, bool, OcTreeKey::KeyHash> KeyBoolMap;
127 #endif
128 
129 
130  class KeyRay {
131  public:
132 
133  KeyRay () {
134  ray.resize(100000);
135  reset();
136  }
137  void reset() {
138  end_of_ray = begin();
139  }
140  void addKey(OcTreeKey& k) {
141  assert(end_of_ray != ray.end());
142  *end_of_ray = k;
143  ++end_of_ray;
144  }
145 
146  size_t size() const { return end_of_ray - ray.begin(); }
147  size_t sizeMax() const { return 100000; }
148 
151  typedef std::vector<OcTreeKey>::reverse_iterator reverse_iterator;
152 
153  iterator begin() { return ray.begin(); }
154  iterator end() { return end_of_ray; }
155  const_iterator begin() const { return ray.begin(); }
156  const_iterator end() const { return end_of_ray; }
157 
158  reverse_iterator rbegin() { return (reverse_iterator) end_of_ray; }
159  reverse_iterator rend() { return ray.rend(); }
160 
161  public:
162 
163  std::vector<OcTreeKey> ray;
165  };
166 
167  /**
168  * Computes the key of a child node while traversing the octree, given
169  * child index and current key
170  *
171  * @param[in] pos index of child node (0..7)
172  * @param[in] center_offset_key constant offset of octree keys
173  * @param[in] parent_key current (parent) key
174  * @param[out] child_key computed child key
175  */
176  inline void computeChildKey (const unsigned int& pos, const unsigned short int& center_offset_key,
177  const OcTreeKey& parent_key, OcTreeKey& child_key) {
178 
179  if (pos & 1) child_key[0] = parent_key[0] + center_offset_key;
180  else child_key[0] = parent_key[0] - center_offset_key - (center_offset_key ? 0 : 1);
181  // y-axis
182  if (pos & 2) child_key[1] = parent_key[1] + center_offset_key;
183  else child_key[1] = parent_key[1] - center_offset_key - (center_offset_key ? 0 : 1);
184  // z-axis
185  if (pos & 4) child_key[2] = parent_key[2] + center_offset_key;
186  else child_key[2] = parent_key[2] - center_offset_key - (center_offset_key ? 0 : 1);
187  }
188 
189  /// generate child index (between 0 and 7) from key at given tree depth
190  inline unsigned char computeChildIdx(const OcTreeKey& key, int depth){
191  unsigned char pos = 0;
192  if (key.k[0] & (1 << depth)) pos += 1;
193  if (key.k[1] & (1 << depth)) pos += 2;
194  if (key.k[2] & (1 << depth)) pos += 4;
195  return pos;
196  }
197 
198  /**
199  * Generates a unique key for all keys on a certain level of the tree
200  *
201  * @param level from the bottom (= tree_depth - depth of key)
202  * @param key input indexing key (at lowest resolution / level)
203  * @return key corresponding to the input key at the given level
204  */
205  inline OcTreeKey computeIndexKey(unsigned short int level, const OcTreeKey& key) {
206  unsigned short int mask = 65535 << level;
207  OcTreeKey result = key;
208  result[0] &= mask;
209  result[1] &= mask;
210  result[2] &= mask;
211  return result;
212  }
213 
214 } // namespace
215 
216 #endif
OctoMap: A probabilistic, flexible, and compact 3D mapping library for robotic systems.
unsigned char computeChildIdx(const OcTreeKey &key, int depth)
generate child index (between 0 and 7) from key at given tree depth
Definition: OcTreeKey.h:190
size_t size() const
Definition: OcTreeKey.h:146
Provides a hash function on Keys.
Definition: OcTreeKey.h:97
std::vector< OcTreeKey >::iterator end_of_ray
Definition: OcTreeKey.h:164
Scalar * iterator
Definition: eigen_plugins.h:23
std::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:126
std::vector< OcTreeKey > ray
Definition: OcTreeKey.h:163
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:26
OcTreeKey(unsigned short int a, unsigned short int b, unsigned short int c)
Definition: OcTreeKey.h:72
OcTreeKey(const OcTreeKey &other)
Definition: OcTreeKey.h:74
const Scalar * const_iterator
Definition: eigen_plugins.h:24
void computeChildKey(const unsigned int &pos, const unsigned short int &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:176
bool operator!=(const OcTreeKey &other) const
Definition: OcTreeKey.h:80
void addKey(OcTreeKey &k)
Definition: OcTreeKey.h:140
unsigned short int k[3]
Definition: OcTreeKey.h:94
reverse_iterator rbegin()
Definition: OcTreeKey.h:158
reverse_iterator rend()
Definition: OcTreeKey.h:159
size_t sizeMax() const
Definition: OcTreeKey.h:147
bool operator==(const OcTreeKey &other) const
Definition: OcTreeKey.h:77
std::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:115
const_iterator end() const
Definition: OcTreeKey.h:156
iterator end()
Definition: OcTreeKey.h:154
size_t operator()(const OcTreeKey &key) const
Definition: OcTreeKey.h:98
const unsigned short int & operator[](unsigned int i) const
Definition: OcTreeKey.h:87
std::vector< OcTreeKey >::const_iterator const_iterator
Definition: OcTreeKey.h:150
std::vector< OcTreeKey >::iterator iterator
Definition: OcTreeKey.h:149
std::vector< OcTreeKey >::reverse_iterator reverse_iterator
Definition: OcTreeKey.h:151
OcTreeKey is a container class for internal key addressing.
Definition: OcTreeKey.h:68
iterator begin()
Definition: OcTreeKey.h:153
OcTreeKey & operator=(const OcTreeKey &other)
Definition: OcTreeKey.h:83
const_iterator begin() const
Definition: OcTreeKey.h:155
OcTreeKey computeIndexKey(unsigned short int level, const OcTreeKey &key)
Generates a unique key for all keys on a certain level of the tree.
Definition: OcTreeKey.h:205



Page generated by Doxygen 1.8.11 for MRPT 1.3.2 SVN:Unversioned directory at Sun May 1 08:45:24 UTC 2016