OcTreeKey.h
Go to the documentation of this file.
00001 /*
00002  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
00003  * http://octomap.github.com/
00004  *
00005  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
00006  * All rights reserved.
00007  * License: New BSD
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions are met:
00011  *
00012  *     * Redistributions of source code must retain the above copyright
00013  *       notice, this list of conditions and the following disclaimer.
00014  *     * Redistributions in binary form must reproduce the above copyright
00015  *       notice, this list of conditions and the following disclaimer in the
00016  *       documentation and/or other materials provided with the distribution.
00017  *     * Neither the name of the University of Freiburg nor the names of its
00018  *       contributors may be used to endorse or promote products derived from
00019  *       this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00023  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00024  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00025  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00026  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00027  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00028  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00029  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00030  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  * POSSIBILITY OF SUCH DAMAGE.
00032  */
00033 
00034 #ifndef OCTOMAP_OCTREE_KEY_H
00035 #define OCTOMAP_OCTREE_KEY_H
00036 
00037 /* According to c++ standard including this header has no practical effect
00038  * but it can be used to determine the c++ standard library implementation.
00039  */
00040 #include <ciso646>
00041 
00042 #include <assert.h>
00043 
00044 /* Libc++ does not implement the TR1 namespace, all c++11 related functionality
00045  * is instead implemented in the std namespace.
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         // a hashing function 
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     // x-axis
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     // y-axis
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     // z-axis
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 } // namespace
00211 
00212 #endif


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Aug 27 2015 14:13:14