euler.h
Go to the documentation of this file.
00001 
00059 /*
00060  * euler.h
00061  *
00062  *  Created on: 26.05.2012
00063  *      Author: josh
00064  */
00065 
00066 #ifndef EULER_H_
00067 #define EULER_H_
00068 
00069 
00070 #include <eigen3/Eigen/Dense>
00071 
00073 namespace DOF6
00074 {
00075 
00079   template<typename TYPE>
00080   class EulerAngles
00081   {
00082     typedef Eigen::Matrix<TYPE,3,3> Matrix;
00083     typedef Eigen::Matrix<TYPE,3,1> Vector;
00084 
00085     TYPE alpha_, beta_, gamma_;
00086   public:
00087     EulerAngles():
00088       alpha_(0), beta_(0), gamma_(0)
00089     {
00090     }
00091 
00092     EulerAngles(const Matrix &R)
00093     {
00094       from(R);
00095     }
00096     EulerAngles(const TYPE alpha, const TYPE beta, const TYPE gamma):
00097       alpha_(alpha), beta_(beta), gamma_(gamma)
00098     {
00099     }
00100 
00102     void from(const Matrix &R) {
00103       if(std::abs(R(2,0))!=1)
00104       {
00105         beta_ = -std::asin(R(2,0));
00106         const TYPE t=std::cos(beta_);
00107         gamma_= std::atan2(R(2,1)/t, R(2,2)/t);
00108         alpha_= std::atan2(R(1,0)/t, R(0,0)/t);
00109       }
00110       else
00111       {
00112         alpha_ = 0;
00113         if(R(2,0)==-1)
00114         {
00115           beta_ = M_PI/2;
00116           gamma_= std::atan2(R(0,1),R(0,2));
00117         }
00118         else
00119         {
00120           beta_ = -M_PI/2;
00121           gamma_= std::atan2(-R(0,1),-R(0,2));
00122         }
00123       }
00124     }
00125 
00126     Matrix toRotMat() const {
00127       Matrix R;
00128 
00129       R(0,0)=std::cos(beta_)*std::cos(alpha_);
00130       R(0,1)=std::sin(gamma_)*std::sin(beta_)*std::cos(alpha_)-std::cos(gamma_)*std::sin(alpha_);
00131       R(0,2)=std::cos(gamma_)*std::sin(beta_)*std::cos(alpha_)+std::sin(gamma_)*std::sin(alpha_);
00132 
00133       R(1,0)=std::cos(beta_)*std::sin(alpha_);
00134       R(1,1)=std::sin(gamma_)*std::sin(beta_)*std::sin(alpha_)+std::cos(gamma_)*std::cos(alpha_);
00135       R(1,2)=std::cos(gamma_)*std::sin(beta_)*std::sin(alpha_)-std::sin(gamma_)*std::cos(alpha_);
00136 
00137       R(2,0)=-std::sin(beta_);
00138       R(2,1)=std::cos(beta_)*std::sin(gamma_);
00139       R(2,2)=std::cos(beta_)*std::cos(gamma_);
00140 
00141       return R;
00142     }
00143 
00144     inline operator Matrix() const { return toRotMat(); }
00145 
00146     inline EulerAngles operator-(const EulerAngles &o) const {
00147       return EulerAngles(alpha_-o.alpha_, beta_-o.beta_, gamma_-o.gamma_);
00148     }
00149 
00150     inline EulerAngles operator+(const EulerAngles &o) const {
00151       return EulerAngles(alpha_+o.alpha_, beta_+o.beta_, gamma_+o.gamma_);
00152     }
00153 
00154     inline TYPE norm() const
00155     {
00156       return std::sqrt( alpha_*alpha_ + beta_*beta_ + gamma_*gamma_ );
00157     }
00158 
00159     inline Vector getVector() const {
00160       Vector v;
00161       v(0) = alpha_;
00162       v(1) = beta_;
00163       v(2) = gamma_;
00164       return v;
00165     }
00166 
00167     template<typename TTYPE>
00168     friend EulerAngles<TTYPE> operator*(const TTYPE m, const EulerAngles<TTYPE> &o);
00169 
00170     template<typename TTYPE>
00171     friend std::ostream &operator<<(std::ostream &os, const EulerAngles<TTYPE> &o);
00172   };
00173 
00174   template<typename TYPE>
00175   inline EulerAngles<TYPE> operator*(const TYPE m, const EulerAngles<TYPE> &o) {
00176     return EulerAngles<TYPE>(m*o.alpha_, m*o.beta_, m*o.gamma_);
00177   }
00178 
00179   template<typename TTYPE>
00180   inline std::ostream &operator<<(std::ostream &os, const EulerAngles<TTYPE> &o) {
00181     os<<"alpha: "<<o.alpha_<<"\n";
00182     os<<"beta:  "<<o.beta_<<"\n";
00183     os<<"gamma: "<<o.gamma_<<"\n";
00184     return os;
00185   }
00186 
00187   typedef EulerAngles<float> EulerAnglesf;
00188   typedef EulerAngles<double> EulerAnglesd;
00189 }
00190 
00191 
00192 #endif /* EULER_H_ */


cob_3d_mapping_slam
Author(s): Joshua Hampp
autogenerated on Wed Aug 26 2015 11:04:50