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 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
00041
00042
00043 template<class T>
00044 class CPose2DGridTemplate
00045 {
00046 protected:
00047
00048
00049 double m_xMin, m_xMax,
00050 m_yMin, m_yMax,
00051 m_phiMin, m_phiMax,
00052 m_resolutionXY,m_resolutionPhi;
00053
00054
00055
00056 size_t m_sizeX,m_sizeY,m_sizePhi, m_sizeXY;
00057
00058
00059
00060 int m_idxLeftX, m_idxLeftY, m_idxLeftPhi;
00061
00062
00063
00064 std::vector<T> m_data;
00065
00066 public:
00067
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
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
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
00095
00096 double idx2x(size_t x) const
00097 {
00098 ASSERT_(x<m_sizeX);
00099 return m_xMin + x * m_resolutionXY;
00100 }
00101
00102
00103
00104 double idx2y(size_t y) const
00105 {
00106 ASSERT_(y<m_sizeY);
00107 return m_yMin + y * m_resolutionXY;
00108 }
00109
00110
00111
00112 double idx2phi(size_t phi) const
00113 {
00114 ASSERT_(phi<m_sizePhi);
00115 return m_phiMin + phi * m_resolutionPhi;
00116 }
00117
00118
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
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
00158 ASSERT_( xMax > xMin );
00159 ASSERT_( yMax > yMin );
00160 ASSERT_( phiMax >= phiMin );
00161 ASSERT_( resolutionXY>0 );
00162 ASSERT_( resolutionPhi>0 );
00163
00164
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
00172 m_idxLeftX = round( xMin/resolutionXY ) ;
00173 m_idxLeftY = round( yMin/resolutionXY ) ;
00174 m_idxLeftPhi = round( phiMin/resolutionPhi ) ;
00175
00176
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
00183 m_data.clear();
00184 m_data.resize( m_sizeX * m_sizeY * m_sizePhi );
00185 }
00186
00187
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
00195
00196 T* getByPos( double x,double y, double phi )
00197 {
00198 return getByIndex( x2idx(x),y2idx(y),phi2idx(phi) );
00199 }
00200
00201
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
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
00222
00223 void getAsMatrix( const double &phi, CMatrixTemplate<T> &outMat )
00224 {
00225 MRPT_TRY_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_TRY_END
00233 }
00234
00235
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 };
00251
00252 }
00253 }
00254
00255 #endif