transformation2.hh
Go to the documentation of this file.
00001 /**********************************************************************
00002  *
00003  * This source code is part of the Tree-based Network Optimizer (TORO)
00004  *
00005  * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss, 
00006  *                         Slawomir Grzonka, and Wolfram Burgard
00007  *
00008  * TORO is licences under the Common Creative License,
00009  * Attribution-NonCommercial-ShareAlike 3.0
00010  *
00011  * You are free:
00012  *   - to Share - to copy, distribute and transmit the work
00013  *   - to Remix - to adapt the work
00014  *
00015  * Under the following conditions:
00016  *
00017  *   - Attribution. You must attribute the work in the manner specified
00018  *     by the author or licensor (but not in any way that suggests that
00019  *     they endorse you or your use of the work).
00020  *  
00021  *   - Noncommercial. You may not use this work for commercial purposes.
00022  *  
00023  *   - Share Alike. If you alter, transform, or build upon this work,
00024  *     you may distribute the resulting work only under the same or
00025  *     similar license to this one.
00026  *
00027  * Any of the above conditions can be waived if you get permission
00028  * from the copyright holder.  Nothing in this license impairs or
00029  * restricts the author's moral rights.
00030  *
00031  * TORO is distributed in the hope that it will be useful,
00032  * but WITHOUT ANY WARRANTY; without even the implied 
00033  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
00034  * PURPOSE.  
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 } // namespace AISNavigation
00411 
00412 #endif


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:31