分享

eigen转换四元数和欧拉角

 海漩涡 2017-10-09
#include <iostream>  
#include <stdio.h>  
#include <Eigen/Core>  
#include <Eigen/Geometry>  

using namespace std;

#define PI (3.1415926535897932346f)  

::Eigen::Quaterniond  Euler_to_Quaterniond(double yaw, double pitching, double droll)
{
::Eigen::Vector3d ea0(yaw,pitching,droll);  
    ::Eigen::Matrix3d R;  
    R = ::Eigen::AngleAxisd(ea0[0], ::Eigen::Vector3d::UnitZ())  
        * ::Eigen::AngleAxisd(ea0[1], ::Eigen::Vector3d::UnitY())  
        * ::Eigen::AngleAxisd(ea0[2], ::Eigen::Vector3d::UnitX());  
  
    //cout << R << endl << endl;  
  
    //RotationMatrix to Quaterniond  
    ::Eigen::Quaterniond q;   
    q = R;      
    cout <<"x:"<<q.x()<<"\t"<<"y:"<<q.y()<<"\t"<<"z:"<<q.z()<<"\t"<<"w:"<<q.w()<<endl;
return q;
}

::Eigen::Vector3d Quaterniond_to_Euler(double x, double y, double z, double w)
{
::Eigen::Quaterniond q(w,x,y,z);
::Eigen::Matrix3d Rx = q.toRotationMatrix();  
::Eigen::Vector3d ea1 = Rx.eulerAngles(2,1,0);       
    cout <<ea1[0]<<" "<<ea1[1]<<" "<<ea1[2]<<endl;
return ea1;
}


int main(int argc, char **argv)   
{    
    double yaw = PI/3,pitching = PI/4,droll = PI/6;  
//double yaw = PI,pitching = PI,droll = PI;

printf("T yaw[%f] pitching[%f] droll[%f]\n",yaw,pitching,droll);

::Eigen::Quaterniond q = Euler_to_Quaterniond(yaw, pitching, droll);

    ::Eigen::Vector3d    V = Quaterniond_to_Euler(q.x(), q.y(), q.z(), q.w());
//cout << V/PI*180 << endl;
#if 0      
printf("T yaw[%f] pitch[%f]  droll[%f]\n", yaw, pitching, droll);
      //EulerAngles to RotationMatrix  
    ::Eigen::Vector3d ea0(yaw,pitching,droll);  
    ::Eigen::Matrix3d R;  
    R = ::Eigen::AngleAxisd(ea0[0], ::Eigen::Vector3d::UnitZ())  
        * ::Eigen::AngleAxisd(ea0[1], ::Eigen::Vector3d::UnitY())  
        * ::Eigen::AngleAxisd(ea0[2], ::Eigen::Vector3d::UnitX());  
  
    cout << R << endl << endl;  
  
    //RotationMatrix to Quaterniond  
    ::Eigen::Quaterniond q;   
    q = R;      
    cout << q.x() << endl << endl;  
    cout << q.y() << endl << endl;  
    cout << q.z() << endl << endl;  
    cout << q.w() << endl << endl;  

::Eigen::Quaterniond q1(q.w(),q.x(),q.y(),q.z()); 
 
    //Quaterniond to RotationMatrix  
    ::Eigen::Matrix3d Rx = q1.toRotationMatrix();  
    cout << Rx << endl << endl;  
      
      
    //RotationMatrix to EulerAngles  
    ::Eigen::Vector3d ea1 = Rx.eulerAngles(2,1,0);       
    //cout << ea1/PI*180 << endl << endl;  
    cout << ea1 << endl << endl;  
      
    std::cin.ignore();  
#endif
    return 0;  
}  

    本站是提供个人知识管理的网络存储空间,所有内容均由用户发布,不代表本站观点。请注意甄别内容中的联系方式、诱导购买等信息,谨防诈骗。如发现有害或侵权内容,请点击一键举报。
    转藏 分享 献花(0

    0条评论

    发表

    请遵守用户 评论公约

    类似文章 更多