(十三)pcl-eigen篇-四元数，欧拉角和旋转矩阵相互转换

参考文章

#include <iostream>
#include <Eigen/Dense>
#include <pcl/pcl_macros.h>
using namespace Eigen;
using namespace std;
int main()
{
    Eigen::Matrix<float, 4, 4> transformation = Eigen::Matrix<float, 4, 4>::Identity();
    Eigen::Quaterniond quaternion;

    //1,从弧度(欧拉角)转四元数
    float yaw = M_PI / 4; // 弧度角
    //float pitch = M_PI / 4; // 弧度角
    //float roll = M_PI / 4; // 弧度角
    float pitch = 0; // 弧度角
    float roll = 0; // 弧度角
    quaternion =    Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *
                    Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
                    Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
    cout << "4元数w：" << endl << quaternion.w() << endl;
    cout << "4元数x：" << endl << quaternion.x() << endl;
    cout << "4元数y：" << endl << quaternion.y() << endl;
    cout << "4元数z：" << endl << quaternion.z() << endl;
    cout << "4元数：" << endl << quaternion.matrix() << endl;
    Eigen::Isometry3d iso = Eigen::Translation3d(1,2,3) * quaternion;
    Eigen::Matrix4d res = iso.matrix();
    cout << "等距映射：" << endl << res << endl;


    // 2，从旋转矩阵构造四元数
    Eigen::Matrix<double, 3, 3> rot;
    rot = quaternion.matrix();
    Eigen::Quaterniond qua(rot);
    cout << "qua4元数w：" << endl << qua.w() << endl;
    cout << "qua4元数x：" << endl << qua.x() << endl;
    cout << "qua4元数y：" << endl << qua.y() << endl;
    cout << "qua4元数z：" << endl << qua.z() << endl;

    // 3，从四元数转换为旋转矩阵
    Eigen::Matrix<double, 3, 3> rotation = qua.toRotationMatrix();
    cout << "旋转矩阵：" << endl << rotation << endl;

    // 4，从四元数转换为欧拉角
    Eigen::Vector3d euler = qua.toRotationMatrix().eulerAngles(2, 1, 0);
    cout << "欧拉角：" << endl << euler << endl;  //弧度单位

    // 5，从欧拉角转换为旋转矩阵(先转四元数, 再转旋转矩阵)
    //Eigen::Quaterniond quaternion_1 =   Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *
    //                                    Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
    //                                    Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
    Eigen::Quaterniond quaternion_1 =   Eigen::AngleAxisd(euler(0), Eigen::Vector3d::UnitZ()) *
                                        Eigen::AngleAxisd(euler(1), Eigen::Vector3d::UnitY()) *
                                        Eigen::AngleAxisd(euler(2), Eigen::Vector3d::UnitX());
    Eigen::Matrix3d rotation_2 = quaternion_1.toRotationMatrix();
    cout << "旋转矩阵：" << endl << rotation_2 << endl;  //弧度单位
    return 0;
}

输出：

4元数w：
0.92388
4元数x：
0
4元数y：
0
4元数z：
0.382683
4元数：
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
等距映射：
 0.707107 -0.707107         0         1
 0.707107  0.707107         0         2
        0         0         1         3
        0         0         0         1
qua4元数w：
0.92388
qua4元数x：
0
qua4元数y：
0
qua4元数z：
0.382683
旋转矩阵：
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
欧拉角：
0.785398
      -0
       0
旋转矩阵：
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1