10 #define CDynamicGrid_H 17 #define _USE_MATH_DEFINES // (For VS to define M_PI, etc. in cmath) 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;
47 double m_x_min,m_x_max,
m_y_min,m_y_max,m_resolution;
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) :
53 m_map(), m_x_min(),m_x_max(),m_y_min(),m_y_max(),
54 m_resolution(),m_size_x(), m_size_y()
56 setSize(x_min,x_max,y_min,y_max,resolution);
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)
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);
81 m_resolution = resolution;
84 m_size_x =
round((m_x_max-m_x_min)/m_resolution);
85 m_size_y =
round((m_y_max-m_y_min)/m_resolution);
89 m_map.assign(m_size_x*m_size_y, *fill_value);
90 else m_map.resize(m_size_x*m_size_y);
96 m_map.resize(m_size_x*m_size_y);
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 )
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;
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;
126 if (additionalMarginMeters>0)
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);
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);
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);
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);
152 typename std::vector<T> new_map;
153 new_map.resize(new_size_x*new_size_y,defaultValueNewCells);
158 for (y=0;y<m_size_y;y++)
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);
174 m_size_x = new_size_x;
175 m_size_y = new_size_y;
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 ];
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 ];
205 if( cx>=m_size_x || cy>=m_size_y)
207 else return &m_map[ cx + cy*m_size_x ];
212 inline const T*
cellByIndex(
unsigned int cx,
unsigned int cy )
const 214 if( cx>=m_size_x || cy>=m_size_y)
216 else return &m_map[ cx + cy*m_size_x ];
220 inline size_t getSizeX()
const {
return m_size_x; }
223 inline size_t getSizeY()
const {
return m_size_y; }
226 inline double getXMin()
const {
return m_x_min; }
229 inline double getXMax()
const {
return m_x_max; }
232 inline double getYMin()
const {
return m_y_min; }
235 inline double getYMax()
const {
return m_y_max; }
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; }
246 inline void idx2cxcy(
const int &idx,
int &cx,
int &cy )
const 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; }
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++);
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() ]); }
290 aux_saver aux(*
this);
291 return aux.saveToTextFile(fileName);
296 out << m_x_min << m_x_max << m_y_min << m_y_max;
298 out << static_cast<uint32_t>(m_size_x) << static_cast<uint32_t>(m_size_y);
301 if (!cast_from_float) {
302 in >> m_x_min >> m_x_max >> m_y_min >> m_y_max;
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;
311 m_size_x = nX; m_size_y = nY;
std::vector< T > & m_map_castaway_const() const
Used only from logically const method that really need to modify the object.
double getXMax() const
Returns the "x" coordinate of right side of grid map.
void clear()
Erase the contents of all the cells.
int xy2idx(double x, double y) const
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...
int x2idx(double x) const
Transform a coordinate values into cell indexes.
double getYMin() const
Returns the "y" coordinate of top side of grid map.
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.
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.
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...
T value(details::expression_node< T > *n)
void fill(const T &value)
Fills all the cells with the same value.
void idx2cxcy(const int &idx, int &cx, int &cy) const
Transform a global (linear) cell index value into its corresponding (x,y) cell indexes.
int y2idx(double y) const
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 idx2y(int cy) const
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
A 2D grid of dynamic size which stores any kind of data at each cell.
double getYMax() const
Returns the "y" coordinate of bottom side of grid map.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
std::vector< T > m_map
The cells.
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
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...
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.
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...
double idx2x(int cx) const
Transform a cell index into a coordinate value of the cell central point.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double getResolution() const
Returns the resolution of the grid map.
const T * cellByPos(double x, double y) const
virtual ~CDynamicGrid()
Destructor.
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...
bool saveToTextFile(const std::string &fileName) const
Saves a float representation of the grid (via "cell2float()") to a text file.
x y t t *t x y t t t x y t t t x *y t *t t x *y t *t t x y t t t x y t t t x(y+z)
int round(const T value)
Returns the closer integer (int) to x.
void dyngridcommon_writeToStream(mrpt::utils::CStream &out) const
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.
void dyngridcommon_readFromStream(mrpt::utils::CStream &in, bool cast_from_float=false)
void getAsMatrix(MAT &m) const
Get the entire grid as a matrix.