00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef CDynamicGrid_H
00029 #define CDynamicGrid_H
00030
00031 #include <mrpt/utils/utils_defs.h>
00032
00033 namespace mrpt
00034 {
00035 namespace utils
00036 {
00037 using namespace mrpt::system;
00038
00041 template <class T>
00042 class CDynamicGrid
00043 {
00044 protected:
00047 std::vector<T> m_map;
00048
00050 std::vector<T> & m_map_castaway_const() const { return const_cast< std::vector<T>& >( m_map ); }
00051
00052 float m_x_min,m_x_max,m_y_min,m_y_max;
00053 float m_resolution;
00054 unsigned int m_size_x, m_size_y;
00055
00056 public:
00059 CDynamicGrid( float x_min = -10.0f,
00060 float x_max = 10.0f,
00061 float y_min = -10.0f,
00062 float y_max = 10.0f,
00063 float resolution = 0.10f) :
00064 m_map(),
00065 m_x_min(),m_x_max(),m_y_min(),m_y_max(),
00066 m_resolution(),
00067 m_size_x(), m_size_y()
00068 {
00069 setSize(x_min,x_max,y_min,y_max,resolution);
00070 }
00071
00074 virtual ~CDynamicGrid()
00075 {
00076 }
00077
00081 void setSize( float x_min,
00082 float x_max,
00083 float y_min,
00084 float y_max,
00085 float resolution )
00086 {
00087
00088 m_x_min = resolution*round(x_min/resolution);
00089 m_y_min = resolution*round(y_min/resolution);
00090 m_x_max = resolution*round(x_max/resolution);
00091 m_y_max = resolution*round(y_max/resolution);
00092
00093
00094 m_resolution = resolution;
00095
00096
00097 m_size_x = round((m_x_max-m_x_min)/m_resolution);
00098 m_size_y = round((m_y_max-m_y_min)/m_resolution);
00099
00100
00101 m_map.resize(m_size_x*m_size_y);
00102 }
00103
00106 void clear()
00107 {
00108 m_map.clear();
00109 m_map.resize(m_size_x*m_size_y);
00110 }
00111
00114 void fill( const T& value )
00115 {
00116 for (typename std::vector<T>::iterator it = m_map.begin();it!=m_map.end();it++)
00117 *it = value;
00118 }
00119
00123 virtual void resize(
00124 float new_x_min,
00125 float new_x_max,
00126 float new_y_min,
00127 float new_y_max,
00128 const T& defaultValueNewCells,
00129 float additionalMarginMeters = 2.0f )
00130 {
00131 MRPT_TRY_START;
00132
00133 unsigned int x,y;
00134 unsigned int extra_x_izq,extra_y_arr,new_size_x=0,new_size_y=0;
00135 typename std::vector<T> new_map;
00136 typename std::vector<T>::iterator itSrc,itDst;
00137
00138
00139 if (new_x_min>=m_x_min &&
00140 new_y_min>=m_y_min &&
00141 new_x_max<=m_x_max &&
00142 new_y_max<=m_y_max) return;
00143
00144 if (new_x_min>m_x_min) new_x_min=m_x_min;
00145 if (new_x_max<m_x_max) new_x_max=m_x_max;
00146 if (new_y_min>m_y_min) new_y_min=m_y_min;
00147 if (new_y_max<m_y_max) new_y_max=m_y_max;
00148
00149
00150 if (additionalMarginMeters>0)
00151 {
00152 if (new_x_min<m_x_min) new_x_min= floor(new_x_min-additionalMarginMeters);
00153 if (new_x_max>m_x_max) new_x_max= ceil(new_x_max+additionalMarginMeters);
00154 if (new_y_min<m_y_min) new_y_min= floor(new_y_min-additionalMarginMeters);
00155 if (new_y_max>m_y_max) new_y_max= ceil(new_y_max+additionalMarginMeters);
00156 }
00157
00158
00159 if (fabs(new_x_min/m_resolution - round(new_x_min/m_resolution))>0.05f )
00160 new_x_min = m_resolution*round(new_x_min/m_resolution);
00161 if (fabs(new_y_min/m_resolution - round(new_y_min/m_resolution))>0.05f )
00162 new_y_min = m_resolution*round(new_y_min/m_resolution);
00163 if (fabs(new_x_max/m_resolution - round(new_x_max/m_resolution))>0.05f )
00164 new_x_max = m_resolution*round(new_x_max/m_resolution);
00165 if (fabs(new_y_max/m_resolution - round(new_y_max/m_resolution))>0.05f )
00166 new_y_max = m_resolution*round(new_y_max/m_resolution);
00167
00168
00169 extra_x_izq = round((m_x_min-new_x_min) / m_resolution);
00170 extra_y_arr = round((m_y_min-new_y_min) / m_resolution);
00171
00172 new_size_x = round((new_x_max-new_x_min) / m_resolution);
00173 new_size_y = round((new_y_max-new_y_min) / m_resolution);
00174
00175
00176 new_map.resize(new_size_x*new_size_y,defaultValueNewCells);
00177
00178
00179 for (y=0;y<m_size_y;y++)
00180 {
00181 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);
00182 x<m_size_x;
00183 x++,itSrc++,itDst++)
00184 {
00185 *itDst = *itSrc;
00186 }
00187 }
00188
00189
00190 m_x_min = new_x_min;
00191 m_x_max = new_x_max;
00192 m_y_min = new_y_min;
00193 m_y_max = new_y_max;
00194
00195 m_size_x = new_size_x;
00196 m_size_y = new_size_y;
00197
00198
00199 m_map.swap(new_map);
00200
00201 MRPT_TRY_END;
00202
00203 }
00204
00207 T* cellByPos( float x, float y )
00208 {
00209 int cx = x2idx(x);
00210 int cy = y2idx(y);
00211
00212 if( cx<0 || cx>=static_cast<int>(m_size_x) ) return NULL;
00213 if( cy<0 || cy>=static_cast<int>(m_size_y) ) return NULL;
00214
00215 return &m_map[ cx + cy*m_size_x ];
00216 }
00217
00220 const T* cellByPos( float x, float y ) const
00221 {
00222 int cx = x2idx(x);
00223 int cy = y2idx(y);
00224
00225 if( cx<0 || cx>=static_cast<int>(m_size_x) ) return NULL;
00226 if( cy<0 || cy>=static_cast<int>(m_size_y) ) return NULL;
00227
00228 return &m_map[ cx + cy*m_size_x ];
00229 }
00230
00233 T* cellByIndex( unsigned int cx, unsigned int cy )
00234 {
00235 if( cx>=m_size_x || cy>=m_size_y)
00236 return NULL;
00237 else return &m_map[ cx + cy*m_size_x ];
00238 }
00239
00242 const T* cellByIndex( unsigned int cx, unsigned int cy ) const
00243 {
00244 if( cx>=m_size_x || cy>=m_size_y)
00245 return NULL;
00246 else return &m_map[ cx + cy*m_size_x ];
00247 }
00248
00251 unsigned int getSizeX() { return m_size_x; }
00252
00255 unsigned int getSizeY() { return m_size_y; }
00256
00259 float getXMin()const { return m_x_min; }
00260
00263 float getXMax()const { return m_x_max; }
00264
00267 float getYMin()const { return m_y_min; }
00268
00271 float getYMax()const { return m_y_max; }
00272
00275 float getResolution()const { return m_resolution; }
00276
00278 virtual float cell2float(const T& c) const
00279 {
00280 return 0;
00281 }
00282
00283 void saveToTextFile(const std::string &fileName) const
00284 {
00285 FILE *f=os::fopen(fileName.c_str(),"wt");
00286 if(!f) return;
00287 for (unsigned int cy=0;cy<m_size_y;cy++)
00288 {
00289 for (unsigned int cx=0;cx<m_size_x;cx++)
00290 os::fprintf(f,"%f ",cell2float(m_map[ cx + cy*m_size_x ]));
00291 os::fprintf(f,"\n");
00292 }
00293 os::fclose(f);
00294 }
00295
00298 inline int x2idx(float x) const { return static_cast<int>( (x-m_x_min)/m_resolution ); }
00299 inline int y2idx(float y) const { return static_cast<int>( (y-m_y_min)/m_resolution ); }
00300 inline int xy2idx(float x,float y) const { return x2idx(x) + y2idx(y)*m_size_x; }
00301
00303 inline void idx2cxcy(const int &idx, int &cx, int &cy ) const
00304 {
00305 cx = idx % m_size_x;
00306 cy = idx / m_size_x;
00307 }
00308
00311 inline float idx2x(int cx) const { return m_x_min+(cx+0.5f)*m_resolution; }
00312 inline float idx2y(int cy) const { return m_y_min+(cy+0.5f)*m_resolution; }
00313
00316 inline int x2idx(float x,float x_min) const { return static_cast<int>((x-m_x_min)/m_resolution ); }
00317 inline int y2idx(float y, float y_min) const { return static_cast<int>((y-m_y_min)/m_resolution ); }
00318
00319 };
00320
00321 }
00322 }
00323 #endif