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



Page generated by Doxygen 1.8.11 for MRPT 1.3.2 SVN:Unversioned directory at Sun May 1 08:45:24 UTC 2016