Go to the documentation of this file.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
00029
00030
00031
00032
00033
00034
00035
00036
00044 #ifndef _TRANSFORMATION2_HXX_
00045 #define _TRANSFORMATION2_HXX_
00046
00047 #include <cmath>
00048
00049 namespace AISNavigation
00050 {
00051
00053 template <class T>
00054 struct Vector2{
00055 T values[2] ;
00056
00058 Vector2(T x, T y) {values[0]=x; values[1]=y;}
00060 Vector2() {values[0]=0; values[1]=0;}
00061
00063 inline const T& x() const {return values[0];}
00065 inline const T& y() const {return values[1];}
00066
00068 inline T& x() {return values[0];}
00070 inline T& y() {return values[1];}
00071
00073 inline T norm2() const {
00074 return values[0]*values[0]+values[1]*values[1];
00075 }
00076
00077 };
00078
00080 template <class T>
00081 inline Vector2<T> operator * (const T& d, const Vector2<T>& v) {
00082 return Vector2<T>(v.values[0]*d, v.values[1]*d);
00083 }
00084
00086 template <class T>
00087 inline Vector2<T> operator * (const Vector2<T>& v, const T& d) {
00088 return Vector2<T>(v.values[0]*d, v.values[1]*d);
00089 }
00090
00092 template <class T>
00093 inline T operator * (const Vector2<T>& v1, const Vector2<T>& v2){
00094 return v1.values[0]*v2.values[0]
00095 + v1.values[1]*v2.values[1];
00096 }
00097
00099 template <class T>
00100 inline Vector2<T> operator + (const Vector2<T>& v1, const Vector2<T>& v2){
00101 return Vector2<T>(v1.values[0]+v2.values[0],
00102 v1.values[1]+v2.values[1]);
00103 }
00104
00106 template <class T>
00107 Vector2<T> operator - (const Vector2<T>& v1, const Vector2<T>& v2){
00108 return Vector2<T>(v1.values[0]-v2.values[0],
00109 v1.values[1]-v2.values[1]);
00110 }
00111
00112
00119 template <class T>
00120 struct Pose2{
00121 T values[3];
00122
00124 inline const T& x() const {return values[0];}
00126 inline const T& y() const {return values[1];}
00128 inline const T& theta() const {return values[2];}
00129
00131 inline T& x() {return values[0];}
00133 inline T& y() {return values[1];}
00135 inline T& theta() {return values[2];}
00136
00138 Pose2(){
00139 values[0]=0.; values[1]=0.; values[2]=0.;
00140 }
00141
00143 Pose2(const T& x, const T& y, const T& theta){
00144 values[0]=x, values[1]=y, values[2]=theta;
00145 }
00146 };
00147
00149 template <class T>
00150 Pose2<T> operator * (const Pose2<T>& v, const T& d){
00151 Pose2<T> r;
00152 for (int i=0; i<3; i++){
00153 r.values[i]=v.values[i]*d;
00154 }
00155 return r;
00156 }
00157
00158
00160 template <class T>
00161 struct Transformation2{
00162 T rotationMatrix[2][2];
00163 T translationVector[2];
00164
00168 Transformation2(bool initAsIdentity = true){
00169 if (initAsIdentity) {
00170 rotationMatrix[0][0]=1.; rotationMatrix[0][1]=0.;
00171 rotationMatrix[1][0]=0.; rotationMatrix[1][1]=1.;
00172 translationVector[0]=0.;
00173 translationVector[1]=0.;
00174 }
00175 }
00176
00178 inline static Transformation2<T> identity(){
00179 Transformation2<T> m(true);
00180 return m;
00181 }
00182
00184 Transformation2 (const T& x, const T& y, const T& theta){
00185 setRotation(theta);
00186 setTranslation(x,y);
00187 }
00188
00190 Transformation2 (const T& _theta, const Vector2<T>& trans){
00191 setRotation(_theta);
00192 setTranslation(trans.x(), trans.y());
00193 }
00194
00195
00197 Transformation2 (const Pose2<T>& v){
00198 setRotation(v.theta());
00199 setTranslation(v.x(),v.y());
00200 }
00201
00202
00204 inline Vector2<T> translation() const {
00205 return Vector2<T>(translationVector[0],
00206 translationVector[1]);
00207 }
00208
00210 inline T rotation() const {
00211 return atan2(rotationMatrix[1][0],rotationMatrix[0][0]);
00212 }
00213
00215 inline Pose2<T> toPoseType() const {
00216 Vector2<T> t=translation();
00217 T r=rotation();
00218 Pose2<T> rv(t.x(), t.y(), r );
00219 return rv;
00220 }
00221
00223 inline void setTranslation(const Vector2<T>& t){
00224 setTranslation(t.x(),t.y());
00225 }
00226
00228 inline void setRotation(const T& theta){
00229 T s=sin(theta), c=cos(theta);
00230 rotationMatrix[0][0]=c, rotationMatrix[0][1]=-s;
00231 rotationMatrix[1][0]=s, rotationMatrix[1][1]= c;
00232 }
00233
00235 inline void setTranslation(const T& x, const T& y){
00236 translationVector[0]=x;
00237 translationVector[1]=y;
00238 }
00239
00241 inline Transformation2<T> inv() const {
00242 Transformation2<T> rv(*this);
00243 for (int i=0; i<2; i++)
00244 for (int j=0; j<2; j++){
00245 rv.rotationMatrix[i][j]=rotationMatrix[j][i];
00246 }
00247
00248 for (int i=0; i<2; i++){
00249 rv.translationVector[i]=0;
00250 for (int j=0; j<2; j++){
00251 rv.translationVector[i]-=rv.rotationMatrix[i][j]*translationVector[j];
00252 }
00253 }
00254 return rv;
00255 }
00256
00257 };
00258
00260 template <class T>
00261 Vector2<T> operator * (const Transformation2<T>& m, const Vector2<T>& v){
00262 return Vector2<T>(
00263 m.rotationMatrix[0][0]*v.values[0]+
00264 m.rotationMatrix[0][1]*v.values[1]+
00265 m.translationVector[0],
00266 m.rotationMatrix[1][0]*v.values[0]+
00267 m.rotationMatrix[1][1]*v.values[1]+
00268 m.translationVector[1]);
00269 }
00270
00272 template <class T>
00273 Transformation2<T> operator * (const Transformation2<T>& m1, const Transformation2<T>& m2){
00274 Transformation2<T> rt;
00275 for (int i=0; i<2; i++)
00276 for (int j=0; j<2; j++){
00277 rt.rotationMatrix[i][j]=0.;
00278 for (int k=0; k<2; k++)
00279 rt.rotationMatrix[i][j]+=m1.rotationMatrix[i][k]*m2.rotationMatrix[k][j];
00280 }
00281 for (int i=0; i<2; i++){
00282 rt.translationVector[i]=m1.translationVector[i];
00283 for (int j=0; j<2; j++)
00284 rt.translationVector[i]+=m1.rotationMatrix[i][j]*m2.translationVector[j];
00285 }
00286 return rt;
00287 }
00288
00289
00291 template <class T>
00292 struct SMatrix3{
00293 T values[3][3];
00294 T det() const;
00295 SMatrix3<T> transpose() const;
00296 SMatrix3<T> adj() const;
00297 SMatrix3<T> inv() const;
00298 };
00299
00300
00302 template <class T>
00303 Pose2<T> operator * (const SMatrix3<T>& m, const Pose2<T>& p){
00304 Pose2<T> v;
00305 for (int i=0; i<3; i++){
00306 v.values[i]=0.;
00307 for (int j=0; j<3; j++)
00308 v.values[i]+=m.values[i][j]*p.values[j];
00309 }
00310 return v;
00311 }
00312
00314 template <class T>
00315 SMatrix3<T> operator * (const SMatrix3<T>& s, T& d){
00316 SMatrix3<T> m;
00317 for (int i=0; i<3; i++)
00318 for (int j=0; j<3; j++)
00319 m.values[i][j]=d*s.values[i][j];
00320 return m;
00321 }
00322
00324 template <class T>
00325 SMatrix3<T> operator * (const SMatrix3<T>& s1, const SMatrix3<T>& s2){
00326 SMatrix3<T> m;
00327 for (int i=0; i<3; i++)
00328 for (int j=0; j<3; j++){
00329 m.values[i][j]=0.;
00330 for (int k=0; k<3; k++){
00331 m.values[i][j]+=s1.values[i][k]*s2.values[k][j];
00332 }
00333 }
00334 return m;
00335 }
00336
00338 template <class T>
00339 SMatrix3<T> operator + (const SMatrix3<T>& s1, const SMatrix3<T>& s2){
00340 SMatrix3<T> m;
00341 for (int i=0; i<3; i++)
00342 for (int j=0; j<3; j++){
00343 m.values[i][j]=s1.values[i][j]+s2.values[i][j];
00344 }
00345 return m;
00346 }
00347
00348
00350 template <class T>
00351 T SMatrix3<T>::det() const{
00352 T dp= values[0][0]*values[1][1]*values[2][2]
00353 +values[0][1]*values[1][2]*values[2][0]
00354 +values[0][2]*values[1][0]*values[2][1];
00355 T dm=values[2][0]*values[1][1]*values[0][2]
00356 +values[2][1]*values[1][2]*values[0][0]
00357 +values[2][2]*values[1][0]*values[0][1];
00358 return dp-dm;
00359 }
00360
00362 template <class T>
00363 SMatrix3<T> SMatrix3<T>::transpose() const{
00364 SMatrix3<T> m;
00365 for (int i=0; i<3; i++)
00366 for (int j=0; j<3; j++)
00367 m.values[j][i]=values[i][j];
00368 return m;
00369 }
00370
00372 template <class T>
00373 SMatrix3<T> SMatrix3<T>::adj() const{
00374 SMatrix3<T> m;
00375 m.values[0][0]= values[1][1]*values[2][2]-values[2][1]*values[1][2];
00376 m.values[0][1]=-values[1][0]*values[2][2]+values[1][2]*values[2][0];
00377 m.values[0][2]= values[1][0]*values[2][1]-values[2][0]*values[1][1];
00378 m.values[1][0]=-values[0][1]*values[2][2]+values[2][1]*values[0][2];
00379 m.values[1][1]= values[0][0]*values[2][2]-values[2][0]*values[0][2];
00380 m.values[1][2]=-values[0][0]*values[2][1]+values[2][0]*values[0][1];
00381 m.values[2][0]= values[0][1]*values[1][2]-values[1][1]*values[0][2];
00382 m.values[2][1]=-values[0][0]*values[1][2]+values[1][0]*values[0][2];
00383 m.values[2][2]= values[0][0]*values[1][1]-values[1][0]*values[0][1];
00384 return m;
00385 }
00386
00388 template <class T>
00389 SMatrix3<T> SMatrix3<T>::inv() const{
00390 T id=1./det();
00391 SMatrix3<T> i=adj().transpose();
00392 return i*id;
00393 }
00394
00395
00396
00398 template <class T>
00399 struct Operations2D{
00400 typedef T BaseType;
00401 typedef Pose2<T> PoseType;
00402 typedef Pose2<T> ParametersType;
00403 typedef T RotationType;
00404 typedef Vector2<T> TranslationType;
00405 typedef Transformation2<T> TransformationType;
00406 typedef SMatrix3<T> CovarianceType;
00407 typedef SMatrix3<T> InformationType;
00408 };
00409
00410 }
00411
00412 #endif