欧拉角顺序与转换

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

运算是左乘，下面方式记作Z-Y-X顺序，先绕x轴roll,再绕y轴pitch,最后绕z轴yaw，即为RPY

c++版本：

// 转欧拉角
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)
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，则对应先roll，后ptich，最后yaw的旋转可以计算如下：

python版本：

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

# `Z-Y-X`顺序，先绕x轴roll,再绕y轴pitch,最后绕z轴yaw
# 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('zxy', [yaw, pitch, roll], degrees=False)
rotation_matrix = Rotation.as_matrix()
quaternion = Rotation.as_quat()