map.h
Go to the documentation of this file.
00001 #ifndef MAP_H
00002 #define MAP_H
00003 #include <gmapping/utils/point.h>
00004 #include <assert.h>
00005 #include "accessstate.h"
00006 #include "array2d.h"
00007 
00008 namespace GMapping {
00013 typedef Array2D<double> DoubleArray2D;
00014 
00015 template <class Cell, class Storage, const bool isClass=true> 
00016 class Map{
00017         public:
00018                 Map(int mapSizeX, int mapSizeY, double delta);
00019                 Map(const Point& center, double worldSizeX, double worldSizeY, double delta);
00020                 Map(const Point& center, double xmin, double ymin, double xmax, double ymax, double delta);
00021                 /* the standard implementation works filen in this case*/
00022                 //Map(const Map& g);
00023                 //Map& operator =(const Map& g);
00024                 void resize(double xmin, double ymin, double xmax, double ymax);
00025                 void grow(double xmin, double ymin, double xmax, double ymax);
00026                 inline IntPoint world2map(const Point& p) const;
00027                 inline Point map2world(const IntPoint& p) const;
00028                 inline IntPoint world2map(double x, double y) const 
00029                   { return world2map(Point(x,y)); }
00030                 inline Point map2world(int x, int y) const 
00031                   { return map2world(IntPoint(x,y)); }
00032 
00033                 inline Point getCenter() const {return m_center;}       
00034                 inline double getWorldSizeX() const {return m_worldSizeX;}
00035                 inline double getWorldSizeY() const {return m_worldSizeY;}
00036                 inline int getMapSizeX() const {return m_mapSizeX;}
00037                 inline int getMapSizeY() const {return m_mapSizeY;}
00038                 inline double getDelta() const { return m_delta;}
00039                 inline double getMapResolution() const { return m_delta;}
00040                 inline double getResolution() const { return m_delta;}
00041                 inline void getSize(double & xmin, double& ymin, double& xmax, double& ymax) const {
00042                         Point min=map2world(0,0), max=map2world(IntPoint(m_mapSizeX-1, m_mapSizeY-1)); 
00043                         xmin=min.x, ymin=min.y,  xmax=max.x, ymax=max.y; 
00044                 }
00045                 
00046                 inline Cell& cell(int x, int y) {
00047                   return cell(IntPoint(x, y));
00048                 }
00049                 inline Cell& cell(const IntPoint& p);
00050 
00051                 inline const Cell& cell(int x, int y) const  {
00052                   return cell(IntPoint(x, y));
00053                 }
00054                 inline const Cell& cell(const IntPoint& p) const;
00055 
00056                 inline Cell& cell(double x, double y) {
00057                   return cell(Point(x, y));
00058                 }
00059                 inline Cell& cell(const Point& p);
00060 
00061                 inline const Cell& cell(double x, double y) const {
00062                   return cell(Point(x, y));
00063                 }
00064 
00065                 inline bool isInside(int x, int y) const {
00066                   return m_storage.cellState(IntPoint(x,y))&Inside;
00067                 }
00068                 inline bool isInside(const IntPoint& p) const {
00069                   return m_storage.cellState(p)&Inside;
00070                 }
00071 
00072                 inline bool isInside(double x, double y) const {
00073                   return m_storage.cellState(world2map(x,y))&Inside;
00074                 }
00075                 inline bool isInside(const Point& p) const {
00076                   return m_storage.cellState(world2map(p))&Inside;
00077                 }
00078 
00079                 inline const Cell& cell(const Point& p) const;
00080 
00081                 inline Storage& storage() { return m_storage; }
00082                 inline const Storage& storage() const { return m_storage; }
00083                 DoubleArray2D* toDoubleArray() const;
00084                 Map<double, DoubleArray2D, false>* toDoubleMap() const;
00085                 
00086         protected:
00087                 Point m_center;
00088                 double m_worldSizeX, m_worldSizeY, m_delta;
00089                 Storage m_storage;
00090                 int m_mapSizeX, m_mapSizeY;
00091                 int m_sizeX2, m_sizeY2;
00092         static const Cell m_unknown;
00093 };
00094 
00095 typedef Map<double, DoubleArray2D, false> DoubleMap;
00096 
00097 template <class Cell, class Storage, const bool isClass>
00098   const Cell  Map<Cell,Storage,isClass>::m_unknown = Cell(-1);
00099 
00100 template <class Cell, class Storage, const bool isClass>
00101 Map<Cell,Storage,isClass>::Map(int mapSizeX, int mapSizeY, double delta):
00102         m_storage(mapSizeX, mapSizeY){
00103         m_worldSizeX=mapSizeX * delta;
00104         m_worldSizeY=mapSizeY * delta;
00105         m_delta=delta;
00106         m_center=Point(0.5*m_worldSizeX, 0.5*m_worldSizeY);
00107         m_sizeX2=m_mapSizeX>>1;
00108         m_sizeY2=m_mapSizeY>>1;
00109 }
00110 
00111 template <class Cell, class Storage, const bool isClass>
00112 Map<Cell,Storage,isClass>::Map(const Point& center, double worldSizeX, double worldSizeY, double delta):
00113         m_storage((int)ceil(worldSizeX/delta), (int)ceil(worldSizeY/delta)){
00114         m_center=center;
00115         m_worldSizeX=worldSizeX;
00116         m_worldSizeY=worldSizeY;
00117         m_delta=delta;
00118         m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
00119         m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
00120         m_sizeX2=m_mapSizeX>>1;
00121         m_sizeY2=m_mapSizeY>>1;
00122 }
00123 
00124 template <class Cell, class Storage, const bool isClass>
00125 Map<Cell,Storage,isClass>::Map(const Point& center, double xmin, double ymin, double xmax, double ymax, double delta):
00126         m_storage((int)ceil((xmax-xmin)/delta), (int)ceil((ymax-ymin)/delta)){
00127         m_center=center;
00128         m_worldSizeX=xmax-xmin;
00129         m_worldSizeY=ymax-ymin;
00130         m_delta=delta;
00131         m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
00132         m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
00133         m_sizeX2=(int)round((m_center.x-xmin)/m_delta);
00134         m_sizeY2=(int)round((m_center.y-ymin)/m_delta);
00135 }
00136                 
00137 template <class Cell, class Storage, const bool isClass>
00138 void Map<Cell,Storage,isClass>::resize(double xmin, double ymin, double xmax, double ymax){
00139         IntPoint imin=world2map(xmin, ymin);
00140         IntPoint imax=world2map(xmax, ymax);
00141         int pxmin, pymin, pxmax, pymax;
00142         pxmin=(int)floor((float)imin.x/(1<<m_storage.getPatchMagnitude()));
00143         pxmax=(int)ceil((float)imax.x/(1<<m_storage.getPatchMagnitude()));
00144         pymin=(int)floor((float)imin.y/(1<<m_storage.getPatchMagnitude()));
00145         pymax=(int)ceil((float)imax.y/(1<<m_storage.getPatchMagnitude()));
00146         m_storage.resize(pxmin, pymin, pxmax, pymax);
00147         m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
00148         m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
00149         m_worldSizeX=xmax-xmin;
00150         m_worldSizeY=ymax-ymin;
00151         m_sizeX2-=pxmin*(1<<m_storage.getPatchMagnitude());
00152         m_sizeY2-=pymin*(1<<m_storage.getPatchMagnitude());
00153 }
00154 
00155 template <class Cell, class Storage, const bool isClass>
00156 void Map<Cell,Storage,isClass>::grow(double xmin, double ymin, double xmax, double ymax){
00157         IntPoint imin=world2map(xmin, ymin);
00158         IntPoint imax=world2map(xmax, ymax);
00159         if (isInside(imin) && isInside(imax))
00160                 return;
00161         imin=min(imin, IntPoint(0,0));
00162         imax=max(imax, IntPoint(m_mapSizeX-1,m_mapSizeY-1));
00163         int pxmin, pymin, pxmax, pymax;
00164         pxmin=(int)floor((float)imin.x/(1<<m_storage.getPatchMagnitude()));
00165         pxmax=(int)ceil((float)imax.x/(1<<m_storage.getPatchMagnitude()));
00166         pymin=(int)floor((float)imin.y/(1<<m_storage.getPatchMagnitude()));
00167         pymax=(int)ceil((float)imax.y/(1<<m_storage.getPatchMagnitude()));
00168         m_storage.resize(pxmin, pymin, pxmax, pymax);
00169         m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
00170         m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
00171         m_worldSizeX=xmax-xmin;
00172         m_worldSizeY=ymax-ymin;
00173         m_sizeX2-=pxmin*(1<<m_storage.getPatchMagnitude());
00174         m_sizeY2-=pymin*(1<<m_storage.getPatchMagnitude());
00175 }
00176 
00177 
00178 template <class Cell, class Storage, const bool isClass>
00179 IntPoint Map<Cell,Storage,isClass>::world2map(const Point& p) const{
00180         return IntPoint( (int)round((p.x-m_center.x)/m_delta)+m_sizeX2, (int)round((p.y-m_center.y)/m_delta)+m_sizeY2);
00181 }
00182 
00183 template <class Cell, class Storage, const bool isClass>
00184 Point Map<Cell,Storage,isClass>::map2world(const IntPoint& p) const{
00185         return Point( (p.x-m_sizeX2)*m_delta,
00186                       (p.y-m_sizeY2)*m_delta)+m_center;
00187 }
00188 
00189 
00190 template <class Cell, class Storage, const bool isClass>
00191 Cell& Map<Cell,Storage,isClass>::cell(const IntPoint& p) {
00192         AccessibilityState s=m_storage.cellState(p);
00193         if (! (s&Inside))
00194                 assert(0);
00195         //if (s&Allocated) return m_storage.cell(p); assert(0);
00196 
00197         // this will never happend. Just to satify the compiler..
00198         return m_storage.cell(p);
00199 
00200 }
00201 
00202 template <class Cell, class Storage, const bool isClass>
00203 Cell& Map<Cell,Storage,isClass>::cell(const Point& p) {
00204         IntPoint ip=world2map(p);
00205         AccessibilityState s=m_storage.cellState(ip);
00206         if (! (s&Inside))
00207                 assert(0);
00208         //if (s&Allocated) return m_storage.cell(ip); assert(0);
00209 
00210         // this will never happend. Just to satify the compiler..
00211         return m_storage.cell(ip);
00212 }
00213 
00214 template <class Cell, class Storage, const bool isClass>
00215   const Cell& Map<Cell,Storage,isClass>::cell(const IntPoint& p) const {
00216   AccessibilityState s=m_storage.cellState(p);
00217   //if (! s&Inside) assert(0);
00218   if (s&Allocated)       
00219     return m_storage.cell(p);
00220   return m_unknown;
00221 }
00222 
00223 template <class Cell, class Storage, const bool isClass>
00224 const  Cell& Map<Cell,Storage,isClass>::cell(const Point& p) const {
00225   IntPoint ip=world2map(p);
00226   AccessibilityState s=m_storage.cellState(ip);
00227   //if (! s&Inside) assert(0);
00228   if (s&Allocated)       
00229     return m_storage.cell(ip);
00230   return  m_unknown;
00231 }
00232 
00233 
00234 //FIXME check why the last line of the map is corrupted.
00235 template <class Cell, class Storage, const bool isClass>
00236 DoubleArray2D* Map<Cell,Storage,isClass>::toDoubleArray() const{
00237         DoubleArray2D* darr=new DoubleArray2D(getMapSizeX()-1, getMapSizeY()-1);
00238         for(int x=0; x<getMapSizeX()-1; x++)
00239                 for(int y=0; y<getMapSizeY()-1; y++){
00240                         IntPoint p(x,y);
00241                         darr->cell(p)=cell(p);
00242                 }
00243         return darr;
00244 }
00245 
00246 
00247 template <class Cell, class Storage, const bool isClass>
00248 Map<double, DoubleArray2D, false>* Map<Cell,Storage,isClass>::toDoubleMap() const{
00249 //FIXME size the map so that m_center will be setted accordingly
00250         Point pmin=map2world(IntPoint(0,0));
00251         Point pmax=map2world(getMapSizeX()-1,getMapSizeY()-1);
00252         Point center=(pmax+pmin)*0.5;
00253         Map<double, DoubleArray2D, false>*  plainMap=new Map<double, DoubleArray2D, false>(center, (pmax-pmin).x, (pmax-pmin).y, getDelta());
00254         for(int x=0; x<getMapSizeX()-1; x++)
00255                 for(int y=0; y<getMapSizeY()-1; y++){
00256                         IntPoint p(x,y);
00257                         plainMap->cell(p)=cell(p);
00258                 }
00259         return plainMap;
00260 }
00261 
00262 };
00263 
00264 #endif
00265 


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Thu Jun 6 2019 19:25:13