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.
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
double idx2y(int cy) const
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...
std::vector< T > m_map
The cells.
void idx2cxcy(const int &idx, int &cx, int &cy) const
Transform a global (linear) cell index value into its corresponding (x,y) cell indexes.
void clear()
Erase the contents of all the cells.
virtual ~CDynamicGrid()
Destructor.
double getResolution() const
Returns the resolution of the grid map.
void dyngridcommon_writeToStream(mrpt::utils::CStream &out) const
bool saveToTextFile(const std::string &fileName) const
Saves a float representation of the grid (via "cell2float()") to a text file.
int x2idx(double x) const
Transform a coordinate values into cell indexes.
void dyngridcommon_readFromStream(mrpt::utils::CStream &in, bool cast_from_float=false)
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.
const T * cellByPos(double x, double y) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
std::vector< T > & m_map_castaway_const() const
Used only from logically const method that really need to modify the object.
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.
int xy2idx(double x, double y) const
double getYMin() const
Returns the "y" coordinate of top side of grid map.
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...
void fill(const T &value)
Fills all the cells with the same value.
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.
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...
double getXMin() const
Returns the "x" coordinate of left side of grid map.
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
void getAsMatrix(MAT &m) const
Get the entire grid as a matrix.
double idx2x(int cx) const
Transform a cell index into a coordinate value of the cell central point.
double getYMax() const
Returns the "y" coordinate of bottom side of grid map.
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...
double getXMax() const
Returns the "x" coordinate of right side of grid map.
int y2idx(double y) const
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition CStream.h:39
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.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
unsigned long uint32_t
Definition pstdint.h:216
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.8 for MRPT 1.4.0 SVN: at Thu Dec 14 16:54:58 UTC 2023