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>
5 #include "gmapping/grid/accessstate.h"
6 #include "gmapping/grid/array2d.h"
7 
8 namespace GMapping {
13 typedef Array2D<double> DoubleArray2D;
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 {
42  Point min=map2world(0,0), max=map2world(IntPoint(m_mapSizeX-1, m_mapSizeY-1));
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;
84  Map<double, DoubleArray2D, false>* toDoubleMap() const;
85 
86  protected:
87  Point m_center;
88  double m_worldSizeX, m_worldSizeY, m_delta;
89  Storage m_storage;
90  int m_mapSizeX, m_mapSizeY;
91  int m_sizeX2, m_sizeY2;
92  static const Cell m_unknown;
93 };
94 
95 typedef Map<double, DoubleArray2D, false> DoubleMap;
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>
179 IntPoint Map<Cell,Storage,isClass>::world2map(const Point& p) const{
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>
184 Point Map<Cell,Storage,isClass>::map2world(const IntPoint& p) const{
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>
191 Cell& Map<Cell,Storage,isClass>::cell(const IntPoint& p) {
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>
203 Cell& Map<Cell,Storage,isClass>::cell(const Point& p) {
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>
236 DoubleArray2D* Map<Cell,Storage,isClass>::toDoubleArray() const{
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>
248 Map<double, DoubleArray2D, false>* Map<Cell,Storage,isClass>::toDoubleMap() const{
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 
min
double min(double a, double b)
Definition: gfs2img.cpp:18
GMapping::Array2D::cell
const Cell & cell(int x, int y) const
Definition: array2d.h:169
GMapping::Map::world2map
IntPoint world2map(double x, double y) const
Definition: map.h:28
GMapping::Map::getMapSizeY
int getMapSizeY() const
Definition: map.h:37
point.h
GMapping::Map::m_sizeY2
int m_sizeY2
Definition: map.h:91
GMapping::Map::m_unknown
static const Cell m_unknown
Definition: map.h:92
GMapping::Map::isInside
bool isInside(int x, int y) const
Definition: map.h:65
GMapping::Map::getSize
void getSize(double &xmin, double &ymin, double &xmax, double &ymax) const
Definition: map.h:41
GMapping::min
point< T > min(const point< T > &p1, const point< T > &p2)
Definition: point.h:154
GMapping::Map::grow
void grow(double xmin, double ymin, double xmax, double ymax)
Definition: map.h:156
GMapping::Map::toDoubleArray
DoubleArray2D * toDoubleArray() const
Definition: map.h:236
GMapping
Definition: configfile.cpp:34
GMapping::max
point< T > max(const point< T > &p1, const point< T > &p2)
Definition: point.h:146
GMapping::Map::map2world
Point map2world(const IntPoint &p) const
Definition: map.h:184
GMapping::Map::storage
Storage & storage()
Definition: map.h:81
GMapping::DoubleMap
Map< double, DoubleArray2D, false > DoubleMap
Definition: map.h:95
GMapping::Map::m_worldSizeY
double m_worldSizeY
Definition: map.h:88
GMapping::Map::m_center
Point m_center
Definition: map.h:87
delta
const char *const *argv double delta
Definition: gfs2stream.cpp:19
GMapping::Map::getDelta
double getDelta() const
Definition: map.h:38
GMapping::Map
Definition: map.h:16
GMapping::Map::isInside
bool isInside(double x, double y) const
Definition: map.h:72
GMapping::IntPoint
point< int > IntPoint
Definition: point.h:201
GMapping::Map::getMapSizeX
int getMapSizeX() const
Definition: map.h:36
array2d.h
GMapping::Array2D
Definition: array2d.h:12
GMapping::DoubleArray2D
Array2D< double > DoubleArray2D
Definition: map.h:13
GMapping::point::y
T y
Definition: point.h:16
GMapping::Point
point< double > Point
Definition: point.h:202
GMapping::Map::cell
const Cell & cell(int x, int y) const
Definition: map.h:51
GMapping::Map::getMapResolution
double getMapResolution() const
Definition: map.h:39
GMapping::Map::storage
const Storage & storage() const
Definition: map.h:82
GMapping::Map::Map
Map(int mapSizeX, int mapSizeY, double delta)
Definition: map.h:101
GMapping::Map::m_storage
Storage m_storage
Definition: map.h:89
GMapping::Map::getResolution
double getResolution() const
Definition: map.h:40
GMapping::point< double >
GMapping::Map::map2world
Point map2world(int x, int y) const
Definition: map.h:30
accessstate.h
GMapping::Map::world2map
IntPoint world2map(const Point &p) const
Definition: map.h:179
GMapping::Map::m_mapSizeY
int m_mapSizeY
Definition: map.h:90
GMapping::Map::cell
const Cell & cell(double x, double y) const
Definition: map.h:61
GMapping::AccessibilityState
AccessibilityState
Definition: accessstate.h:5
GMapping::Map::toDoubleMap
Map< double, DoubleArray2D, false > * toDoubleMap() const
Definition: map.h:248
GMapping::Map::cell
Cell & cell(int x, int y)
Definition: map.h:46
GMapping::Map::m_sizeX2
int m_sizeX2
Definition: map.h:91
GMapping::Map::m_worldSizeX
double m_worldSizeX
Definition: map.h:88
GMapping::Allocated
@ Allocated
Definition: accessstate.h:5
GMapping::Map::isInside
bool isInside(const Point &p) const
Definition: map.h:75
GMapping::Map::getCenter
Point getCenter() const
Definition: map.h:33
GMapping::Map::resize
void resize(double xmin, double ymin, double xmax, double ymax)
Definition: map.h:138
GMapping::Map::cell
Cell & cell(double x, double y)
Definition: map.h:56
GMapping::Map::getWorldSizeX
double getWorldSizeX() const
Definition: map.h:34
GMapping::Map::isInside
bool isInside(const IntPoint &p) const
Definition: map.h:68
GMapping::Map::m_mapSizeX
int m_mapSizeX
Definition: map.h:90
GMapping::Inside
@ Inside
Definition: accessstate.h:5
GMapping::Map::getWorldSizeY
double getWorldSizeY() const
Definition: map.h:35
GMapping::Map::m_delta
double m_delta
Definition: map.h:88
max
double max(double a, double b)
Definition: gfs2img.cpp:22
GMapping::point::x
T x
Definition: point.h:16

openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Thu Oct 19 2023 02:25:51