transformation2.hh

Go to the documentation of this file.

/**********************************************************************
 *
 * This source code is part of the Tree-based Network Optimizer (TORO)
 *
 * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss, 
 *                         Slawomir Grzonka, and Wolfram Burgard
 *
 * TORO is licences under the Common Creative License,
 * Attribution-NonCommercial-ShareAlike 3.0
 *
 * You are free:
 *   - to Share - to copy, distribute and transmit the work
 *   - to Remix - to adapt the work
 *
 * Under the following conditions:
 *
 *   - Attribution. You must attribute the work in the manner specified
 *     by the author or licensor (but not in any way that suggests that
 *     they endorse you or your use of the work).
 *  
 *   - Noncommercial. You may not use this work for commercial purposes.
 *  
 *   - Share Alike. If you alter, transform, or build upon this work,
 *     you may distribute the resulting work only under the same or
 *     similar license to this one.
 *
 * Any of the above conditions can be waived if you get permission
 * from the copyright holder.  Nothing in this license impairs or
 * restricts the author's moral rights.
 *
 * TORO is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied 
 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 * PURPOSE.  
 **********************************************************************/

#ifndef _TRANSFORMATION2_HXX_
#define _TRANSFORMATION2_HXX_

#include <cmath>

namespace AISNavigation
{

template <class T>
struct Vector2{
  T values[2] ; 

  Vector2(T x, T y)        {values[0]=x; values[1]=y;}
  Vector2()                {values[0]=0; values[1]=0;}

  inline const T& x()      const  {return values[0];}
  inline const T& y()      const  {return values[1];}

  inline T& x()            {return values[0];}
  inline T& y()            {return values[1];}

  inline T norm2() const   {
    return values[0]*values[0]+values[1]*values[1];
  }

};

template <class T>
inline Vector2<T> operator * (const T& d, const Vector2<T>& v) {
  return Vector2<T>(v.values[0]*d, v.values[1]*d);
}

template <class T>
inline Vector2<T> operator * (const Vector2<T>& v, const T& d) {
  return Vector2<T>(v.values[0]*d, v.values[1]*d);
}

template <class T>
inline T operator * (const Vector2<T>& v1, const Vector2<T>& v2){
  return v1.values[0]*v2.values[0] 
    + v1.values[1]*v2.values[1];
}

template <class T>
inline Vector2<T> operator + (const Vector2<T>& v1, const Vector2<T>& v2){
  return Vector2<T>(v1.values[0]+v2.values[0], 
                    v1.values[1]+v2.values[1]);
}

template <class T>
Vector2<T> operator - (const Vector2<T>& v1, const Vector2<T>& v2){
  return Vector2<T>(v1.values[0]-v2.values[0], 
                    v1.values[1]-v2.values[1]);
}


template <class T>
struct Pose2{
  T values[3];

  inline const T& x()     const  {return values[0];}
  inline const T& y()     const  {return values[1];}
  inline const T& theta() const  {return values[2];}

  inline T& x()            {return values[0];}
  inline T& y()            {return values[1];}
  inline T& theta()        {return values[2];}

  Pose2(){
    values[0]=0.; values[1]=0.;  values[2]=0.;
  }

  Pose2(const T& x, const T& y, const T& theta){
    values[0]=x, values[1]=y, values[2]=theta;
  }
};

template <class T>
Pose2<T> operator * (const Pose2<T>& v, const T& d){
  Pose2<T> r;
  for (int i=0; i<3; i++){
    r.values[i]=v.values[i]*d;
  }
  return r;
}


template <class T>
struct Transformation2{
  T rotationMatrix[2][2]; 
  T translationVector[2]; 

  Transformation2(bool initAsIdentity = true){
    if (initAsIdentity) {
      rotationMatrix[0][0]=1.; rotationMatrix[0][1]=0.;
      rotationMatrix[1][0]=0.; rotationMatrix[1][1]=1.;
      translationVector[0]=0.;
      translationVector[1]=0.;
    }
  }

  inline static Transformation2<T> identity(){
    Transformation2<T> m(true);
    return m;
  }

  Transformation2 (const T& x, const T& y, const T& theta){
    setRotation(theta);
    setTranslation(x,y);
  }

  Transformation2 (const T& _theta, const Vector2<T>& trans):
    Transformation2(trans.x(), trans.y(), _theta){}


  Transformation2 (const Pose2<T>& v){
    setRotation(v.theta());
    setTranslation(v.x(),v.y());
  }


  inline Vector2<T> translation() const {
    return Vector2<T>(translationVector[0], 
                      translationVector[1]);
  }
  
  inline T rotation()    const {
    return atan2(rotationMatrix[1][0],rotationMatrix[0][0]);
  }
  
  inline Pose2<T> toPoseType()   const {
    Vector2<T> t=translation();
    T r=rotation();
    Pose2<T> rv(t.x(), t.y(), r );
    return rv;
  }
  
  inline void setTranslation(const Vector2<T>& t){
    setTranslation(t.x(),t.y());
  }

  inline void setRotation(const T& theta){
    T s=sin(theta), c=cos(theta);
    rotationMatrix[0][0]=c, rotationMatrix[0][1]=-s;
    rotationMatrix[1][0]=s, rotationMatrix[1][1]= c;
  }

  inline void setTranslation(const T& x, const T& y){
    translationVector[0]=x;
    translationVector[1]=y;
  }

  inline Transformation2<T> inv() const {
    Transformation2<T> rv(*this);
    for (int i=0; i<2; i++)
      for (int j=0; j<2; j++){
        rv.rotationMatrix[i][j]=rotationMatrix[j][i];
      }

    for (int i=0; i<2; i++){
      rv.translationVector[i]=0;
      for (int j=0; j<2; j++){
        rv.translationVector[i]-=rv.rotationMatrix[i][j]*translationVector[j];
      }
    }
    return rv;
  }

};

template <class T>
Vector2<T> operator * (const Transformation2<T>& m, const Vector2<T>& v){
  return Vector2<T>(
                    m.rotationMatrix[0][0]*v.values[0]+
                    m.rotationMatrix[0][1]*v.values[1]+
                    m.translationVector[0],
                    m.rotationMatrix[1][0]*v.values[0]+
                    m.rotationMatrix[1][1]*v.values[1]+
                    m.translationVector[1]);
}

template <class T>
Transformation2<T> operator * (const Transformation2<T>& m1, const Transformation2<T>& m2){
  Transformation2<T> rt;
  for (int i=0; i<2; i++)
    for (int j=0; j<2; j++){
      rt.rotationMatrix[i][j]=0.;
      for (int k=0; k<2; k++)
        rt.rotationMatrix[i][j]+=m1.rotationMatrix[i][k]*m2.rotationMatrix[k][j];
    }
  for (int i=0; i<2; i++){
    rt.translationVector[i]=m1.translationVector[i];
    for (int j=0; j<2; j++)
      rt.translationVector[i]+=m1.rotationMatrix[i][j]*m2.translationVector[j];
  }
  return rt;
}


template <class T>
struct SMatrix3{
  T values[3][3];
  T det() const;
  SMatrix3<T> transpose() const;
  SMatrix3<T> adj() const;
  SMatrix3<T> inv() const;
};


template <class T>
Pose2<T> operator * (const SMatrix3<T>& m, const Pose2<T>& p){
  Pose2<T> v;
  for (int i=0; i<3; i++){
    v.values[i]=0.;
    for (int j=0; j<3; j++)
      v.values[i]+=m.values[i][j]*p.values[j];
  }
  return v;
}

template <class T>
SMatrix3<T> operator * (const SMatrix3<T>& s, T& d){
  SMatrix3<T> m;
  for (int i=0; i<3; i++)
    for (int j=0; j<3; j++)
      m.values[i][j]=d*s.values[i][j];
  return m;
}

template <class T>
SMatrix3<T> operator * (const SMatrix3<T>& s1, const SMatrix3<T>& s2){
  SMatrix3<T> m;
  for (int i=0; i<3; i++)
    for (int j=0; j<3; j++){
      m.values[i][j]=0.;
      for (int k=0; k<3; k++){
        m.values[i][j]+=s1.values[i][k]*s2.values[k][j];
      }
    }
  return m;
}

template <class T>
SMatrix3<T> operator + (const SMatrix3<T>& s1, const SMatrix3<T>& s2){
  SMatrix3<T> m;
  for (int i=0; i<3; i++)
    for (int j=0; j<3; j++){
      m.values[i][j]=s1.values[i][j]+s2.values[i][j];
    }
  return m;
}


template <class T>
T SMatrix3<T>::det() const{
   T dp= values[0][0]*values[1][1]*values[2][2]
     +values[0][1]*values[1][2]*values[2][0]
     +values[0][2]*values[1][0]*values[2][1];
   T dm=values[2][0]*values[1][1]*values[0][2]
     +values[2][1]*values[1][2]*values[0][0]
     +values[2][2]*values[1][0]*values[0][1];
   return dp-dm;
}

template <class T>
SMatrix3<T>  SMatrix3<T>::transpose() const{
   SMatrix3<T> m;
   for (int i=0; i<3; i++)
     for (int j=0; j<3; j++)
       m.values[j][i]=values[i][j];
   return m;
}

template <class T>
SMatrix3<T>  SMatrix3<T>::adj() const{
   SMatrix3<T> m;
   m.values[0][0]= values[1][1]*values[2][2]-values[2][1]*values[1][2];
   m.values[0][1]=-values[1][0]*values[2][2]+values[1][2]*values[2][0];
   m.values[0][2]= values[1][0]*values[2][1]-values[2][0]*values[1][1];
   m.values[1][0]=-values[0][1]*values[2][2]+values[2][1]*values[0][2];
   m.values[1][1]= values[0][0]*values[2][2]-values[2][0]*values[0][2];
   m.values[1][2]=-values[0][0]*values[2][1]+values[2][0]*values[0][1];
   m.values[2][0]= values[0][1]*values[1][2]-values[1][1]*values[0][2];
   m.values[2][1]=-values[0][0]*values[1][2]+values[1][0]*values[0][2];
   m.values[2][2]= values[0][0]*values[1][1]-values[1][0]*values[0][1];
   return m;
}

template <class T>
SMatrix3<T>  SMatrix3<T>::inv() const{
   T id=1./det();
   SMatrix3<T> i=adj().transpose();
   return i*id;
}



template <class T>
struct Operations2D{
  typedef T                  BaseType;            
  typedef Pose2<T>           PoseType;            
  typedef Pose2<T>           ParametersType;      
  typedef T                  RotationType;        
  typedef Vector2<T>         TranslationType;     
  typedef Transformation2<T> TransformationType;  
  typedef SMatrix3<T>        CovarianceType;      
  typedef SMatrix3<T>        InformationType;     
};

} // namespace AISNavigation

#endif