Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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
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
20namespace 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
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) :
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
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:41
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: CDynamicGrid.h:220
double idx2y(int cy) const
Definition: CDynamicGrid.h:254
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
std::vector< T > m_map
The cells.
Definition: CDynamicGrid.h:43
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
void clear()
Erase the contents of all the cells.
Definition: CDynamicGrid.h:94
virtual ~CDynamicGrid()
Destructor.
Definition: CDynamicGrid.h:60
double getResolution() const
Returns the resolution of the grid map.
Definition: CDynamicGrid.h:238
void dyngridcommon_writeToStream(mrpt::utils::CStream &out) const
Definition: CDynamicGrid.h:295
bool saveToTextFile(const std::string &fileName) const
Saves a float representation of the grid (via "cell2float()") to a text file.
Definition: CDynamicGrid.h:280
int x2idx(double x) const
Transform a coordinate values into cell indexes.
Definition: CDynamicGrid.h:241
void dyngridcommon_readFromStream(mrpt::utils::CStream &in, bool cast_from_float=false)
Definition: CDynamicGrid.h:300
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
const T * cellByPos(double x, double y) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
Definition: CDynamicGrid.h:192
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
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
int xy2idx(double x, double y) const
Definition: CDynamicGrid.h:243
double getYMin() const
Returns the "y" coordinate of top side of grid map.
Definition: CDynamicGrid.h:232
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
void fill(const T &value)
Fills all the cells with the same value.
Definition: CDynamicGrid.h:101
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
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
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
void getAsMatrix(MAT &m) const
Get the entire grid as a matrix.
Definition: CDynamicGrid.h:263
double idx2x(int cx) const
Transform a cell index into a coordinate value of the cell central point.
Definition: CDynamicGrid.h:253
double getYMax() const
Returns the "y" coordinate of bottom side of grid map.
Definition: CDynamicGrid.h:235
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 getXMax() const
Returns the "x" coordinate of right side of grid map.
Definition: CDynamicGrid.h:229
int y2idx(double y) const
Definition: CDynamicGrid.h:242
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:39
Scalar * iterator
Definition: eigen_plugins.h:23
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: mrpt_macros.h:290
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool saveToTextFile(const std::string &fileName) const
virtual unsigned int getSizeY() const =0
virtual unsigned int getSizeX() const =0
virtual float getCellAsFloat(unsigned int cx, unsigned int cy) const =0



Page generated by Doxygen 1.9.4 for MRPT 1.4.0 SVN: at Sun Aug 14 11:28:23 UTC 2022