旋转矩阵、旋转向量（轴角）、四元数、欧拉角之间相互转换的代码实现(利用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= 
"<<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=
"<<R<<endl<<endl;
    //将旋转向量转化为四元数q
    Eigen::Quaterniond q = Eigen::Quaterniond(v);
    cout<<"q=
"<<q.coeffs()<<endl<<endl;//coeffs的顺序:(x,y,z,w)
    cout<<"q=
"<<q.x()<<endl<<q.y()<<endl<<q.z()<<endl<<q.w()<<endl<<endl;//四元数的另一种输出方式


    //将旋转矩阵转化为四元数
    q = Eigen::Quaterniond(R);
     cout<<"q=
"<<q.coeffs()<<endl<<endl;
    //将旋转矩阵转化为欧拉角
     Eigen::Vector3d euler_angles=R.eulerAngles(2,1,0);
     cout<<"yaw(Z) pitch(Y) roll(X)=
"<<euler_angles.transpose()<<endl<<endl;


     //将四元数转化为旋转矩阵
     R=Eigen::Matrix3d(q);
     cout<<"R=
"<<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