Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 #ifndef CDynamicGrid_H
10 #define CDynamicGrid_H
11 
12 #include <mrpt/utils/core_defs.h>
13 #include <mrpt/utils/round.h>
14 #include <mrpt/utils/CStream.h>
15 #include <vector>
16 #include <string>
17 #define _USE_MATH_DEFINES // (For VS to define M_PI, etc. in cmath)
18 #include <cmath>
19 
20 namespace mrpt
21 {
22  namespace utils
23  {
24  namespace internal {
25  // Aux class.
27  {
28  bool saveToTextFile(const std::string &fileName) const;
29  virtual unsigned int getSizeX() const = 0;
30  virtual unsigned int getSizeY() const = 0;
31  virtual float getCellAsFloat(unsigned int cx,unsigned int cy) const = 0;
32  };
33  } // internal
34 
35  /** A 2D grid of dynamic size which stores any kind of data at each cell.
36  * \tparam T The type of each cell in the 2D grid.
37  * \ingroup mrpt_base_grp
38  */
39  template <class T>
41  {
42  protected:
43  std::vector<T> m_map; //!< The cells
44  /** Used only from logically const method that really need to modify the object */
45  inline std::vector<T> & m_map_castaway_const() const { return const_cast< std::vector<T>& >( m_map ); }
46 
48  size_t m_size_x, m_size_y;
49 
50  public:
51  /** Constructor */
52  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) :
53  m_map(), m_x_min(),m_x_max(),m_y_min(),m_y_max(),
55  {
56  setSize(x_min,x_max,y_min,y_max,resolution);
57  }
58 
59  /** Destructor */
60  virtual ~CDynamicGrid() { }
61 
62  /** Changes the size of the grid, ERASING all previous contents.
63  * If \a fill_value is left as NULL, the contents of cells may be undefined (some will remain with
64  * their old values, the new ones will have the default cell value, but the location of old values
65  * may change wrt their old places).
66  * If \a fill_value is not NULL, it is assured that all cells will have a copy of that value after resizing.
67  * \sa resize, fill
68  */
69  void setSize(
70  const double x_min, const double x_max,
71  const double y_min, const double y_max,
72  const double resolution, const T * fill_value = NULL)
73  {
74  // Adjust sizes to adapt them to full sized cells acording to the resolution:
75  m_x_min = resolution*round(x_min/resolution);
76  m_y_min = resolution*round(y_min/resolution);
77  m_x_max = resolution*round(x_max/resolution);
78  m_y_max = resolution*round(y_max/resolution);
79 
80  // Res:
81  m_resolution = resolution;
82 
83  // Now the number of cells should be integers:
86 
87  // Cells memory:
88  if (fill_value)
89  m_map.assign(m_size_x*m_size_y, *fill_value);
90  else m_map.resize(m_size_x*m_size_y);
91  }
92 
93  /** Erase the contents of all the cells. */
94  void clear() {
95  m_map.clear();
96  m_map.resize(m_size_x*m_size_y);
97  }
98 
99  /** Fills all the cells with the same value
100  */
101  inline void fill( const T& value ) {
102  for (typename std::vector<T>::iterator it=m_map.begin();it!=m_map.end();++it)
103  *it=value;
104  }
105 
106  /** Changes the size of the grid, maintaining previous contents.
107  * \sa setSize
108  */
109  virtual void resize(
110  double new_x_min, double new_x_max,
111  double new_y_min, double new_y_max,
112  const T& defaultValueNewCells, double additionalMarginMeters = 2.0 )
113  {
114  // Is resize really necesary?
115  if (new_x_min>=m_x_min &&
116  new_y_min>=m_y_min &&
117  new_x_max<=m_x_max &&
118  new_y_max<=m_y_max) return;
119 
120  if (new_x_min>m_x_min) new_x_min=m_x_min;
121  if (new_x_max<m_x_max) new_x_max=m_x_max;
122  if (new_y_min>m_y_min) new_y_min=m_y_min;
123  if (new_y_max<m_y_max) new_y_max=m_y_max;
124 
125  // Additional margin:
126  if (additionalMarginMeters>0)
127  {
128  if (new_x_min<m_x_min) new_x_min= floor(new_x_min-additionalMarginMeters);
129  if (new_x_max>m_x_max) new_x_max= ceil(new_x_max+additionalMarginMeters);
130  if (new_y_min<m_y_min) new_y_min= floor(new_y_min-additionalMarginMeters);
131  if (new_y_max>m_y_max) new_y_max= ceil(new_y_max+additionalMarginMeters);
132  }
133 
134  // Adjust sizes to adapt them to full sized cells acording to the resolution:
135  if (fabs(new_x_min/m_resolution - round(new_x_min/m_resolution))>0.05f )
136  new_x_min = m_resolution*round(new_x_min/m_resolution);
137  if (fabs(new_y_min/m_resolution - round(new_y_min/m_resolution))>0.05f )
138  new_y_min = m_resolution*round(new_y_min/m_resolution);
139  if (fabs(new_x_max/m_resolution - round(new_x_max/m_resolution))>0.05f )
140  new_x_max = m_resolution*round(new_x_max/m_resolution);
141  if (fabs(new_y_max/m_resolution - round(new_y_max/m_resolution))>0.05f )
142  new_y_max = m_resolution*round(new_y_max/m_resolution);
143 
144  // Change the map size: Extensions at each side:
145  unsigned int extra_x_izq = round((m_x_min-new_x_min) / m_resolution);
146  unsigned int extra_y_arr = round((m_y_min-new_y_min) / m_resolution);
147 
148  unsigned int new_size_x = round((new_x_max-new_x_min) / m_resolution);
149  unsigned int new_size_y = round((new_y_max-new_y_min) / m_resolution);
150 
151  // Reserve new memory:
152  typename std::vector<T> new_map;
153  new_map.resize(new_size_x*new_size_y,defaultValueNewCells);
154 
155  // Copy previous rows:
156  unsigned int x,y;
157  typename std::vector<T>::iterator itSrc,itDst;
158  for (y=0;y<m_size_y;y++)
159  {
160  for (x=0,itSrc=(m_map.begin()+y*m_size_x),itDst=(new_map.begin()+extra_x_izq + (y+extra_y_arr)*new_size_x);
161  x<m_size_x;
162  ++x,++itSrc,++itDst)
163  {
164  *itDst = *itSrc;
165  }
166  }
167 
168  // Update the new map limits:
169  m_x_min = new_x_min;
170  m_x_max = new_x_max;
171  m_y_min = new_y_min;
172  m_y_max = new_y_max;
173 
174  m_size_x = new_size_x;
175  m_size_y = new_size_y;
176 
177  // Keep the new map only:
178  m_map.swap(new_map);
179  }
180 
181  /** Returns a pointer to the contents of a cell given by its coordinates, or NULL if it is out of the map extensions.
182  */
183  inline T* cellByPos( double x, double y )
184  {
185  const int cx = x2idx(x);
186  const int cy = y2idx(y);
187  if( cx<0 || cx>=static_cast<int>(m_size_x) ) return NULL;
188  if( cy<0 || cy>=static_cast<int>(m_size_y) ) return NULL;
189  return &m_map[ cx + cy*m_size_x ];
190  }
191  /** \overload */
192  inline const T* cellByPos( double x, double y ) const
193  {
194  const int cx = x2idx(x);
195  const int cy = y2idx(y);
196  if( cx<0 || cx>=static_cast<int>(m_size_x) ) return NULL;
197  if( cy<0 || cy>=static_cast<int>(m_size_y) ) return NULL;
198  return &m_map[ cx + cy*m_size_x ];
199  }
200 
201  /** Returns a pointer to the contents of a cell given by its cell indexes, or NULL if it is out of the map extensions.
202  */
203  inline T* cellByIndex( unsigned int cx, unsigned int cy )
204  {
205  if( cx>=m_size_x || cy>=m_size_y)
206  return NULL;
207  else return &m_map[ cx + cy*m_size_x ];
208  }
209 
210  /** Returns a pointer to the contents of a cell given by its cell indexes, or NULL if it is out of the map extensions.
211  */
212  inline const T* cellByIndex( unsigned int cx, unsigned int cy ) const
213  {
214  if( cx>=m_size_x || cy>=m_size_y)
215  return NULL;
216  else return &m_map[ cx + cy*m_size_x ];
217  }
218 
219  /** Returns the horizontal size of grid map in cells count */
220  inline size_t getSizeX() const { return m_size_x; }
221 
222  /** Returns the vertical size of grid map in cells count */
223  inline size_t getSizeY() const { return m_size_y; }
224 
225  /** Returns the "x" coordinate of left side of grid map */
226  inline double getXMin()const { return m_x_min; }
227 
228  /** Returns the "x" coordinate of right side of grid map */
229  inline double getXMax()const { return m_x_max; }
230 
231  /** Returns the "y" coordinate of top side of grid map */
232  inline double getYMin()const { return m_y_min; }
233 
234  /** Returns the "y" coordinate of bottom side of grid map */
235  inline double getYMax()const { return m_y_max; }
236 
237  /** Returns the resolution of the grid map */
238  inline double getResolution()const { return m_resolution; }
239 
240  /** Transform a coordinate values into cell indexes */
241  inline int x2idx(double x) const { return static_cast<int>( (x-m_x_min)/m_resolution ); }
242  inline int y2idx(double y) const { return static_cast<int>( (y-m_y_min)/m_resolution ); }
243  inline int xy2idx(double x,double y) const { return x2idx(x) + y2idx(y)*m_size_x; }
244 
245  /** Transform a global (linear) cell index value into its corresponding (x,y) cell indexes. */
246  inline void idx2cxcy(const int &idx, int &cx, int &cy ) const
247  {
248  cx = idx % m_size_x;
249  cy = idx / m_size_x;
250  }
251 
252  /** Transform a cell index into a coordinate value of the cell central point */
253  inline double idx2x(int cx) const { return m_x_min+(cx+0.5)*m_resolution; }
254  inline double idx2y(int cy) const { return m_y_min+(cy+0.5)*m_resolution; }
255 
256  /** Get the entire grid as a matrix.
257  * \tparam MAT The type of the matrix, typically a mrpt::math::CMatrixDouble.
258  * \param[out] m The output matrix; will be set automatically to the correct size.
259  * Entry (cy,cx) in the matrix contains the grid cell with indices (cx,cy).
260  * \note This method will compile only for cell types that can be converted to the type of the matrix elements (e.g. double).
261  */
262  template <class MAT>
263  void getAsMatrix(MAT &m) const
264  {
265  m.setSize(m_size_y, m_size_x);
266  if (m_map.empty()) return;
267  const T* c = &m_map[0];
268  for (size_t cy=0;cy<m_size_y;cy++)
269  for (size_t cx=0;cx<m_size_x;cx++)
270  m.set_unsafe(cy,cx, *c++);
271  }
272 
273  /** The user must implement this in order to provide "saveToTextFile" a way to convert each cell into a numeric value */
274  virtual float cell2float(const T& c) const
275  {
277  return 0;
278  }
279  /** Saves a float representation of the grid (via "cell2float()") to a text file. \return false on error */
280  bool saveToTextFile(const std::string &fileName) const
281  {
282  struct aux_saver : public internal::dynamic_grid_txt_saver
283  {
284  aux_saver(const CDynamicGrid<T> &obj) : m_obj(obj) {}
285  virtual unsigned int getSizeX() const { return m_obj.getSizeX(); }
286  virtual unsigned int getSizeY() const { return m_obj.getSizeY(); }
287  virtual float getCellAsFloat(unsigned int cx,unsigned int cy) const { return m_obj.cell2float(m_obj.m_map[ cx + cy*m_obj.getSizeX() ]); }
288  const CDynamicGrid<T> & m_obj;
289  };
290  aux_saver aux(*this);
291  return aux.saveToTextFile(fileName);
292  }
293 
294  protected:
296  out << m_x_min << m_x_max << m_y_min << m_y_max;
297  out << m_resolution;
298  out << static_cast<uint32_t>(m_size_x) << static_cast<uint32_t>(m_size_y);
299  }
300  void dyngridcommon_readFromStream(mrpt::utils::CStream &in, bool cast_from_float = false) {
301  if (!cast_from_float) {
302  in >> m_x_min >> m_x_max >> m_y_min >> m_y_max;
303  in >> m_resolution;
304  } else {
305  float xmin,xmax,ymin,ymax,res;
306  in >> xmin >> xmax >> ymin >> ymax >> res;
307  m_x_min = xmin; m_x_max = xmax; m_y_min = ymin; m_y_max = ymax; m_resolution = res;
308  }
309  uint32_t nX,nY;
310  in >> nX >> nY;
311  m_size_x = nX; m_size_y = nY;
312  m_map.resize(nX*nY);
313  }
314 
315 
316  }; // end of CDynamicGrid<>
317 
318  } // End of namespace
319 } // end of namespace
320 #endif
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 getXMax() const
Returns the "x" coordinate of right side of grid map.
Definition: CDynamicGrid.h:229
void clear()
Erase the contents of all the cells.
Definition: CDynamicGrid.h:94
int xy2idx(double x, double y) const
Definition: CDynamicGrid.h:243
virtual float cell2float(const T &c) const
The user must implement this in order to provide "saveToTextFile" a way to convert each cell into a n...
Definition: CDynamicGrid.h:274
int x2idx(double x) const
Transform a coordinate values into cell indexes.
Definition: CDynamicGrid.h:241
Scalar * iterator
Definition: eigen_plugins.h:23
double getYMin() const
Returns the "y" coordinate of top side of grid map.
Definition: CDynamicGrid.h:232
double getXMin() const
Returns the "x" coordinate of left side of grid map.
Definition: CDynamicGrid.h:226
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: CDynamicGrid.h:223
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:109
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classe...
void fill(const T &value)
Fills all the cells with the same value.
Definition: CDynamicGrid.h:101
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
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:246
int y2idx(double y) const
Definition: CDynamicGrid.h:242
T * cellByPos(double x, double y)
Returns a pointer to the contents of a cell given by its coordinates, or NULL if it is out of the map...
Definition: CDynamicGrid.h:183
double idx2y(int cy) const
Definition: CDynamicGrid.h:254
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:40
double getYMax() const
Returns the "y" coordinate of bottom side of grid map.
Definition: CDynamicGrid.h:235
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
const GLubyte * c
Definition: glext.h:5590
std::vector< T > m_map
The cells.
Definition: CDynamicGrid.h:43
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: CDynamicGrid.h:220
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:52
GLsizei const GLchar ** string
Definition: glext.h:3919
T * cellByIndex(unsigned int cx, unsigned int cy)
Returns a pointer to the contents of a cell given by its cell indexes, or NULL if it is out of the ma...
Definition: CDynamicGrid.h:203
double idx2x(int cx) const
Transform a cell index into a coordinate value of the cell central point.
Definition: CDynamicGrid.h:253
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double getResolution() const
Returns the resolution of the grid map.
Definition: CDynamicGrid.h:238
const T * cellByPos(double x, double y) const
Definition: CDynamicGrid.h:192
virtual ~CDynamicGrid()
Destructor.
Definition: CDynamicGrid.h:60
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 NULL if it is out of the ma...
Definition: CDynamicGrid.h:212
bool saveToTextFile(const std::string &fileName) const
Saves a float representation of the grid (via "cell2float()") to a text file.
Definition: CDynamicGrid.h:280
GLuint in
Definition: glext.h:6301
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
void dyngridcommon_writeToStream(mrpt::utils::CStream &out) const
Definition: CDynamicGrid.h:295
GLenum GLint GLint y
Definition: glext.h:3516
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=NULL)
Changes the size of the grid, ERASING all previous contents.
Definition: CDynamicGrid.h:69
GLsizei const GLfloat * value
Definition: glext.h:3929
GLuint res
Definition: glext.h:6298
GLenum GLint x
Definition: glext.h:3516
unsigned __int32 uint32_t
Definition: rptypes.h:49
void dyngridcommon_readFromStream(mrpt::utils::CStream &in, bool cast_from_float=false)
Definition: CDynamicGrid.h:300
void getAsMatrix(MAT &m) const
Get the entire grid as a matrix.
Definition: CDynamicGrid.h:263



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019