(十四)pcl-eigen篇-四元数转欧拉角之优化

使用qua.toRotationMatrix().eulerAngles(2, 1, 0)来得到的欧拉角可能不是我们所希望的，因为4元数可以对应俩个欧拉角，以下是wiki百科中所给出的优化方法。

#include <iostream>
#include <Eigen/Dense>
#include <pcl/pcl_macros.h>
using namespace Eigen;
using namespace std;

struct EulerAngles {
    double roll, pitch, yaw;
};
EulerAngles ToEulerAngles(Quaterniond q);

int main()
{
    Eigen::Matrix<float, 4, 4> transformation = Eigen::Matrix<float, 4, 4>::Identity();
    Eigen::Quaterniond quaternion;

    float yaw = M_PI / 2; // 弧度角
    //float pitch = M_PI / 4; // 弧度角
    //float roll = M_PI / 4; // 弧度角
    float pitch = -M_PI; // 弧度角
    float roll = -M_PI / 2; // 弧度角
    //float pitch = M_PI / 2; // 弧度角
    //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;

    quaternion.w() = 0.00392036;
    quaternion.x() = -0.00511095;
    quaternion.y() = -0.613622;
    quaternion.z() = 0.789573;


    cout << "4元数：" << endl << quaternion.matrix() << endl;
    //Eigen::Isometry3d iso = Eigen::Translation3d(1, 2, 3) * quaternion;
    Eigen::Isometry3d iso = Eigen::Translation3d(2,1,0) * quaternion;
    Eigen::Matrix4d res = iso.matrix();
    cout << "等距映射：" << endl << res << endl;

    // 从旋转矩阵构造四元数
    Eigen::Matrix<double, 3, 3> rot;
    rot = quaternion.matrix();
    Eigen::Quaterniond qua(rot);


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

    //=======================研究四元数与欧拉角转换是超过2*M_PI=============================//
    //2,保证出来的欧拉角不会超过2PI
    EulerAngles a = ToEulerAngles(qua);
    cout << "优化后的欧拉角yaw:" << endl << a.yaw << endl;  //弧度单位
    cout << "优化后的欧拉角pitch:" << endl << a.pitch << endl;  //弧度单位
    cout << "优化后的欧拉角roll:" << endl << a.roll << endl;  //弧度单位
    

    return 0;
}


//https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
EulerAngles ToEulerAngles(Quaterniond q) {
    EulerAngles angles;

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w() * q.x() + q.y() * q.z());
    double cosr_cosp = 1 - 2 * (q.x() * q.x() + q.y() * q.y());
    angles.roll = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2 * (q.w() * q.y() - q.z() * q.x());
    if (std::abs(sinp) >= 1)
        angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        angles.pitch = std::asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w() * q.z() + q.x() * q.y());
    double cosy_cosp = 1 - 2 * (q.y() * q.y() + q.z() * q.z());
    angles.yaw = std::atan2(siny_cosp, cosy_cosp);

    return angles;
}

输出：

4元数x：
0.5
4元数y：
-0.5
4元数z：
-0.5
4元数：
  -0.999915 8.15619e-05  -0.0128822
  0.0124632   -0.246903   -0.968959
 -0.0032597   -0.969039    0.246884
等距映射：
  -0.999915 8.15619e-05  -0.0128822           2
  0.0124632   -0.246903   -0.968959           1
 -0.0032597   -0.969039    0.246884           0
          0           0           0           1
欧拉角：
   3.12913
0.00325971
  -1.32133
优化后的欧拉角yaw:
3.12913
优化后的欧拉角pitch:
0.0032597
优化后的欧拉角roll:
-1.32133