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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020