Main MRPT website > C++ reference
MRPT logo

CDynamicGrid.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
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         /** A 2D grid of dynamic size which stores any kind of data at each cell.
00040                   * \tparam T The type of each cell in the 2D grid.
00041           */
00042         template <class T>
00043         class CDynamicGrid
00044         {
00045         protected:
00046             /** The cells.
00047               */
00048             std::vector<T>              m_map;
00049 
00050             /** Used only from logically const method that really need to modify the object */
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             /** Constructor
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             /** Destructor
00074               */
00075             virtual ~CDynamicGrid()
00076             {
00077             }
00078 
00079             /** Changes the size of the grid, ERASING all previous contents.
00080               * \sa resize
00081               */
00082             void  setSize(      float   x_min,
00083                                         float   x_max,
00084                                         float   y_min,
00085                                         float   y_max,
00086                                         float   resolution )
00087             {
00088                 // Adjust sizes to adapt them to full sized cells acording to the resolution:
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                 // Res:
00095                 m_resolution = resolution;
00096 
00097                 // Now the number of cells should be integers:
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                 // Cells memory:
00102                 m_map.resize(m_size_x*m_size_y);
00103             }
00104 
00105             /** Erase the contents of all the cells.
00106               */
00107             void  clear()
00108             {
00109                 m_map.clear();
00110                 m_map.resize(m_size_x*m_size_y);
00111             }
00112 
00113             /** Fills all the cells with the same value
00114               */
00115             inline void fill( const T& value ) { std::fill(m_map.begin(),m_map.end(),value); }
00116 
00117             /** Changes the size of the grid, maintaining previous contents.
00118               * \sa setSize
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                 // Is resize really necesary?
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                 // Additional margin:
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                 // Adjust sizes to adapt them to full sized cells acording to the resolution:
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                 // Change the map size: Extensions at each side:
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                 // Reserve new memory:
00178                 new_map.resize(new_size_x*new_size_y,defaultValueNewCells);
00179 
00180                 // Copy previous rows:
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                 // Update the new map limits:
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                 // Keep the new map only:
00201                 m_map.swap(new_map);
00202 
00203                 MRPT_END;
00204 
00205             }
00206 
00207             /** Returns a pointer to the contents of a cell given by its coordinates, or NULL if it is out of the map extensions.
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             /** Returns a pointer to the contents of a cell given by its coordinates, or NULL if it is out of the map extensions.
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             /** Returns a pointer to the contents of a cell given by its cell indexes, or NULL if it is out of the map extensions.
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             /** Returns a pointer to the contents of a cell given by its cell indexes, or NULL if it is out of the map extensions.
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             /** Returns the horizontal size of grid map in cells count.
00252                 */
00253             inline size_t getSizeX() const { return m_size_x; }
00254 
00255             /** Returns the vertical size of grid map in cells count.
00256                 */
00257             inline size_t getSizeY() const { return m_size_y; }
00258 
00259             /** Returns the "x" coordinate of left side of grid map.
00260                 */
00261             inline float  getXMin()const  { return m_x_min; }
00262 
00263             /** Returns the "x" coordinate of right side of grid map.
00264                 */
00265             inline float  getXMax()const  { return m_x_max; }
00266 
00267             /** Returns the "y" coordinate of top side of grid map.
00268                 */
00269             inline float  getYMin()const  { return m_y_min; }
00270 
00271             /** Returns the "y" coordinate of bottom side of grid map.
00272                 */
00273             inline float  getYMax()const  { return m_y_max; }
00274 
00275             /** Returns the resolution of the grid map.
00276                 */
00277             inline float  getResolution()const  { return m_resolution; }
00278 
00279             /** The user must implement this in order to provide "saveToTextFile" a way to convert each cell into a numeric value */
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             /** Transform a coordinate values into cell indexes.
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             /** Transform a global (linear) cell index value into its corresponding (x,y) cell indexes. */
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             /** Transform a cell index into a coordinate value.
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             /** Transform a coordinate value into a cell index, using a diferent "x_min" value
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                         /** Get the entire grid as a matrix. 
00322                           *  \tparam MAT The type of the matrix, typically a CMatrixDouble.
00323                           *  \param[out] m The output matrix; will be set automatically to the correct size.
00324                           *  Entry (cy,cx) in the matrix contains the grid cell with indices (cx,cy).
00325                           * \note This method will compile only for cell types that can be converted to the type of the matrix elements (e.g. double).
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         } // End of namespace
00341 } // end of namespace
00342 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011