MRPT  1.9.9
CDynamicGrid.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/core/round.h>
12 #include <vector>
13 #include <string>
14 #include <cmath>
15 #include <cstddef>
16 
17 namespace mrpt
18 {
19 namespace containers
20 {
21 namespace internal
22 {
23 // Aux class.
25 {
26  bool saveToTextFile(const std::string& fileName) const;
27  virtual unsigned int getSizeX() const = 0;
28  virtual unsigned int getSizeY() const = 0;
29  virtual float getCellAsFloat(unsigned int cx, unsigned int cy) const = 0;
30 };
31 } // internal
32 
33 /** A 2D grid of dynamic size which stores any kind of data at each cell.
34  * \tparam T The type of each cell in the 2D grid.
35  * \ingroup mrpt_containers_grp
36  */
37 template <class T>
39 {
40  protected:
41  /** The cells */
42  std::vector<T> m_map;
43  /** Used only from logically const method that really need to modify the
44  * object */
45  inline std::vector<T>& m_map_castaway_const() const
46  {
47  return const_cast<std::vector<T>&>(m_map);
48  }
49 
51  size_t m_size_x, m_size_y;
52 
53  public:
54  /** Constructor */
56  double x_min = -10.0, double x_max = 10.0, double y_min = -10.0f,
57  double y_max = 10.0f, double resolution = 0.10f)
58  : m_map(),
59  m_x_min(),
60  m_x_max(),
61  m_y_min(),
62  m_y_max(),
63  m_resolution(),
64  m_size_x(),
65  m_size_y()
66  {
67  setSize(x_min, x_max, y_min, y_max, resolution);
68  }
69 
70  /** Destructor */
71  virtual ~CDynamicGrid() {}
72  /** Changes the size of the grid, ERASING all previous contents.
73  * If \a fill_value is left as nullptr, the contents of cells may be
74  * undefined (some will remain with
75  * their old values, the new ones will have the default cell value, but
76  * the location of old values
77  * may change wrt their old places).
78  * If \a fill_value is not nullptr, it is assured that all cells will have
79  * a copy of that value after resizing.
80  * \sa resize, fill
81  */
82  void setSize(
83  const double x_min, const double x_max, const double y_min,
84  const double y_max, const double resolution,
85  const T* fill_value = nullptr)
86  {
87  // Adjust sizes to adapt them to full sized cells acording to the
88  // resolution:
89  m_x_min = resolution * round(x_min / resolution);
90  m_y_min = resolution * round(y_min / resolution);
91  m_x_max = resolution * round(x_max / resolution);
92  m_y_max = resolution * round(y_max / resolution);
93 
94  // Res:
95  m_resolution = resolution;
96 
97  // Now the number of cells should be integers:
100 
101  // Cells memory:
102  if (fill_value)
103  m_map.assign(m_size_x * m_size_y, *fill_value);
104  else
105  m_map.resize(m_size_x * m_size_y);
106  }
107 
108  /** Erase the contents of all the cells. */
109  void clear()
110  {
111  m_map.clear();
112  m_map.resize(m_size_x * m_size_y);
113  }
114 
115  /** Fills all the cells with the same value
116  */
117  inline void fill(const T& value)
118  {
119  for (typename std::vector<T>::iterator it = m_map.begin();
120  it != m_map.end(); ++it)
121  *it = value;
122  }
123 
124  /** Changes the size of the grid, maintaining previous contents.
125  * \sa setSize
126  */
127  virtual void resize(
128  double new_x_min, double new_x_max, double new_y_min, double new_y_max,
129  const T& defaultValueNewCells, double additionalMarginMeters = 2.0)
130  {
131  // Is resize really necesary?
132  if (new_x_min >= m_x_min && new_y_min >= m_y_min &&
133  new_x_max <= m_x_max && new_y_max <= m_y_max)
134  return;
135 
136  if (new_x_min > m_x_min) new_x_min = m_x_min;
137  if (new_x_max < m_x_max) new_x_max = m_x_max;
138  if (new_y_min > m_y_min) new_y_min = m_y_min;
139  if (new_y_max < m_y_max) new_y_max = m_y_max;
140 
141  // Additional margin:
142  if (additionalMarginMeters > 0)
143  {
144  if (new_x_min < m_x_min)
145  new_x_min = floor(new_x_min - additionalMarginMeters);
146  if (new_x_max > m_x_max)
147  new_x_max = ceil(new_x_max + additionalMarginMeters);
148  if (new_y_min < m_y_min)
149  new_y_min = floor(new_y_min - additionalMarginMeters);
150  if (new_y_max > m_y_max)
151  new_y_max = ceil(new_y_max + additionalMarginMeters);
152  }
153 
154  // Adjust sizes to adapt them to full sized cells acording to the
155  // resolution:
156  if (fabs(new_x_min / m_resolution - round(new_x_min / m_resolution)) >
157  0.05f)
158  new_x_min = m_resolution * round(new_x_min / m_resolution);
159  if (fabs(new_y_min / m_resolution - round(new_y_min / m_resolution)) >
160  0.05f)
161  new_y_min = m_resolution * round(new_y_min / m_resolution);
162  if (fabs(new_x_max / m_resolution - round(new_x_max / m_resolution)) >
163  0.05f)
164  new_x_max = m_resolution * round(new_x_max / m_resolution);
165  if (fabs(new_y_max / m_resolution - round(new_y_max / m_resolution)) >
166  0.05f)
167  new_y_max = m_resolution * round(new_y_max / m_resolution);
168 
169  // Change the map size: Extensions at each side:
170  unsigned int extra_x_izq = round((m_x_min - new_x_min) / m_resolution);
171  unsigned int extra_y_arr = round((m_y_min - new_y_min) / m_resolution);
172 
173  unsigned int new_size_x = round((new_x_max - new_x_min) / m_resolution);
174  unsigned int new_size_y = round((new_y_max - new_y_min) / m_resolution);
175 
176  // Reserve new memory:
177  typename std::vector<T> new_map;
178  new_map.resize(new_size_x * new_size_y, defaultValueNewCells);
179 
180  // Copy previous rows:
181  unsigned int x, y;
182  typename std::vector<T>::iterator itSrc, itDst;
183  for (y = 0; y < m_size_y; y++)
184  {
185  for (x = 0, itSrc = (m_map.begin() + y * m_size_x),
186  itDst =
187  (new_map.begin() + extra_x_izq +
188  (y + extra_y_arr) * new_size_x);
189  x < m_size_x; ++x, ++itSrc, ++itDst)
190  {
191  *itDst = *itSrc;
192  }
193  }
194 
195  // Update the new map limits:
196  m_x_min = new_x_min;
197  m_x_max = new_x_max;
198  m_y_min = new_y_min;
199  m_y_max = new_y_max;
200 
201  m_size_x = new_size_x;
202  m_size_y = new_size_y;
203 
204  // Keep the new map only:
205  m_map.swap(new_map);
206  }
207 
208  /** Returns a pointer to the contents of a cell given by its coordinates, or
209  * nullptr if it is out of the map extensions.
210  */
211  inline T* cellByPos(double x, double y)
212  {
213  const int cx = x2idx(x);
214  const int cy = y2idx(y);
215  if (cx < 0 || cx >= static_cast<int>(m_size_x)) return nullptr;
216  if (cy < 0 || cy >= static_cast<int>(m_size_y)) return nullptr;
217  return &m_map[cx + cy * m_size_x];
218  }
219  /** \overload */
220  inline const T* cellByPos(double x, double y) const
221  {
222  const int cx = x2idx(x);
223  const int cy = y2idx(y);
224  if (cx < 0 || cx >= static_cast<int>(m_size_x)) return nullptr;
225  if (cy < 0 || cy >= static_cast<int>(m_size_y)) return nullptr;
226  return &m_map[cx + cy * m_size_x];
227  }
228 
229  /** Returns a pointer to the contents of a cell given by its cell indexes,
230  * or nullptr if it is out of the map extensions.
231  */
232  inline T* cellByIndex(unsigned int cx, unsigned int cy)
233  {
234  if (cx >= m_size_x || cy >= m_size_y)
235  return nullptr;
236  else
237  return &m_map[cx + cy * m_size_x];
238  }
239 
240  /** Returns a pointer to the contents of a cell given by its cell indexes,
241  * or nullptr if it is out of the map extensions.
242  */
243  inline const T* cellByIndex(unsigned int cx, unsigned int cy) const
244  {
245  if (cx >= m_size_x || cy >= m_size_y)
246  return nullptr;
247  else
248  return &m_map[cx + cy * m_size_x];
249  }
250 
251  /** Returns the horizontal size of grid map in cells count */
252  inline size_t getSizeX() const { return m_size_x; }
253  /** Returns the vertical size of grid map in cells count */
254  inline size_t getSizeY() const { return m_size_y; }
255  /** Returns the "x" coordinate of left side of grid map */
256  inline double getXMin() const { return m_x_min; }
257  /** Returns the "x" coordinate of right side of grid map */
258  inline double getXMax() const { return m_x_max; }
259  /** Returns the "y" coordinate of top side of grid map */
260  inline double getYMin() const { return m_y_min; }
261  /** Returns the "y" coordinate of bottom side of grid map */
262  inline double getYMax() const { return m_y_max; }
263  /** Returns the resolution of the grid map */
264  inline double getResolution() const { return m_resolution; }
265  /** Transform a coordinate values into cell indexes */
266  inline int x2idx(double x) const
267  {
268  return static_cast<int>((x - m_x_min) / m_resolution);
269  }
270  inline int y2idx(double y) const
271  {
272  return static_cast<int>((y - m_y_min) / m_resolution);
273  }
274  inline int xy2idx(double x, double y) const
275  {
276  return x2idx(x) + y2idx(y) * m_size_x;
277  }
278 
279  /** Transform a global (linear) cell index value into its corresponding
280  * (x,y) cell indexes. */
281  inline void idx2cxcy(const int& idx, int& cx, int& cy) const
282  {
283  cx = idx % m_size_x;
284  cy = idx / m_size_x;
285  }
286 
287  /** Transform a cell index into a coordinate value of the cell central point
288  */
289  inline double idx2x(int cx) const
290  {
291  return m_x_min + (cx + 0.5) * m_resolution;
292  }
293  inline double idx2y(int cy) const
294  {
295  return m_y_min + (cy + 0.5) * m_resolution;
296  }
297 
298  /** Get the entire grid as a matrix.
299  * \tparam MAT The type of the matrix, typically a
300  * mrpt::math::CMatrixDouble.
301  * \param[out] m The output matrix; will be set automatically to the
302  * correct size.
303  * Entry (cy,cx) in the matrix contains the grid cell with indices
304  * (cx,cy).
305  * \note This method will compile only for cell types that can be
306  * converted to the type of the matrix elements (e.g. double).
307  */
308  template <class MAT>
309  void getAsMatrix(MAT& m) const
310  {
311  m.setSize(m_size_y, m_size_x);
312  if (m_map.empty()) return;
313  const T* c = &m_map[0];
314  for (size_t cy = 0; cy < m_size_y; cy++)
315  for (size_t cx = 0; cx < m_size_x; cx++) m.set_unsafe(cy, cx, *c++);
316  }
317 
318  /** The user must implement this in order to provide "saveToTextFile" a way
319  * to convert each cell into a numeric value */
320  virtual float cell2float(const T&) const { return 0; }
321  /** Saves a float representation of the grid (via "cell2float()") to a text
322  * file. \return false on error */
323  bool saveToTextFile(const std::string& fileName) const
324  {
325  struct aux_saver : public internal::dynamic_grid_txt_saver
326  {
327  aux_saver(const CDynamicGrid<T>& obj) : m_obj(obj) {}
328  virtual unsigned int getSizeX() const { return m_obj.getSizeX(); }
329  virtual unsigned int getSizeY() const { return m_obj.getSizeY(); }
330  virtual float getCellAsFloat(unsigned int cx, unsigned int cy) const
331  {
332  return m_obj.cell2float(
333  m_obj.m_map[cx + cy * m_obj.getSizeX()]);
334  }
335  const CDynamicGrid<T>& m_obj;
336  };
337  aux_saver aux(*this);
338  return aux.saveToTextFile(fileName);
339  }
340 
341  protected:
342  template <class STREAM>
343  void dyngridcommon_writeToStream(STREAM& out) const
344  {
345  out << m_x_min << m_x_max << m_y_min << m_y_max;
346  out << m_resolution;
347  out << static_cast<uint32_t>(m_size_x)
348  << static_cast<uint32_t>(m_size_y);
349  }
350  template <class STREAM>
351  void dyngridcommon_readFromStream(STREAM& in, bool cast_from_float = false)
352  {
353  if (!cast_from_float)
354  {
355  in >> m_x_min >> m_x_max >> m_y_min >> m_y_max;
356  in >> m_resolution;
357  }
358  else
359  {
360  float xmin, xmax, ymin, ymax, res;
361  in >> xmin >> xmax >> ymin >> ymax >> res;
362  m_x_min = xmin;
363  m_x_max = xmax;
364  m_y_min = ymin;
365  m_y_max = ymax;
366  m_resolution = res;
367  }
368  uint32_t nX, nY;
369  in >> nX >> nY;
370  m_size_x = nX;
371  m_size_y = nY;
372  m_map.resize(nX * nY);
373  }
374 
375 }; // end of CDynamicGrid<>
376 
377 } // End of namespace
378 } // end of namespace
Scalar * iterator
Definition: eigen_plugins.h:26
std::vector< T > m_map
The cells.
Definition: CDynamicGrid.h:42
void clear()
Erase the contents of all the cells.
Definition: CDynamicGrid.h:109
double getYMax() const
Returns the "y" coordinate of bottom side of grid map.
Definition: CDynamicGrid.h:262
int y2idx(double y) const
Definition: CDynamicGrid.h:270
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:38
void fill(const T &value)
Fills all the cells with the same value.
Definition: CDynamicGrid.h:117
double getResolution() const
Returns the resolution of the grid map.
Definition: CDynamicGrid.h:264
double getYMin() const
Returns the "y" coordinate of top side of grid map.
Definition: CDynamicGrid.h:260
void dyngridcommon_writeToStream(STREAM &out) const
Definition: CDynamicGrid.h:343
double idx2x(int cx) const
Transform a cell index into a coordinate value of the cell central point.
Definition: CDynamicGrid.h:289
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
std::vector< T > & m_map_castaway_const() const
Used only from logically const method that really need to modify the object.
Definition: CDynamicGrid.h:45
double idx2y(int cy) const
Definition: CDynamicGrid.h:293
void setSize(const double x_min, const double x_max, const double y_min, const double y_max, const double resolution, const T *fill_value=nullptr)
Changes the size of the grid, ERASING all previous contents.
Definition: CDynamicGrid.h:82
int xy2idx(double x, double y) const
Definition: CDynamicGrid.h:274
double getXMin() const
Returns the "x" coordinate of left side of grid map.
Definition: CDynamicGrid.h:256
const GLubyte * c
Definition: glext.h:6313
bool saveToTextFile(const std::string &fileName) const
virtual float cell2float(const T &) const
The user must implement this in order to provide "saveToTextFile" a way to convert each cell into a n...
Definition: CDynamicGrid.h:320
T * cellByPos(double x, double y)
Returns a pointer to the contents of a cell given by its coordinates, or nullptr if it is out of the ...
Definition: CDynamicGrid.h:211
virtual void resize(double new_x_min, double new_x_max, double new_y_min, double new_y_max, const T &defaultValueNewCells, double additionalMarginMeters=2.0)
Changes the size of the grid, maintaining previous contents.
Definition: CDynamicGrid.h:127
void getAsMatrix(MAT &m) const
Get the entire grid as a matrix.
Definition: CDynamicGrid.h:309
const T * cellByPos(double x, double y) const
Definition: CDynamicGrid.h:220
T * cellByIndex(unsigned int cx, unsigned int cy)
Returns a pointer to the contents of a cell given by its cell indexes, or nullptr if it is out of the...
Definition: CDynamicGrid.h:232
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: CDynamicGrid.h:252
void dyngridcommon_readFromStream(STREAM &in, bool cast_from_float=false)
Definition: CDynamicGrid.h:351
GLsizei const GLchar ** string
Definition: glext.h:4101
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: CDynamicGrid.h:254
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
CDynamicGrid(double x_min=-10.0, double x_max=10.0, double y_min=-10.0f, double y_max=10.0f, double resolution=0.10f)
Constructor.
Definition: CDynamicGrid.h:55
GLuint in
Definition: glext.h:7274
virtual float getCellAsFloat(unsigned int cx, unsigned int cy) const =0
bool saveToTextFile(const std::string &fileName) const
Saves a float representation of the grid (via "cell2float()") to a text file.
Definition: CDynamicGrid.h:323
int x2idx(double x) const
Transform a coordinate values into cell indexes.
Definition: CDynamicGrid.h:266
double getXMax() const
Returns the "x" coordinate of right side of grid map.
Definition: CDynamicGrid.h:258
virtual ~CDynamicGrid()
Destructor.
Definition: CDynamicGrid.h:71
GLenum GLint GLint y
Definition: glext.h:3538
GLsizei const GLfloat * value
Definition: glext.h:4117
GLuint res
Definition: glext.h:7268
void idx2cxcy(const int &idx, int &cx, int &cy) const
Transform a global (linear) cell index value into its corresponding (x,y) cell indexes.
Definition: CDynamicGrid.h:281
GLenum GLint x
Definition: glext.h:3538
unsigned __int32 uint32_t
Definition: rptypes.h:47
virtual unsigned int getSizeX() const =0
const T * cellByIndex(unsigned int cx, unsigned int cy) const
Returns a pointer to the contents of a cell given by its cell indexes, or nullptr if it is out of the...
Definition: CDynamicGrid.h:243
virtual unsigned int getSizeY() const =0
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020