Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #ifndef OCTOMAP_OCTREE_KEY_H
00035 #define OCTOMAP_OCTREE_KEY_H
00036
00037
00038
00039
00040 #include <ciso646>
00041
00042 #include <assert.h>
00043
00044
00045
00046
00047 #if defined(__GNUC__) && ! defined(_LIBCPP_VERSION)
00048 #include <tr1/unordered_set>
00049 #include <tr1/unordered_map>
00050 namespace octomap {
00051 namespace unordered_ns = std::tr1;
00052 };
00053 #else
00054 #include <unordered_set>
00055 #include <unordered_map>
00056 namespace octomap {
00057 namespace unordered_ns = std;
00058 }
00059 #endif
00060
00061 namespace octomap {
00062
00068 class OcTreeKey {
00069
00070 public:
00071 OcTreeKey () {}
00072 OcTreeKey (unsigned short int a, unsigned short int b, unsigned short int c)
00073 { k[0] = a; k[1] = b; k[2] = c; }
00074 OcTreeKey(const OcTreeKey& other){
00075 k[0] = other.k[0]; k[1] = other.k[1]; k[2] = other.k[2];
00076 }
00077 bool operator== (const OcTreeKey &other) const {
00078 return ((k[0] == other[0]) && (k[1] == other[1]) && (k[2] == other[2]));
00079 }
00080 bool operator!= (const OcTreeKey &other) const {
00081 return( (k[0] != other[0]) || (k[1] != other[1]) || (k[2] != other[2]) );
00082 }
00083 OcTreeKey& operator=(const OcTreeKey& other){
00084 k[0] = other.k[0]; k[1] = other.k[1]; k[2] = other.k[2];
00085 return *this;
00086 }
00087 const unsigned short int& operator[] (unsigned int i) const {
00088 return k[i];
00089 }
00090 unsigned short int& operator[] (unsigned int i) {
00091 return k[i];
00092 }
00093
00094 unsigned short int k[3];
00095
00097 struct KeyHash{
00098 size_t operator()(const OcTreeKey& key) const{
00099
00100 return key.k[0] + 1337*key.k[1] + 345637*key.k[2];
00101 }
00102 };
00103
00104 };
00105
00112 typedef unordered_ns::unordered_set<OcTreeKey, OcTreeKey::KeyHash> KeySet;
00113
00119 typedef unordered_ns::unordered_map<OcTreeKey, bool, OcTreeKey::KeyHash> KeyBoolMap;
00120
00121
00122 class KeyRay {
00123 public:
00124
00125 KeyRay () {
00126 ray.resize(100000);
00127 reset();
00128 }
00129 void reset() {
00130 end_of_ray = begin();
00131 }
00132 void addKey(OcTreeKey& k) {
00133 assert(end_of_ray != ray.end());
00134 *end_of_ray = k;
00135 end_of_ray++;
00136 }
00137
00138 unsigned int size() const { return end_of_ray - ray.begin(); }
00139 unsigned int sizeMax() const { return 100000; }
00140
00141 typedef std::vector<OcTreeKey>::iterator iterator;
00142 typedef std::vector<OcTreeKey>::const_iterator const_iterator;
00143 typedef std::vector<OcTreeKey>::reverse_iterator reverse_iterator;
00144
00145 iterator begin() { return ray.begin(); }
00146 iterator end() { return end_of_ray; }
00147 const_iterator begin() const { return ray.begin(); }
00148 const_iterator end() const { return end_of_ray; }
00149
00150 reverse_iterator rbegin() { return (reverse_iterator) end_of_ray; }
00151 reverse_iterator rend() { return ray.rend(); }
00152
00153 public:
00154
00155 std::vector<OcTreeKey> ray;
00156 std::vector<OcTreeKey>::iterator end_of_ray;
00157 };
00158
00168 inline void computeChildKey (const unsigned int& pos, const unsigned short int& center_offset_key,
00169 const OcTreeKey& parent_key, OcTreeKey& child_key) {
00170
00171 if (pos & 1) child_key[0] = parent_key[0] + center_offset_key;
00172 else child_key[0] = parent_key[0] - center_offset_key - (center_offset_key ? 0 : 1);
00173
00174 if (pos & 2) child_key[1] = parent_key[1] + center_offset_key;
00175 else child_key[1] = parent_key[1] - center_offset_key - (center_offset_key ? 0 : 1);
00176
00177 if (pos & 4) child_key[2] = parent_key[2] + center_offset_key;
00178 else child_key[2] = parent_key[2] - center_offset_key - (center_offset_key ? 0 : 1);
00179 }
00180
00182 inline unsigned char computeChildIdx(const OcTreeKey& key, int depth){
00183 unsigned char pos = 0;
00184 if (key.k[0] & (1 << depth)) pos += 1;
00185 if (key.k[1] & (1 << depth)) pos += 2;
00186 if (key.k[2] & (1 << depth)) pos += 4;
00187 return pos;
00188 }
00189
00197 inline OcTreeKey computeIndexKey(unsigned short int level, const OcTreeKey& key) {
00198 if (level == 0)
00199 return key;
00200 else {
00201 unsigned short int mask = 65535 << level;
00202 OcTreeKey result = key;
00203 result[0] &= mask;
00204 result[1] &= mask;
00205 result[2] &= mask;
00206 return result;
00207 }
00208 }
00209
00210 }
00211
00212 #endif