旋轉矩陣、旋轉向量（軸角）、四元數、歐拉角之間相互轉換的代碼實現(利用Eigen實現)

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
// Eigen 幾何模塊
#include <Eigen/Geometry>


int main ( int argc, char** argv )
{

    //旋轉矩陣R
    //3X3的旋轉矩陣可以用Matrix3d，也Matrix3f
    Eigen::Matrix3d R=Eigen::Matrix3d::Identity();//將其賦值為單位矩陣
//    Eigen::Matrix3d R;
//    R.setIdentity();  //也是將旋轉矩陣賦值為單位矩陣，與上面效果一樣
    cout<<"R= \n"<<R<<endl;

    //旋轉向量v（軸角）
    //旋轉向量使用AngleAxisd(f),底層不直接是Matrix，但因為重載了運算符，運算可以當作矩陣
    Eigen::AngleAxisd v(M_PI/4,Eigen::Vector3d(0,0,1));//沿z軸旋轉了45度
    cout << "rotation vector: Angle is: " << v.angle() * (180 / M_PI)<<endl//旋轉角
         << "  Axis is: " << v.axis().transpose() << endl<<endl;//旋轉軸
    //將旋轉向量轉換為旋轉矩陣
    //方式一：用matrix()
//    R=v.matrix();
    //方式二：用toRotationMatrix()
    R=v.toRotationMatrix();
    cout<<"R=\n"<<R<<endl<<endl;
    //將旋轉向量轉化為四元數q
    Eigen::Quaterniond q = Eigen::Quaterniond(v);
    cout<<"q=\n"<<q.coeffs()<<endl<<endl;//coeffs的順序:(x,y,z,w)
    cout<<"q=\n"<<q.x()<<endl<<q.y()<<endl<<q.z()<<endl<<q.w()<<endl<<endl;//四元數的另一種輸出方式


    //將旋轉矩陣轉化為四元數
    q = Eigen::Quaterniond(R);
     cout<<"q=\n"<<q.coeffs()<<endl<<endl;
    //將旋轉矩陣轉化為歐拉角
     Eigen::Vector3d euler_angles=R.eulerAngles(2,1,0);
     cout<<"yaw(Z) pitch(Y) roll(X)=\n"<<euler_angles.transpose()<<endl<<endl;


     //將四元數轉化為旋轉矩陣
     R=Eigen::Matrix3d(q);
     cout<<"R=\n"<<R<<endl<<endl;

     //將四元數轉化為旋轉向量
     v=Eigen::AngleAxisd(q);
     cout<<v.matrix()<<endl<<endl;
        return 0;
}

運行結果：R=
1 0 0
0 1 0
0 0 1
rotation vector: Angle is: 45
  Axis is: 0 0 1

R=
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1

q=
       0
       0
0.382683
 0.92388

q=
0
0
0.382683
0.92388

q=
       0
       0
0.382683
 0.92388

yaw(Z) pitch(Y) roll(X)=
0.785398       -0        0

R=
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1

 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1