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
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
void clear()
Erase the contents of all the cells.
Definition: CDynamicGrid.h:100
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
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
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: CDynamicGrid.h:237
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:107
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
int xy2idx(float x, float y) const
Definition: CDynamicGrid.h:263
int y2idx(float y, float y_min) const
Definition: CDynamicGrid.h:284
float getYMin() const
Returns the "y" coordinate of top side of grid map.
Definition: CDynamicGrid.h:249
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
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: CDynamicGrid.h:233
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...
float getXMin() const
Returns the "x" coordinate of left side of grid map.
Definition: CDynamicGrid.h:241
float idx2y(int cy) const
Definition: CDynamicGrid.h:275
int x2idx(float x) const
Transform a coordinate values into cell indexes.
Definition: CDynamicGrid.h:261
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
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
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
float getYMax() const
Returns the "y" coordinate of bottom side of grid map.
Definition: CDynamicGrid.h:253
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
float getResolution() const
Returns the resolution of the grid map.
Definition: CDynamicGrid.h:257
virtual ~CDynamicGrid()
Destructor.
Definition: CDynamicGrid.h:66
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
float getXMax() const
Returns the "x" coordinate of right side of grid map.
Definition: CDynamicGrid.h:245
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
bool saveToTextFile(const std::string &fileName) const
Saves a float representation of the grid (via "cell2float()") to a text file.
Definition: CDynamicGrid.h:314
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
float idx2x(int cx) const
Transform a cell index into a coordinate value.
Definition: CDynamicGrid.h:274
int y2idx(float y) const
Definition: CDynamicGrid.h:262
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
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 getAsMatrix(MAT &m) const
Get the entire grid as a matrix.
Definition: CDynamicGrid.h:297



Page generated by Doxygen 1.8.12 for MRPT 1.3.2 SVN: at Thu Nov 10 13:22:34 UTC 2016