更多转换见：
基于eigen实现
基于python实现
原理推导
这里就记录下顺序：

1 内旋 外旋

• 每次旋转是绕固定轴（比如世界坐标系）旋转，称为外旋。
• 每次旋转是绕自身旋转之后的轴旋转，称为内旋。目前我们用的是内旋

按照内旋方式，Z-Y-X旋转顺序（指先绕自身轴Z，再绕自身轴Y，最后绕自身轴X），可得旋转矩阵（内旋是右乘）：
$$R=R_z{R}_y{R}_x$$
按照外旋方式，X-Y-Z旋转顺序（指先绕固定轴X，再绕固定轴Y，最后绕固定轴Z），可得旋转矩阵（外旋是左乘）：
$$R=R_z{R}_y{R}_x$$

说明：ZYX顺序的内旋等价于XYZ顺序的外旋

2 旋转顺序与代码：

下面例子都是zyx旋转顺序，即按照内旋的Z-Y-X(yaw-pitch-roll)或外旋的X-Y-Z(roll-pitch-yaw)，返回值都是以[yaw, pitch, roll]存储

c++版本：

// 转四元数(这个公式乘法就很明了，内旋右乘顺序是ZYX，看成外旋左乘就是XYZ，他们等价)
Eigen::Quaternionf q_;
q_ = Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()) * 
     Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) * 
     Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX());

// 转欧拉角
// euler_angles存储顺序是：yaw(z) pitch(y) roll(x); 2，1，0代表zyx
Eigen::Matrix3d rotation_matrix
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); 

Eigen的角度范围有点问题，

// 实验室自己的代码，返回的是[roll pitch yaw]， 也是ZYX顺序
void get_euler_from_R(Vector<3> &e, const Matrix<3,3> &R) {
   float phi = atan2(R(2, 1), R(2, 2)); // roll
   float theta = asin(-R(2, 0));		// pitch
   float psi = atan2(R(1, 0), R(0, 0));	// yaw
   float pi = M_PI;

   if (fabs(theta - pi/2.0f) < 1.0e-3) {
       phi = 0.0f;
       psi = atan2(R(1, 2), R(0, 2));
   } else if (fabs(theta + pi/2.0f) < 1.0e-3) {
       phi = 0.0f;
       psi = atan2(-R(1, 2), -R(0, 2));
   }
   e(0) = phi;
   e(1) = theta;
   e(2) = psi;
}

推导：假设欧拉角yaw、pitch、roll的角度为alpha, beta, gamma，则对应先yaw, 后ptich，最后roll的内旋转（zyx）可以计算如下：

python版本：

import numpy as np
import math
from scipy.spatial.transform import Rotation as R

# euler_angles存储顺序是：[yaw(z) pitch(y) roll(x)]；大写表示内旋，小写外旋
# 转欧拉角
Rotation = R.from_quat([x,y,z,w]) # Rotation只是一个旋转，不是矩阵也不是四元数
euler_angles = Rotation.as_euler('ZYX', degrees=False)
# 转四元数
Rotation = R.from_euler('ZYX', [yaw, pitch, roll], degrees=False)
rotation_matrix = Rotation.as_matrix()
quaternion = Rotation.as_quat()