Main MRPT website > C++ reference
MRPT logo

CPose2DGridTemplate.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 CPose2DGridTemplate_H
00029 #define CPose2DGridTemplate_H
00030 
00031 #include <mrpt/utils/CSerializable.h>
00032 
00033 namespace mrpt
00034 {
00035 namespace poses
00036 {
00037         using namespace mrpt::math;
00038         using namespace mrpt::utils;
00039 
00040         /** This is a template class for storing a 3D (2D+heading) grid containing any kind of data.
00041          * \sa
00042          */
00043         template<class T>
00044         class CPose2DGridTemplate
00045         {
00046         protected:
00047                 /** The limits and resolution of the grid:
00048                   */
00049                 double                          m_xMin, m_xMax,
00050                                                         m_yMin, m_yMax,
00051                                                         m_phiMin, m_phiMax,
00052                                                         m_resolutionXY,m_resolutionPhi;
00053 
00054                 /** The size of "m_data" is m_sizeX ·m_sizeY ·m_sizePhi
00055                   */
00056                 size_t          m_sizeX,m_sizeY,m_sizePhi, m_sizeXY;
00057 
00058                 /** The indexes of the "left" borders:
00059                   */
00060                 int                                     m_idxLeftX, m_idxLeftY, m_idxLeftPhi;
00061 
00062                 /** The data:
00063                   */
00064                 std::vector<T>          m_data;
00065 
00066         public:
00067                 /** Returns "indexes" from coordinates:
00068                   */
00069                 size_t  x2idx(double x) const
00070                 {
00071                         int idx = round( (x-m_xMin) / m_resolutionXY );
00072                         ASSERT_(idx>=0 && idx< static_cast<int>(m_sizeX));
00073                         return idx;
00074                 }
00075 
00076                 /** Returns "indexes" from coordinates:
00077                   */
00078                 size_t  y2idx(double y) const
00079                 {
00080                         int idx = round( (y-m_yMin) / m_resolutionXY );
00081                         ASSERT_(idx>=0 && idx< static_cast<int>(m_sizeY));
00082                         return idx;
00083                 }
00084 
00085                 /** Returns "indexes" from coordinates:
00086                   */
00087                 size_t  phi2idx(double phi) const
00088                 {
00089                         int idx = round( (phi-m_phiMin) / m_resolutionPhi );
00090                         ASSERT_(idx>=0 && idx< static_cast<int>(m_sizePhi) );
00091                         return idx;
00092                 }
00093 
00094                 /** Returns coordinates from "indexes":
00095                   */
00096                 double  idx2x(size_t x) const
00097                 {
00098                         ASSERT_(x<m_sizeX);
00099                         return m_xMin + x * m_resolutionXY;
00100                 }
00101 
00102                 /** Returns coordinates from "indexes":
00103                   */
00104                 double  idx2y(size_t y) const
00105                 {
00106                         ASSERT_(y<m_sizeY);
00107                         return m_yMin + y * m_resolutionXY;
00108                 }
00109 
00110                 /** Returns coordinates from "indexes":
00111                   */
00112                 double  idx2phi(size_t phi) const
00113                 {
00114                         ASSERT_(phi<m_sizePhi);
00115                         return m_phiMin + phi * m_resolutionPhi;
00116                 }
00117 
00118                 /** Default constructor:
00119                   */
00120                 CPose2DGridTemplate(
00121                         double          xMin = -1.0f,
00122                         double          xMax = 1.0f,
00123                         double          yMin = -1.0f,
00124                         double          yMax = 1.0f,
00125                         double          resolutionXY = 0.5f,
00126                         double          resolutionPhi = DEG2RAD(180),
00127                         double          phiMin = -M_PIf,
00128                         double          phiMax = M_PIf
00129                         ) :
00130                                 m_xMin(), m_xMax(),
00131                                 m_yMin(), m_yMax(),
00132                                 m_phiMin(), m_phiMax(),
00133                                 m_resolutionXY(),m_resolutionPhi(),
00134                                 m_sizeX(),m_sizeY(),m_sizePhi(), m_sizeXY(),
00135                                 m_idxLeftX(), m_idxLeftY(), m_idxLeftPhi(),
00136                                 m_data()
00137                 {
00138                         setSize(xMin,xMax,yMin,yMax,resolutionXY,resolutionPhi,phiMin,phiMax);
00139                 }
00140 
00141                 virtual ~CPose2DGridTemplate() { }
00142 
00143 
00144                 /** Changes the limits and size of the grid, erasing previous contents:
00145                   */
00146                 void  setSize(
00147                         double          xMin,
00148                         double          xMax,
00149                         double          yMin,
00150                         double          yMax,
00151                         double          resolutionXY,
00152                         double          resolutionPhi,
00153                         double          phiMin = -M_PIf,
00154                         double          phiMax = M_PIf
00155                         )
00156                 {
00157                         // Checks
00158                         ASSERT_( xMax > xMin );
00159                         ASSERT_( yMax > yMin );
00160                         ASSERT_( phiMax >= phiMin );
00161                         ASSERT_( resolutionXY>0 );
00162                         ASSERT_( resolutionPhi>0 );
00163 
00164                         // Copy data:
00165                         m_xMin = xMin;                  m_xMax = xMax;
00166                         m_yMin = yMin;                  m_yMax = yMax;
00167                         m_phiMin = phiMin;              m_phiMax = phiMax;
00168                         m_resolutionXY = resolutionXY;
00169                         m_resolutionPhi = resolutionPhi;
00170 
00171                         // Compute the indexes of the starting borders:
00172                         m_idxLeftX = round( xMin/resolutionXY ) ;
00173                         m_idxLeftY = round( yMin/resolutionXY ) ;
00174                         m_idxLeftPhi = round( phiMin/resolutionPhi ) ;
00175 
00176                         // Compute new required space:
00177                         m_sizeX = round( xMax/resolutionXY ) - m_idxLeftX + 1;
00178                         m_sizeY = round( yMax/resolutionXY ) - m_idxLeftY + 1;
00179                         m_sizePhi = round( phiMax/resolutionPhi ) - m_idxLeftPhi + 1;
00180                         m_sizeXY = m_sizeX * m_sizeY;
00181 
00182                         // Resize "m_data":
00183                         m_data.clear();
00184                         m_data.resize( m_sizeX * m_sizeY * m_sizePhi );
00185                 }
00186 
00187                 /** Reads the contents of a cell
00188                   */
00189                 const T*  getByPos( double x,double y, double phi ) const
00190                 {
00191             return getByIndex( x2idx(x),y2idx(y),phi2idx(phi) );
00192                 }
00193 
00194                 /** Reads the contents of a cell
00195                   */
00196                 T*  getByPos( double x,double y, double phi )
00197                 {
00198             return getByIndex( x2idx(x),y2idx(y),phi2idx(phi) );
00199                 }
00200 
00201                 /** Reads the contents of a cell
00202                   */
00203                 const T*  getByIndex( size_t x,size_t y, size_t phi )  const
00204                 {
00205                         ASSERT_(x>=0 && x<m_sizeX);
00206                         ASSERT_(y>=0 && y<m_sizeY);
00207                         ASSERT_(phi>=0 && phi<m_sizePhi);
00208                         return &m_data[ phi*m_sizeXY + y*m_sizeX + x ];
00209                 }
00210 
00211                 /** Reads the contents of a cell
00212                   */
00213                 T*  getByIndex( size_t x,size_t y, size_t phi )
00214                 {
00215                         ASSERT_(x>=0 && x<m_sizeX);
00216                         ASSERT_(y>=0 && y<m_sizeY);
00217                         ASSERT_(phi>=0 && phi<m_sizePhi);
00218                         return &m_data[ phi*m_sizeXY + y*m_sizeX + x ];
00219                 }
00220 
00221                 /** Returns the whole grid as a matrix, for a given constant "phi" and where each row contains values for a fixed "y".
00222                   */
00223                 void getAsMatrix( const double &phi, CMatrixTemplate<T> &outMat )
00224                 {
00225                         MRPT_START
00226                         outMat.setSize( m_sizeY, m_sizeX );
00227                         size_t phiIdx = phi2idx(phi);
00228                         ASSERT_(phi<m_sizePhi);
00229                         for (size_t y=0;y<m_sizeY;y++)
00230                                 for (size_t x=0;x<m_sizeX;x++)
00231                                         outMat(y,x)=m_data[ phiIdx*m_sizeXY + y*m_sizeX + x ];
00232                         MRPT_END
00233                 }
00234 
00235                 /** Get info about the grid:
00236                   */
00237                 double  getXMin() const { return m_xMin; }
00238                 double  getXMax() const { return m_xMax; }
00239                 double  getYMin() const { return m_yMin; }
00240                 double  getYMax() const { return m_yMax; }
00241                 double  getPhiMin() const { return m_phiMin; }
00242                 double  getPhiMax() const { return m_phiMax; }
00243                 double  getResolutionXY() const { return m_resolutionXY; }
00244                 double  getResolutionPhi() const { return m_resolutionPhi; }
00245                 size_t  getSizeX() const { return m_sizeX; }
00246                 size_t  getSizeY() const { return m_sizeY; }
00247                 size_t  getSizePhi() const { return m_sizePhi; }
00248 
00249 
00250         }; // End of class def.
00251 
00252         } // End of namespace
00253 } // End of namespace
00254 
00255 #endif



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