GridMapBase.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00013 //       group, TU Darmstadt nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #ifndef __GridMapBase_h_
00030 #define __GridMapBase_h_
00031 
00032 #include <Eigen/Geometry>
00033 #include <Eigen/LU>
00034 
00035 #include "MapDimensionProperties.h"
00036 
00037 namespace hectorslam {
00038 
00043 template<typename ConcreteCellType>
00044 class GridMapBase
00045 {
00046 
00047 public:
00048 
00049   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00050 
00055   bool hasGridValue(int x, int y) const
00056   {
00057     return (x >= 0) && (y >= 0) && (x < this->getSizeX()) && (y < this->getSizeY());
00058   }
00059 
00060   const Eigen::Vector2i& getMapDimensions() const { return mapDimensionProperties.getMapDimensions(); };
00061   int getSizeX() const { return mapDimensionProperties.getSizeX(); };
00062   int getSizeY() const { return mapDimensionProperties.getSizeY(); };
00063 
00064   bool pointOutOfMapBounds(const Eigen::Vector2f& pointMapCoords) const
00065   {
00066     return mapDimensionProperties.pointOutOfMapBounds(pointMapCoords);
00067   }
00068 
00069   virtual void reset()
00070   {
00071     this->clear();
00072   }
00073 
00077   void clear()
00078   {
00079     int size = this->getSizeX() * this->getSizeY();
00080 
00081     for (int i = 0; i < size; ++i) {
00082       this->mapArray[i].resetGridCell();
00083     }
00084 
00085     //this->mapArray[0].set(1.0f);
00086     //this->mapArray[size-1].set(1.0f);
00087   }
00088 
00089 
00090   const MapDimensionProperties& getMapDimProperties() const { return mapDimensionProperties; };
00091 
00095   GridMapBase(float mapResolution, const Eigen::Vector2i& size, const Eigen::Vector2f& offset)
00096     : mapArray(0)
00097     , lastUpdateIndex(-1)
00098   {
00099     Eigen::Vector2i newMapDimensions (size);
00100 
00101     this->setMapGridSize(newMapDimensions);
00102     sizeX = size[0];
00103 
00104     setMapTransformation(offset, mapResolution);
00105 
00106     this->clear();
00107   }
00108 
00112   virtual ~GridMapBase()
00113   {
00114     deleteArray();
00115   }
00116 
00120   void allocateArray(const Eigen::Vector2i& newMapDims)
00121   {
00122     int sizeX = newMapDims.x();
00123     int sizeY = newMapDims.y();
00124 
00125     mapArray = new ConcreteCellType [sizeX*sizeY];
00126 
00127     mapDimensionProperties.setMapCellDims(newMapDims);
00128   }
00129 
00130   void deleteArray()
00131   {
00132     if (mapArray != 0){
00133 
00134       delete[] mapArray;
00135 
00136       mapArray = 0;
00137       mapDimensionProperties.setMapCellDims(Eigen::Vector2i(-1,-1));
00138     }
00139   }
00140 
00141   ConcreteCellType& getCell(int x, int y)
00142   {
00143     return mapArray[y * sizeX + x];
00144   }
00145 
00146   const ConcreteCellType& getCell(int x, int y) const
00147   {
00148     return mapArray[y * sizeX + x];
00149   }
00150 
00151   ConcreteCellType& getCell(int index)
00152   {
00153     return mapArray[index];
00154   }
00155 
00156   const ConcreteCellType& getCell(int index) const
00157   {
00158     return mapArray[index];
00159   }
00160 
00161   void setMapGridSize(const Eigen::Vector2i& newMapDims)
00162   {
00163     if (newMapDims != mapDimensionProperties.getMapDimensions() ){
00164      deleteArray();
00165      allocateArray(newMapDims);
00166      this->reset();
00167     }
00168   }
00169 
00173   GridMapBase(const GridMapBase& other)
00174   {
00175     allocateArray(other.getMapDimensions());
00176     *this = other;
00177   }
00178 
00182   GridMapBase& operator=(const GridMapBase& other)
00183   {
00184     if ( !(this->mapDimensionProperties == other.mapDimensionProperties)){
00185       this->setMapGridSize(other.mapDimensionProperties.getMapDimensions());
00186     }
00187 
00188     this->mapDimensionProperties = other.mapDimensionProperties;
00189 
00190     this->worldTmap = other.worldTmap;
00191     this->mapTworld = other.mapTworld;
00192     this->worldTmap3D = other.worldTmap3D;
00193 
00194     this->scaleToMap = other.scaleToMap;
00195 
00196     //@todo potential resize
00197     int sizeX = this->getSizeX();
00198     int sizeY = this->getSizeY();
00199 
00200     size_t concreteCellSize = sizeof(ConcreteCellType);
00201 
00202     memcpy(this->mapArray, other.mapArray, sizeX*sizeY*concreteCellSize);
00203 
00204     return *this;
00205   }
00206 
00210   inline Eigen::Vector2f getWorldCoords(const Eigen::Vector2f& mapCoords) const
00211   {
00212     return worldTmap * mapCoords;
00213   }
00214 
00218   inline Eigen::Vector2f getMapCoords(const Eigen::Vector2f& worldCoords) const
00219   {
00220     return mapTworld * worldCoords;
00221   }
00222 
00226   inline Eigen::Vector3f getWorldCoordsPose(const Eigen::Vector3f& mapPose) const
00227   {
00228     Eigen::Vector2f worldCoords (worldTmap * mapPose.head<2>());
00229     return Eigen::Vector3f(worldCoords[0], worldCoords[1], mapPose[2]);
00230   }
00231 
00235   inline Eigen::Vector3f getMapCoordsPose(const Eigen::Vector3f& worldPose) const
00236   {
00237     Eigen::Vector2f mapCoords (mapTworld * worldPose.head<2>());
00238     return Eigen::Vector3f(mapCoords[0], mapCoords[1], worldPose[2]);
00239   }
00240 
00241   void setDimensionProperties(const Eigen::Vector2f& topLeftOffsetIn, const Eigen::Vector2i& mapDimensionsIn, float cellLengthIn)
00242   {
00243     setDimensionProperties(MapDimensionProperties(topLeftOffsetIn,mapDimensionsIn,cellLengthIn));
00244   }
00245 
00246   void setDimensionProperties(const MapDimensionProperties& newMapDimProps)
00247   {
00248     //Grid map cell number has changed
00249     if (!newMapDimProps.hasEqualDimensionProperties(this->mapDimensionProperties)){
00250       this->setMapGridSize(newMapDimProps.getMapDimensions());
00251     }
00252 
00253     //Grid map transformation/cell size has changed
00254     if(!newMapDimProps.hasEqualTransformationProperties(this->mapDimensionProperties)){
00255       this->setMapTransformation(newMapDimProps.getTopLeftOffset(), newMapDimProps.getCellLength());
00256     }
00257   }
00258 
00265   void setMapTransformation(const Eigen::Vector2f& topLeftOffset, float cellLength)
00266   {
00267     mapDimensionProperties.setCellLength(cellLength);
00268     mapDimensionProperties.setTopLeftOffset(topLeftOffset);
00269 
00270     scaleToMap = 1.0f / cellLength;
00271 
00272     mapTworld = Eigen::AlignedScaling2f(scaleToMap, scaleToMap) * Eigen::Translation2f(topLeftOffset[0], topLeftOffset[1]);
00273 
00274     worldTmap3D = Eigen::AlignedScaling3f(scaleToMap, scaleToMap, 1.0f) * Eigen::Translation3f(topLeftOffset[0], topLeftOffset[1], 0);
00275 
00276     //std::cout << worldTmap3D.matrix() << std::endl;
00277     worldTmap3D = worldTmap3D.inverse();
00278 
00279     worldTmap = mapTworld.inverse();
00280   }
00281 
00282 
00287   float getScaleToMap() const
00288   {
00289     return scaleToMap;
00290   }
00291 
00296   float getCellLength() const
00297   {
00298     return mapDimensionProperties.getCellLength();
00299   }
00300 
00305   const Eigen::Affine2f& getWorldTmap() const
00306   {
00307     return worldTmap;
00308   }
00309 
00314   const Eigen::Affine3f& getWorldTmap3D() const
00315   {
00316     return worldTmap3D;
00317   }
00318 
00323   const Eigen::Affine2f& getMapTworld() const
00324   {
00325     return mapTworld;
00326   }
00327 
00328   void setUpdated() { lastUpdateIndex++; };
00329   int getUpdateIndex() const { return lastUpdateIndex; };
00330 
00334   bool getMapExtends(int& xMax, int& yMax, int& xMin, int& yMin) const
00335   {
00336     int lowerStart = -1;
00337     int upperStart = 10000;
00338 
00339     int xMaxTemp = lowerStart;
00340     int yMaxTemp = lowerStart;
00341     int xMinTemp = upperStart;
00342     int yMinTemp = upperStart;
00343 
00344     int sizeX = this->getSizeX();
00345     int sizeY = this->getSizeY();
00346 
00347     for (int x = 0; x < sizeX; ++x) {
00348       for (int y = 0; y < sizeY; ++y) {
00349         if (this->mapArray[x][y].getValue() != 0.0f) {
00350 
00351           if (x > xMaxTemp) {
00352             xMaxTemp = x;
00353           }
00354 
00355           if (x < xMinTemp) {
00356             xMinTemp = x;
00357           }
00358 
00359           if (y > yMaxTemp) {
00360             yMaxTemp = y;
00361           }
00362 
00363           if (y < yMinTemp) {
00364             yMinTemp = y;
00365           }
00366         }
00367       }
00368     }
00369 
00370     if ((xMaxTemp != lowerStart) &&
00371         (yMaxTemp != lowerStart) &&
00372         (xMinTemp != upperStart) &&
00373         (yMinTemp != upperStart)) {
00374 
00375       xMax = xMaxTemp;
00376       yMax = yMaxTemp;
00377       xMin = xMinTemp;
00378       yMin = yMinTemp;
00379       return true;
00380     } else {
00381       return false;
00382     }
00383   }
00384 
00385 protected:
00386 
00387   ConcreteCellType *mapArray;    
00388 
00389   float scaleToMap;              
00390 
00391   Eigen::Affine2f worldTmap;     
00392   Eigen::Affine3f worldTmap3D;   
00393   Eigen::Affine2f mapTworld;     
00394 
00395   MapDimensionProperties mapDimensionProperties;
00396   int sizeX;
00397 
00398 private:
00399   int lastUpdateIndex;
00400 };
00401 
00402 }
00403 
00404 #endif


hector_mapping
Author(s): Stefan Kohlbrecher
autogenerated on Mon Oct 6 2014 00:33:51