Main MRPT website > C++ reference
MRPT logo
CDynamicGrid.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CDynamicGrid_H
29 #define CDynamicGrid_H
30 
31 #include <mrpt/utils/utils_defs.h>
32 
33 namespace mrpt
34 {
35  namespace utils
36  {
37  using namespace mrpt::system;
38 
39  /** A 2D grid of dynamic size which stores any kind of data at each cell.
40  * \tparam T The type of each cell in the 2D grid.
41  * \ingroup mrpt_base_grp
42  */
43  template <class T>
44  class CDynamicGrid
45  {
46  protected:
47  /** The cells.
48  */
49  std::vector<T> m_map;
50 
51  /** Used only from logically const method that really need to modify the object */
52  inline std::vector<T> & m_map_castaway_const() const { return const_cast< std::vector<T>& >( m_map ); }
53 
54  float m_x_min,m_x_max,m_y_min,m_y_max;
55  float m_resolution;
56  size_t m_size_x, m_size_y;
57 
58  public:
59  /** Constructor
60  */
61  CDynamicGrid( float x_min = -10.0f,
62  float x_max = 10.0f,
63  float y_min = -10.0f,
64  float y_max = 10.0f,
65  float resolution = 0.10f) :
66  m_map(),
67  m_x_min(),m_x_max(),m_y_min(),m_y_max(),
68  m_resolution(),
69  m_size_x(), m_size_y()
70  {
71  setSize(x_min,x_max,y_min,y_max,resolution);
72  }
73 
74  /** Destructor
75  */
76  virtual ~CDynamicGrid()
77  {
78  }
79 
80  /** Changes the size of the grid, ERASING all previous contents.
81  * \sa resize
82  */
83  void setSize( float x_min,
84  float x_max,
85  float y_min,
86  float y_max,
87  float resolution )
88  {
89  // Adjust sizes to adapt them to full sized cells acording to the resolution:
90  m_x_min = resolution*round(x_min/resolution);
91  m_y_min = resolution*round(y_min/resolution);
92  m_x_max = resolution*round(x_max/resolution);
93  m_y_max = resolution*round(y_max/resolution);
94 
95  // Res:
96  m_resolution = resolution;
97 
98  // Now the number of cells should be integers:
99  m_size_x = round((m_x_max-m_x_min)/m_resolution);
100  m_size_y = round((m_y_max-m_y_min)/m_resolution);
101 
102  // Cells memory:
103  m_map.resize(m_size_x*m_size_y);
104  }
105 
106  /** Erase the contents of all the cells.
107  */
108  void clear()
109  {
110  m_map.clear();
111  m_map.resize(m_size_x*m_size_y);
112  }
113 
114  /** Fills all the cells with the same value
115  */
116  inline void fill( const T& value ) { std::fill(m_map.begin(),m_map.end(),value); }
117 
118  /** Changes the size of the grid, maintaining previous contents.
119  * \sa setSize
120  */
121  virtual void resize(
122  float new_x_min,
123  float new_x_max,
124  float new_y_min,
125  float new_y_max,
126  const T& defaultValueNewCells,
127  float additionalMarginMeters = 2.0f )
128  {
129  MRPT_START
130 
131  MRPT_CHECK_NORMAL_NUMBER(new_x_min)
132  MRPT_CHECK_NORMAL_NUMBER(new_x_max)
133  MRPT_CHECK_NORMAL_NUMBER(new_y_min)
134  MRPT_CHECK_NORMAL_NUMBER(new_y_max)
135 
136  unsigned int x,y;
137  unsigned int extra_x_izq,extra_y_arr,new_size_x=0,new_size_y=0;
138  typename std::vector<T> new_map;
139  typename std::vector<T>::iterator itSrc,itDst;
140 
141  // Is resize really necesary?
142  if (new_x_min>=m_x_min &&
143  new_y_min>=m_y_min &&
144  new_x_max<=m_x_max &&
145  new_y_max<=m_y_max) return;
146 
147  if (new_x_min>m_x_min) new_x_min=m_x_min;
148  if (new_x_max<m_x_max) new_x_max=m_x_max;
149  if (new_y_min>m_y_min) new_y_min=m_y_min;
150  if (new_y_max<m_y_max) new_y_max=m_y_max;
151 
152  // Additional margin:
153  if (additionalMarginMeters>0)
154  {
155  if (new_x_min<m_x_min) new_x_min= floor(new_x_min-additionalMarginMeters);
156  if (new_x_max>m_x_max) new_x_max= ceil(new_x_max+additionalMarginMeters);
157  if (new_y_min<m_y_min) new_y_min= floor(new_y_min-additionalMarginMeters);
158  if (new_y_max>m_y_max) new_y_max= ceil(new_y_max+additionalMarginMeters);
159  }
160 
161  // Adjust sizes to adapt them to full sized cells acording to the resolution:
162  if (fabs(new_x_min/m_resolution - round(new_x_min/m_resolution))>0.05f )
163  new_x_min = m_resolution*round(new_x_min/m_resolution);
164  if (fabs(new_y_min/m_resolution - round(new_y_min/m_resolution))>0.05f )
165  new_y_min = m_resolution*round(new_y_min/m_resolution);
166  if (fabs(new_x_max/m_resolution - round(new_x_max/m_resolution))>0.05f )
167  new_x_max = m_resolution*round(new_x_max/m_resolution);
168  if (fabs(new_y_max/m_resolution - round(new_y_max/m_resolution))>0.05f )
169  new_y_max = m_resolution*round(new_y_max/m_resolution);
170 
171  // Change the map size: Extensions at each side:
172  extra_x_izq = round((m_x_min-new_x_min) / m_resolution);
173  extra_y_arr = round((m_y_min-new_y_min) / m_resolution);
174 
175  new_size_x = round((new_x_max-new_x_min) / m_resolution);
176  new_size_y = round((new_y_max-new_y_min) / m_resolution);
177 
178  // Reserve new memory:
179  new_map.resize(new_size_x*new_size_y,defaultValueNewCells);
180 
181  // Copy previous rows:
182  for (y=0;y<m_size_y;y++)
183  {
184  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);
185  x<m_size_x;
186  x++,itSrc++,itDst++)
187  {
188  *itDst = *itSrc;
189  }
190  }
191 
192  // Update the new map limits:
193  m_x_min = new_x_min;
194  m_x_max = new_x_max;
195  m_y_min = new_y_min;
196  m_y_max = new_y_max;
197 
198  m_size_x = new_size_x;
199  m_size_y = new_size_y;
200 
201  // Keep the new map only:
202  m_map.swap(new_map);
203 
204  MRPT_END
205 
206  }
207 
208  /** Returns a pointer to the contents of a cell given by its coordinates, or NULL if it is out of the map extensions.
209  */
210  inline T* cellByPos( float x, float y )
211  {
212  int cx = x2idx(x);
213  int cy = y2idx(y);
214 
215  if( cx<0 || cx>=static_cast<int>(m_size_x) ) return NULL;
216  if( cy<0 || cy>=static_cast<int>(m_size_y) ) return NULL;
217 
218  return &m_map[ cx + cy*m_size_x ];
219  }
220 
221  /** Returns a pointer to the contents of a cell given by its coordinates, or NULL if it is out of the map extensions.
222  */
223  inline const T* cellByPos( float x, float y ) const
224  {
225  int cx = x2idx(x);
226  int cy = y2idx(y);
227 
228  if( cx<0 || cx>=static_cast<int>(m_size_x) ) return NULL;
229  if( cy<0 || cy>=static_cast<int>(m_size_y) ) return NULL;
230 
231  return &m_map[ cx + cy*m_size_x ];
232  }
233 
234  /** Returns a pointer to the contents of a cell given by its cell indexes, or NULL if it is out of the map extensions.
235  */
236  inline T* cellByIndex( unsigned int cx, unsigned int cy )
237  {
238  if( cx>=m_size_x || cy>=m_size_y)
239  return NULL;
240  else return &m_map[ cx + cy*m_size_x ];
241  }
242 
243  /** Returns a pointer to the contents of a cell given by its cell indexes, or NULL if it is out of the map extensions.
244  */
245  inline const T* cellByIndex( unsigned int cx, unsigned int cy ) const
246  {
247  if( cx>=m_size_x || cy>=m_size_y)
248  return NULL;
249  else return &m_map[ cx + cy*m_size_x ];
250  }
251 
252  /** Returns the horizontal size of grid map in cells count.
253  */
254  inline size_t getSizeX() const { return m_size_x; }
255 
256  /** Returns the vertical size of grid map in cells count.
257  */
258  inline size_t getSizeY() const { return m_size_y; }
259 
260  /** Returns the "x" coordinate of left side of grid map.
261  */
262  inline float getXMin()const { return m_x_min; }
263 
264  /** Returns the "x" coordinate of right side of grid map.
265  */
266  inline float getXMax()const { return m_x_max; }
267 
268  /** Returns the "y" coordinate of top side of grid map.
269  */
270  inline float getYMin()const { return m_y_min; }
271 
272  /** Returns the "y" coordinate of bottom side of grid map.
273  */
274  inline float getYMax()const { return m_y_max; }
275 
276  /** Returns the resolution of the grid map.
277  */
278  inline float getResolution()const { return m_resolution; }
279 
280  /** The user must implement this in order to provide "saveToTextFile" a way to convert each cell into a numeric value */
281  virtual float cell2float(const T& c) const
282  {
283  return 0;
284  }
285 
286  void saveToTextFile(const std::string &fileName) const
287  {
288  FILE *f=os::fopen(fileName.c_str(),"wt");
289  if(!f) return;
290  for (unsigned int cy=0;cy<m_size_y;cy++)
291  {
292  for (unsigned int cx=0;cx<m_size_x;cx++)
293  os::fprintf(f,"%f ",cell2float(m_map[ cx + cy*m_size_x ]));
294  os::fprintf(f,"\n");
295  }
296  os::fclose(f);
297  }
298 
299  /** Transform a coordinate values into cell indexes.
300  */
301  inline int x2idx(float x) const { return static_cast<int>( (x-m_x_min)/m_resolution ); }
302  inline int y2idx(float y) const { return static_cast<int>( (y-m_y_min)/m_resolution ); }
303  inline int xy2idx(float x,float y) const { return x2idx(x) + y2idx(y)*m_size_x; }
304 
305  /** Transform a global (linear) cell index value into its corresponding (x,y) cell indexes. */
306  inline void idx2cxcy(const int &idx, int &cx, int &cy ) const
307  {
308  cx = idx % m_size_x;
309  cy = idx / m_size_x;
310  }
311 
312  /** Transform a cell index into a coordinate value.
313  */
314  inline float idx2x(int cx) const { return m_x_min+(cx+0.5f)*m_resolution; }
315  inline float idx2y(int cy) const { return m_y_min+(cy+0.5f)*m_resolution; }
316 
317  /** Transform a coordinate value into a cell index, using a diferent "x_min" value
318  */
319  inline int x2idx(float x,float x_min) const { return static_cast<int>((x-m_x_min)/m_resolution ); }
320  inline int y2idx(float y, float y_min) const { return static_cast<int>((y-m_y_min)/m_resolution ); }
321 
322  /** Get the entire grid as a matrix.
323  * \tparam MAT The type of the matrix, typically a CMatrixDouble.
324  * \param[out] m The output matrix; will be set automatically to the correct size.
325  * Entry (cy,cx) in the matrix contains the grid cell with indices (cx,cy).
326  * \note This method will compile only for cell types that can be converted to the type of the matrix elements (e.g. double).
327  */
328  template <class MAT>
329  void getAsMatrix(MAT &m) const
330  {
331  m.setSize(m_size_y, m_size_x);
332  if (m_map.empty()) return;
333  const T* c = &m_map[0];
334  for (size_t cy=0;cy<m_size_y;cy++)
335  for (size_t cx=0;cx<m_size_x;cx++)
336  m.set_unsafe(cy,cx, *c++);
337  }
338 
339  };
340 
341  } // End of namespace
342 } // end of namespace
343 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013