旋转矩阵转欧拉角

方法一：Eigen库转欧拉角

此方法有坑，慎用
以下两种方法转换后的yaw,pitch,roll角度不同
（1）eulerAngles(0, 1, 2)
rotation是3*3的旋转矩阵

Eigen::Vector3d euler = rotation.eulerAngles(0, 1, 2);
double roll= euler[0] * 180.0 / M_PI；
double pitch = euler[1] * 180.0 / M_PI；
double yaw = euler[2] * 180.0 / M_PI;

（2）eulerAngles(2, 1, 0)

Eigen::Vector3d euler = rotation.eulerAngles(2, 1, 0);
double yaw= euler[0] * 180.0 / M_PI；
double pitch = euler[1] * 180.0 / M_PI；
double roll = euler[2] * 180.0 / M_PI;

如果发现转换后的角度与期望的差180度，建议采用方法二。

方法二

#include <cmath>
 
struct Quaternion {
    double w, x, y, z;
};
 
struct EulerAngles {
    double roll, pitch, yaw;
};
 
EulerAngles ToEulerAngles(Quaternion 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;
}

int main(){
	Quaternion qua_euler;//此处需自己赋值
	EulerAngles euler;
    euler = ToEulerAngles(qua_euler);
	double yaw = euler.yaw * 180.0 / M_PI;
	double pitch = euler.pitch * 180.0 / M_PI;
	double roll = euler.roll * 180.0 / M_PI;
    return 0;
 }

四元数初始化
方式一：4个标量
Quaterniond q1(1, 2, 3, 4); // 第一种方式 实部为1 ，虚部234

方式二：Vector4d
Quaterniond q2(Vector4d(1, 2, 3, 4)); // 第二种方式 实部为4 ，虚部123

方式三：数组
Quaterniond q2(tmp_q); // 第三种方式，double tmp_q[4]; 实部为4 ，虚部123