10 #define CDynamicGrid_H
16 #define _USE_MATH_DEFINES // (For VS to define M_PI, etc. in cmath)
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;
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()
62 setSize(x_min,x_max,y_min,y_max,resolution);
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)
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);
87 m_resolution = resolution;
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);
95 m_map.assign(m_size_x*m_size_y, *fill_value);
96 else m_map.resize(m_size_x*m_size_y);
102 m_map.resize(m_size_x*m_size_y);
107 inline void fill(
const T& value ) {
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 )
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;
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;
132 if (additionalMarginMeters>0)
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);
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);
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);
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);
158 typename std::vector<T> new_map;
159 new_map.resize(new_size_x*new_size_y,defaultValueNewCells);
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);
180 m_size_x = new_size_x;
181 m_size_y = new_size_y;
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;
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;
217 if( cx>=m_size_x || cy>=m_size_y)
219 else return &m_map[ cx + cy*
m_size_x ];
224 inline const T*
cellByIndex(
unsigned int cx,
unsigned int cy )
const
226 if( cx>=m_size_x || cy>=m_size_y)
228 else return &m_map[ cx + cy*
m_size_x ];
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 ); }
266 inline void idx2cxcy(
const int &idx,
int &cx,
int &cy )
const
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; }
279 inline int x2idx(
float x,
float x_min)
const
282 return static_cast<int>((x-
m_x_min)/m_resolution );
284 inline int y2idx(
float y,
float y_min)
const
287 return static_cast<int>((y-
m_y_min)/m_resolution );
299 m.setSize(m_size_y, m_size_x);
300 if (m_map.empty())
return;
301 const T* c = &m_map[0];
304 m.set_unsafe(cy,cx, *c++);
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() ]); }
324 aux_saver aux(*
this);
325 return aux.saveToTextFile(fileName);
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
void clear()
Erase the contents of all the cells.
float getResolution() const
Returns the resolution of the grid map.
void getAsMatrix(MAT &m) const
Get the entire grid as a matrix.
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...
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...
std::vector< T > & m_map_castaway_const() const
Used only from logically const method that really need to modify the object.
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.
bool saveToTextFile(const std::string &fileName) const
Saves a float representation of the grid (via "cell2float()") to a text file.
void fill(const T &value)
Fills all the cells with the same value.
float idx2y(int cy) const
float getXMin() const
Returns the "x" coordinate of left side of grid map.
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
int x2idx(float x) const
Transform a coordinate values into cell indexes.
float getXMax() const
Returns the "x" coordinate of right side of grid map.
int x2idx(float x, float x_min) const
Transform a coordinate value into a cell index, using a diferent "x_min" value.
A 2D grid of dynamic size which stores any kind of data at each cell.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
std::vector< T > m_map
The cells.
int y2idx(float y, float y_min) const
float idx2x(int cx) const
Transform a cell index into a coordinate value.
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...
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.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
float getYMax() const
Returns the "y" coordinate of bottom side of grid map.
virtual ~CDynamicGrid()
Destructor.
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...
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...
int round(const T value)
Returns the closer integer (int) to x.
float getYMin() const
Returns the "y" coordinate of top side of grid map.
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.
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 xy2idx(float x, float y) const
void idx2cxcy(const int &idx, int &cx, int &cy) const
Transform a global (linear) cell index value into its corresponding (x,y) cell indexes.