map.h
Go to the documentation of this file.
1 #ifndef MAP_H
2 #define MAP_H
3 #include <gmapping/utils/point.h>
4 #include <assert.h>
7 
8 namespace GMapping {
14 
15 template <class Cell, class Storage, const bool isClass=true>
16 class Map{
17  public:
18  Map(int mapSizeX, int mapSizeY, double delta);
19  Map(const Point& center, double worldSizeX, double worldSizeY, double delta);
20  Map(const Point& center, double xmin, double ymin, double xmax, double ymax, double delta);
21  /* the standard implementation works filen in this case*/
22  //Map(const Map& g);
23  //Map& operator =(const Map& g);
24  void resize(double xmin, double ymin, double xmax, double ymax);
25  void grow(double xmin, double ymin, double xmax, double ymax);
26  inline IntPoint world2map(const Point& p) const;
27  inline Point map2world(const IntPoint& p) const;
28  inline IntPoint world2map(double x, double y) const
29  { return world2map(Point(x,y)); }
30  inline Point map2world(int x, int y) const
31  { return map2world(IntPoint(x,y)); }
32 
33  inline Point getCenter() const {return m_center;}
34  inline double getWorldSizeX() const {return m_worldSizeX;}
35  inline double getWorldSizeY() const {return m_worldSizeY;}
36  inline int getMapSizeX() const {return m_mapSizeX;}
37  inline int getMapSizeY() const {return m_mapSizeY;}
38  inline double getDelta() const { return m_delta;}
39  inline double getMapResolution() const { return m_delta;}
40  inline double getResolution() const { return m_delta;}
41  inline void getSize(double & xmin, double& ymin, double& xmax, double& ymax) const {
43  xmin=min.x, ymin=min.y, xmax=max.x, ymax=max.y;
44  }
45 
46  inline Cell& cell(int x, int y) {
47  return cell(IntPoint(x, y));
48  }
49  inline Cell& cell(const IntPoint& p);
50 
51  inline const Cell& cell(int x, int y) const {
52  return cell(IntPoint(x, y));
53  }
54  inline const Cell& cell(const IntPoint& p) const;
55 
56  inline Cell& cell(double x, double y) {
57  return cell(Point(x, y));
58  }
59  inline Cell& cell(const Point& p);
60 
61  inline const Cell& cell(double x, double y) const {
62  return cell(Point(x, y));
63  }
64 
65  inline bool isInside(int x, int y) const {
66  return m_storage.cellState(IntPoint(x,y))&Inside;
67  }
68  inline bool isInside(const IntPoint& p) const {
69  return m_storage.cellState(p)&Inside;
70  }
71 
72  inline bool isInside(double x, double y) const {
73  return m_storage.cellState(world2map(x,y))&Inside;
74  }
75  inline bool isInside(const Point& p) const {
76  return m_storage.cellState(world2map(p))&Inside;
77  }
78 
79  inline const Cell& cell(const Point& p) const;
80 
81  inline Storage& storage() { return m_storage; }
82  inline const Storage& storage() const { return m_storage; }
83  DoubleArray2D* toDoubleArray() const;
85 
86  protected:
89  Storage m_storage;
92  static const Cell m_unknown;
93 };
94 
96 
97 template <class Cell, class Storage, const bool isClass>
98  const Cell Map<Cell,Storage,isClass>::m_unknown = Cell(-1);
99 
100 template <class Cell, class Storage, const bool isClass>
101 Map<Cell,Storage,isClass>::Map(int mapSizeX, int mapSizeY, double delta):
102  m_storage(mapSizeX, mapSizeY){
103  m_worldSizeX=mapSizeX * delta;
104  m_worldSizeY=mapSizeY * delta;
105  m_delta=delta;
106  m_center=Point(0.5*m_worldSizeX, 0.5*m_worldSizeY);
107  m_sizeX2=m_mapSizeX>>1;
108  m_sizeY2=m_mapSizeY>>1;
109 }
110 
111 template <class Cell, class Storage, const bool isClass>
112 Map<Cell,Storage,isClass>::Map(const Point& center, double worldSizeX, double worldSizeY, double delta):
113  m_storage((int)ceil(worldSizeX/delta), (int)ceil(worldSizeY/delta)){
114  m_center=center;
115  m_worldSizeX=worldSizeX;
116  m_worldSizeY=worldSizeY;
117  m_delta=delta;
118  m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
119  m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
120  m_sizeX2=m_mapSizeX>>1;
121  m_sizeY2=m_mapSizeY>>1;
122 }
123 
124 template <class Cell, class Storage, const bool isClass>
125 Map<Cell,Storage,isClass>::Map(const Point& center, double xmin, double ymin, double xmax, double ymax, double delta):
126  m_storage((int)ceil((xmax-xmin)/delta), (int)ceil((ymax-ymin)/delta)){
127  m_center=center;
128  m_worldSizeX=xmax-xmin;
129  m_worldSizeY=ymax-ymin;
130  m_delta=delta;
131  m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
132  m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
133  m_sizeX2=(int)round((m_center.x-xmin)/m_delta);
134  m_sizeY2=(int)round((m_center.y-ymin)/m_delta);
135 }
136 
137 template <class Cell, class Storage, const bool isClass>
138 void Map<Cell,Storage,isClass>::resize(double xmin, double ymin, double xmax, double ymax){
139  IntPoint imin=world2map(xmin, ymin);
140  IntPoint imax=world2map(xmax, ymax);
141  int pxmin, pymin, pxmax, pymax;
142  pxmin=(int)floor((float)imin.x/(1<<m_storage.getPatchMagnitude()));
143  pxmax=(int)ceil((float)imax.x/(1<<m_storage.getPatchMagnitude()));
144  pymin=(int)floor((float)imin.y/(1<<m_storage.getPatchMagnitude()));
145  pymax=(int)ceil((float)imax.y/(1<<m_storage.getPatchMagnitude()));
146  m_storage.resize(pxmin, pymin, pxmax, pymax);
147  m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
148  m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
149  m_worldSizeX=xmax-xmin;
150  m_worldSizeY=ymax-ymin;
151  m_sizeX2-=pxmin*(1<<m_storage.getPatchMagnitude());
152  m_sizeY2-=pymin*(1<<m_storage.getPatchMagnitude());
153 }
154 
155 template <class Cell, class Storage, const bool isClass>
156 void Map<Cell,Storage,isClass>::grow(double xmin, double ymin, double xmax, double ymax){
157  IntPoint imin=world2map(xmin, ymin);
158  IntPoint imax=world2map(xmax, ymax);
159  if (isInside(imin) && isInside(imax))
160  return;
161  imin=min(imin, IntPoint(0,0));
162  imax=max(imax, IntPoint(m_mapSizeX-1,m_mapSizeY-1));
163  int pxmin, pymin, pxmax, pymax;
164  pxmin=(int)floor((float)imin.x/(1<<m_storage.getPatchMagnitude()));
165  pxmax=(int)ceil((float)imax.x/(1<<m_storage.getPatchMagnitude()));
166  pymin=(int)floor((float)imin.y/(1<<m_storage.getPatchMagnitude()));
167  pymax=(int)ceil((float)imax.y/(1<<m_storage.getPatchMagnitude()));
168  m_storage.resize(pxmin, pymin, pxmax, pymax);
169  m_mapSizeX=m_storage.getXSize()<<m_storage.getPatchSize();
170  m_mapSizeY=m_storage.getYSize()<<m_storage.getPatchSize();
171  m_worldSizeX=xmax-xmin;
172  m_worldSizeY=ymax-ymin;
173  m_sizeX2-=pxmin*(1<<m_storage.getPatchMagnitude());
174  m_sizeY2-=pymin*(1<<m_storage.getPatchMagnitude());
175 }
176 
177 
178 template <class Cell, class Storage, const bool isClass>
180  return IntPoint( (int)round((p.x-m_center.x)/m_delta)+m_sizeX2, (int)round((p.y-m_center.y)/m_delta)+m_sizeY2);
181 }
182 
183 template <class Cell, class Storage, const bool isClass>
185  return Point( (p.x-m_sizeX2)*m_delta,
186  (p.y-m_sizeY2)*m_delta)+m_center;
187 }
188 
189 
190 template <class Cell, class Storage, const bool isClass>
192  AccessibilityState s=m_storage.cellState(p);
193  if (! s&Inside)
194  assert(0);
195  //if (s&Allocated) return m_storage.cell(p); assert(0);
196 
197  // this will never happend. Just to satify the compiler..
198  return m_storage.cell(p);
199 
200 }
201 
202 template <class Cell, class Storage, const bool isClass>
204  IntPoint ip=world2map(p);
205  AccessibilityState s=m_storage.cellState(ip);
206  if (! s&Inside)
207  assert(0);
208  //if (s&Allocated) return m_storage.cell(ip); assert(0);
209 
210  // this will never happend. Just to satify the compiler..
211  return m_storage.cell(ip);
212 }
213 
214 template <class Cell, class Storage, const bool isClass>
215  const Cell& Map<Cell,Storage,isClass>::cell(const IntPoint& p) const {
216  AccessibilityState s=m_storage.cellState(p);
217  //if (! s&Inside) assert(0);
218  if (s&Allocated)
219  return m_storage.cell(p);
220  return m_unknown;
221 }
222 
223 template <class Cell, class Storage, const bool isClass>
224 const Cell& Map<Cell,Storage,isClass>::cell(const Point& p) const {
225  IntPoint ip=world2map(p);
226  AccessibilityState s=m_storage.cellState(ip);
227  //if (! s&Inside) assert(0);
228  if (s&Allocated)
229  return m_storage.cell(ip);
230  return m_unknown;
231 }
232 
233 
234 //FIXME check why the last line of the map is corrupted.
235 template <class Cell, class Storage, const bool isClass>
237  DoubleArray2D* darr=new DoubleArray2D(getMapSizeX()-1, getMapSizeY()-1);
238  for(int x=0; x<getMapSizeX()-1; x++)
239  for(int y=0; y<getMapSizeY()-1; y++){
240  IntPoint p(x,y);
241  darr->cell(p)=cell(p);
242  }
243  return darr;
244 }
245 
246 
247 template <class Cell, class Storage, const bool isClass>
249 //FIXME size the map so that m_center will be setted accordingly
250  Point pmin=map2world(IntPoint(0,0));
251  Point pmax=map2world(getMapSizeX()-1,getMapSizeY()-1);
252  Point center=(pmax+pmin)*0.5;
253  Map<double, DoubleArray2D, false>* plainMap=new Map<double, DoubleArray2D, false>(center, (pmax-pmin).x, (pmax-pmin).y, getDelta());
254  for(int x=0; x<getMapSizeX()-1; x++)
255  for(int y=0; y<getMapSizeY()-1; y++){
256  IntPoint p(x,y);
257  plainMap->cell(p)=cell(p);
258  }
259  return plainMap;
260 }
261 
262 };
263 
264 #endif
265 
const char *const *argv double delta
Definition: gfs2stream.cpp:19
Point map2world(const IntPoint &p) const
Definition: map.h:184
Cell & cell(double x, double y)
Definition: map.h:56
point< T > min(const point< T > &p1, const point< T > &p2)
Definition: point.h:154
double getDelta() const
Definition: map.h:38
Cell & cell(int x, int y)
Definition: map.h:46
point< double > Point
Definition: point.h:202
bool isInside(const Point &p) const
Definition: map.h:75
bool isInside(const IntPoint &p) const
Definition: map.h:68
Map(int mapSizeX, int mapSizeY, double delta)
Definition: map.h:101
point< int > IntPoint
Definition: point.h:201
Point map2world(int x, int y) const
Definition: map.h:30
static const Cell m_unknown
Definition: map.h:92
int m_sizeY2
Definition: map.h:91
point< T > max(const point< T > &p1, const point< T > &p2)
Definition: point.h:146
double getWorldSizeX() const
Definition: map.h:34
int m_mapSizeX
Definition: map.h:90
int getMapSizeY() const
Definition: map.h:37
int m_mapSizeY
Definition: map.h:90
int m_sizeX2
Definition: map.h:91
Point getCenter() const
Definition: map.h:33
void getSize(double &xmin, double &ymin, double &xmax, double &ymax) const
Definition: map.h:41
Map< double, DoubleArray2D, false > * toDoubleMap() const
Definition: map.h:248
AccessibilityState
Definition: accessstate.h:5
bool isInside(double x, double y) const
Definition: map.h:72
int getMapSizeX() const
Definition: map.h:36
double m_delta
Definition: map.h:88
double getResolution() const
Definition: map.h:40
Storage m_storage
Definition: map.h:89
double m_worldSizeX
Definition: map.h:88
const Cell & cell(double x, double y) const
Definition: map.h:61
Point m_center
Definition: map.h:87
double min(double a, double b)
Definition: gfs2img.cpp:18
const Cell & cell(int x, int y) const
Definition: map.h:51
DoubleArray2D * toDoubleArray() const
Definition: map.h:236
double m_worldSizeY
Definition: map.h:88
IntPoint world2map(double x, double y) const
Definition: map.h:28
void grow(double xmin, double ymin, double xmax, double ymax)
Definition: map.h:156
void resize(double xmin, double ymin, double xmax, double ymax)
Definition: map.h:138
double getMapResolution() const
Definition: map.h:39
Map< double, DoubleArray2D, false > DoubleMap
Definition: map.h:95
double max(double a, double b)
Definition: gfs2img.cpp:22
IntPoint world2map(const Point &p) const
Definition: map.h:179
const Cell & cell(int x, int y) const
Definition: array2d.h:169
const Storage & storage() const
Definition: map.h:82
Array2D< double > DoubleArray2D
Definition: map.h:13
double getWorldSizeY() const
Definition: map.h:35
bool isInside(int x, int y) const
Definition: map.h:65
Storage & storage()
Definition: map.h:81


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Mon Feb 28 2022 22:59:20