【四元素 欧拉角 旋转矩阵 旋转向量】四种表示方法间的转换关系

/*
* create 2021/12/30
* 四元素 欧拉角 旋转矩阵 旋转向量之间的转换
* 其中四元素需要单独读出 x y z w 输出，其他都可以直接cout
*/

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
#define PI (3.1415926535897932346f)
 
int main(int argc, char **argv) 
{
    // 定义一个四元素
    Eigen::Quaterniond q(0.8, 0.6, 0.0, 0.0);

    // 定义一个欧拉角表示RPY
    Eigen::Vector3d eulerAngle(1,0, 0.0, 0.0);

    // 定义一个旋转矩阵R
    Eigen::Matrix3d R; 
    R << 0.707107, -0.707107, 0, 0.707107, 0.707107, 0, 0, 0, 1;
    
    // 定义一个旋转向量(旋转角度和旋转轴)
    Eigen::AngleAxisd rotation_vector (M_PI/4, Eigen::Vector3d(0, 0, 1)); 


    //【四元素】==========================================
    //--1. 四元素转欧拉角
    Eigen::Vector3d eulerAngle1;
    eulerAngle1 = q.matrix().eulerAngles(2,1,0);
    //--2. 四元素转旋转矩阵
    Eigen::Matrix3d R1;
    R1 = q.matrix();
    //--3. 四元素转旋转向量
    Eigen::AngleAxisd rotation_vector1(q);


    //【欧拉角】==========================================
    //--1. 欧拉角转四元素
    Eigen::Quaterniond q1;
    q1 = Eigen::AngleAxisd(eulerAngle[0], Eigen::Vector3d::UnitZ()) * 
         Eigen::AngleAxisd(eulerAngle[1], Eigen::Vector3d::UnitY()) * 
         Eigen::AngleAxisd(eulerAngle[2], Eigen::Vector3d::UnitX());
    //--2. 欧拉角转旋转矩阵
    Eigen::Matrix3d R1;
    R1 = Eigen::AngleAxisd(eulerAngle[0], Eigen::Vector3d::UnitZ()) * 
         Eigen::AngleAxisd(eulerAngle[1], Eigen::Vector3d::UnitY()) * 
         Eigen::AngleAxisd(eulerAngle[2], Eigen::Vector3d::UnitX());  
    //--3. 欧拉角转旋转向量
    Eigen::AngleAxisd rotation_vector2;
    rotation_vector2 = Eigen::AngleAxisd(eulerAngle[0], Eigen::Vector3d::UnitZ()) * 
                       Eigen::AngleAxisd(eulerAngle[1], Eigen::Vector3d::UnitY()) * 
                       Eigen::AngleAxisd(eulerAngle[2], Eigen::Vector3d::UnitX());  


    //【旋转矩阵】==========================================
    //--1. 旋转矩阵转欧拉角
    Eigen::Vector3d  eulerAngle2;
    eulerAngle2 = R.eulerAngles(2,1,0);
    //--2. 旋转矩阵转四元素
    // 直接赋值操作
    Eigen::Quaterniond q2(R);
    //--3. 旋转矩阵转旋转向量
    Eigen::AngleAxisd rotation_vector3;
    rotation_vector3.fromRotationMatrix(R);


    //【旋转向量】==========================================
    //--1. 旋转向量转欧拉角
    Eigen::Vector3d  eulerAngle3;
    eulerAngle2 = rotation_vector.matrix().eulerAngles(2,1,0);
    //--2. 旋转向量转四元素
    // 直接赋值操作
    Eigen::Quaterniond q3(rotation_vector);
    //--3. 旋转向量转旋转矩阵
    Eigen::Matrix3d R3;
    R3 = rotation_vector.matrix();
    R3 = rotation_vector.toRotationMatrix();

    return 0;
}