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
00039
00040
00041
00042 template <class T>
00043 class CDynamicGrid
00044 {
00045 protected:
00046
00047
00048 std::vector<T> m_map;
00049
00050
00051 inline std::vector<T> & m_map_castaway_const() const { return const_cast< std::vector<T>& >( m_map ); }
00052
00053 float m_x_min,m_x_max,m_y_min,m_y_max;
00054 float m_resolution;
00055 size_t m_size_x, m_size_y;
00056
00057 public:
00058
00059
00060 CDynamicGrid( float x_min = -10.0f,
00061 float x_max = 10.0f,
00062 float y_min = -10.0f,
00063 float y_max = 10.0f,
00064 float resolution = 0.10f) :
00065 m_map(),
00066 m_x_min(),m_x_max(),m_y_min(),m_y_max(),
00067 m_resolution(),
00068 m_size_x(), m_size_y()
00069 {
00070 setSize(x_min,x_max,y_min,y_max,resolution);
00071 }
00072
00073
00074
00075 virtual ~CDynamicGrid()
00076 {
00077 }
00078
00079
00080
00081
00082 void setSize( float x_min,
00083 float x_max,
00084 float y_min,
00085 float y_max,
00086 float resolution )
00087 {
00088
00089 m_x_min = resolution*round(x_min/resolution);
00090 m_y_min = resolution*round(y_min/resolution);
00091 m_x_max = resolution*round(x_max/resolution);
00092 m_y_max = resolution*round(y_max/resolution);
00093
00094
00095 m_resolution = resolution;
00096
00097
00098 m_size_x = round((m_x_max-m_x_min)/m_resolution);
00099 m_size_y = round((m_y_max-m_y_min)/m_resolution);
00100
00101
00102 m_map.resize(m_size_x*m_size_y);
00103 }
00104
00105
00106
00107 void clear()
00108 {
00109 m_map.clear();
00110 m_map.resize(m_size_x*m_size_y);
00111 }
00112
00113
00114
00115 inline void fill( const T& value ) { std::fill(m_map.begin(),m_map.end(),value); }
00116
00117
00118
00119
00120 virtual void resize(
00121 float new_x_min,
00122 float new_x_max,
00123 float new_y_min,
00124 float new_y_max,
00125 const T& defaultValueNewCells,
00126 float additionalMarginMeters = 2.0f )
00127 {
00128 MRPT_START;
00129
00130 MRPT_CHECK_NORMAL_NUMBER(new_x_min)
00131 MRPT_CHECK_NORMAL_NUMBER(new_x_max)
00132 MRPT_CHECK_NORMAL_NUMBER(new_y_min)
00133 MRPT_CHECK_NORMAL_NUMBER(new_y_max)
00134
00135 unsigned int x,y;
00136 unsigned int extra_x_izq,extra_y_arr,new_size_x=0,new_size_y=0;
00137 typename std::vector<T> new_map;
00138 typename std::vector<T>::iterator itSrc,itDst;
00139
00140
00141 if (new_x_min>=m_x_min &&
00142 new_y_min>=m_y_min &&
00143 new_x_max<=m_x_max &&
00144 new_y_max<=m_y_max) return;
00145
00146 if (new_x_min>m_x_min) new_x_min=m_x_min;
00147 if (new_x_max<m_x_max) new_x_max=m_x_max;
00148 if (new_y_min>m_y_min) new_y_min=m_y_min;
00149 if (new_y_max<m_y_max) new_y_max=m_y_max;
00150
00151
00152 if (additionalMarginMeters>0)
00153 {
00154 if (new_x_min<m_x_min) new_x_min= floor(new_x_min-additionalMarginMeters);
00155 if (new_x_max>m_x_max) new_x_max= ceil(new_x_max+additionalMarginMeters);
00156 if (new_y_min<m_y_min) new_y_min= floor(new_y_min-additionalMarginMeters);
00157 if (new_y_max>m_y_max) new_y_max= ceil(new_y_max+additionalMarginMeters);
00158 }
00159
00160
00161 if (fabs(new_x_min/m_resolution - round(new_x_min/m_resolution))>0.05f )
00162 new_x_min = m_resolution*round(new_x_min/m_resolution);
00163 if (fabs(new_y_min/m_resolution - round(new_y_min/m_resolution))>0.05f )
00164 new_y_min = m_resolution*round(new_y_min/m_resolution);
00165 if (fabs(new_x_max/m_resolution - round(new_x_max/m_resolution))>0.05f )
00166 new_x_max = m_resolution*round(new_x_max/m_resolution);
00167 if (fabs(new_y_max/m_resolution - round(new_y_max/m_resolution))>0.05f )
00168 new_y_max = m_resolution*round(new_y_max/m_resolution);
00169
00170
00171 extra_x_izq = round((m_x_min-new_x_min) / m_resolution);
00172 extra_y_arr = round((m_y_min-new_y_min) / m_resolution);
00173
00174 new_size_x = round((new_x_max-new_x_min) / m_resolution);
00175 new_size_y = round((new_y_max-new_y_min) / m_resolution);
00176
00177
00178 new_map.resize(new_size_x*new_size_y,defaultValueNewCells);
00179
00180
00181 for (y=0;y<m_size_y;y++)
00182 {
00183 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);
00184 x<m_size_x;
00185 x++,itSrc++,itDst++)
00186 {
00187 *itDst = *itSrc;
00188 }
00189 }
00190
00191
00192 m_x_min = new_x_min;
00193 m_x_max = new_x_max;
00194 m_y_min = new_y_min;
00195 m_y_max = new_y_max;
00196
00197 m_size_x = new_size_x;
00198 m_size_y = new_size_y;
00199
00200
00201 m_map.swap(new_map);
00202
00203 MRPT_END;
00204
00205 }
00206
00207
00208
00209 inline T* cellByPos( float x, float y )
00210 {
00211 int cx = x2idx(x);
00212 int cy = y2idx(y);
00213
00214 if( cx<0 || cx>=static_cast<int>(m_size_x) ) return NULL;
00215 if( cy<0 || cy>=static_cast<int>(m_size_y) ) return NULL;
00216
00217 return &m_map[ cx + cy*m_size_x ];
00218 }
00219
00220
00221
00222 inline const T* cellByPos( float x, float y ) const
00223 {
00224 int cx = x2idx(x);
00225 int cy = y2idx(y);
00226
00227 if( cx<0 || cx>=static_cast<int>(m_size_x) ) return NULL;
00228 if( cy<0 || cy>=static_cast<int>(m_size_y) ) return NULL;
00229
00230 return &m_map[ cx + cy*m_size_x ];
00231 }
00232
00233
00234
00235 inline T* cellByIndex( unsigned int cx, unsigned int cy )
00236 {
00237 if( cx>=m_size_x || cy>=m_size_y)
00238 return NULL;
00239 else return &m_map[ cx + cy*m_size_x ];
00240 }
00241
00242
00243
00244 inline const T* cellByIndex( unsigned int cx, unsigned int cy ) const
00245 {
00246 if( cx>=m_size_x || cy>=m_size_y)
00247 return NULL;
00248 else return &m_map[ cx + cy*m_size_x ];
00249 }
00250
00251
00252
00253 inline size_t getSizeX() const { return m_size_x; }
00254
00255
00256
00257 inline size_t getSizeY() const { return m_size_y; }
00258
00259
00260
00261 inline float getXMin()const { return m_x_min; }
00262
00263
00264
00265 inline float getXMax()const { return m_x_max; }
00266
00267
00268
00269 inline float getYMin()const { return m_y_min; }
00270
00271
00272
00273 inline float getYMax()const { return m_y_max; }
00274
00275
00276
00277 inline float getResolution()const { return m_resolution; }
00278
00279
00280 virtual float cell2float(const T& c) const
00281 {
00282 return 0;
00283 }
00284
00285 void saveToTextFile(const std::string &fileName) const
00286 {
00287 FILE *f=os::fopen(fileName.c_str(),"wt");
00288 if(!f) return;
00289 for (unsigned int cy=0;cy<m_size_y;cy++)
00290 {
00291 for (unsigned int cx=0;cx<m_size_x;cx++)
00292 os::fprintf(f,"%f ",cell2float(m_map[ cx + cy*m_size_x ]));
00293 os::fprintf(f,"\n");
00294 }
00295 os::fclose(f);
00296 }
00297
00298
00299
00300 inline int x2idx(float x) const { return static_cast<int>( (x-m_x_min)/m_resolution ); }
00301 inline int y2idx(float y) const { return static_cast<int>( (y-m_y_min)/m_resolution ); }
00302 inline int xy2idx(float x,float y) const { return x2idx(x) + y2idx(y)*m_size_x; }
00303
00304
00305 inline void idx2cxcy(const int &idx, int &cx, int &cy ) const
00306 {
00307 cx = idx % m_size_x;
00308 cy = idx / m_size_x;
00309 }
00310
00311
00312
00313 inline float idx2x(int cx) const { return m_x_min+(cx+0.5f)*m_resolution; }
00314 inline float idx2y(int cy) const { return m_y_min+(cy+0.5f)*m_resolution; }
00315
00316
00317
00318 inline int x2idx(float x,float x_min) const { return static_cast<int>((x-m_x_min)/m_resolution ); }
00319 inline int y2idx(float y, float y_min) const { return static_cast<int>((y-m_y_min)/m_resolution ); }
00320
00321
00322
00323
00324
00325
00326
00327 template <class MAT>
00328 void getAsMatrix(MAT &m) const
00329 {
00330 m.setSize(m_size_y, m_size_x);
00331 if (m_map.empty()) return;
00332 const T* c = &m_map[0];
00333 for (size_t cy=0;cy<m_size_y;cy++)
00334 for (size_t cx=0;cx<m_size_x;cx++)
00335 m.set_unsafe(cy,cx, *c++);
00336 }
00337
00338 };
00339
00340 }
00341 }
00342 #endif