旋转矩阵转欧拉角（二自由度约束）

在我的应用场景中有一个角度始终为0，添加这个约束后就不用考虑欧拉角奇异性问题。借此机会自己推导了一下公式，梳理一下欧拉角和旋转矩阵之间的变换关系。

1 关于欧拉角

参考文章：

1. 欧拉角与旋转矩阵的转换关系
2. 三维旋转：欧拉角、四元数、旋转矩阵、轴角之间的转换
3. 欧拉角与旋转矩阵之间的转化公式及原理
4. 无人机飞控算法-姿态估计-欧拉角-旋转矩阵-四元数

需要注意的是，由于欧拉角和旋转矩阵之间的转换关系跟很多因素有关，如各轴旋转角定义、欧拉角顺规、手性定义、参考坐标系等，因此讨论之前需要先明确定义。

本文讨论的内容约束如下：右手系、ZYX顺规（Z-Yaw，X-Roll，Y-Pitch）、内在旋转、主动旋转，也即参考文章【4】中的NED_Z-Y-X_Euler_Angles。

2 转换公式推导

2.1 由欧拉角构造旋转矩阵

欧拉角构造旋转矩阵直接将三个基础旋转矩阵按顺序相乘即可：
$$\begin{gathered} R（Z_1{Y}_2{X}_3）=\left[\begin{array}{ccc}{c}_1& -s_1& 0\\ s_1& c_1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{c}_2& 0& s_2\\ 0& 1& 0\\ -s_2& 0& c_2\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& c_3& -s_3\\ 0& s_3& c_3\end{array}\right] \\ =\left[\begin{array}{ccc}{c}_1{c}_2& c_1{s}_2{s}_3-c_3{s}_1& s_1{s}_3+c_1{c}_3{s}_2\\ c_2{s}_1& c_1{c}_3+s_1{s}_2{s}_3& c_3{s}_1{s}_2-c_1{s}_3\\ -s_2& c_2{s}_3& c_2{c}_3\end{array}\right] \end{gathered}$$

2.2 由旋转矩阵推算欧拉角

2.2.1 一般情况

从上面构造出的旋转矩阵可以很容易地推算出欧拉角：
Y轴俯仰角(pitch):
$$p=arcsin(-R_{3,1})$$
X轴滚转角(roll):
$$r=atan2(R_{3,2}，R_{3,3})$$
Z轴偏航角(yaw):
$$y=atan2(R_{2,1}，R_{1,1})$$
其中

当俯仰角 $p=\pm 90^{。}$时 $c_2=0,s_2=\pm 1$，即$R_{1,1}=0，R_{2,1}=0,R_{3,2}=0,R_{3,3}=0$，旋转矩阵退化成如下形式：
$$R=\left[\begin{array}{ccc}0& \pm c_1{s}_3-c_3{s}_1& s_1{s}_3\pm c_1{c}_3\\ 0& c_1{c}_3\pm s_1{s}_3& \pm c_3{s}_1-c_1{s}_3\\ \pm 1& 0& 0\end{array}\right]$$
此时 $r$ 和 $y$ 的计算公式中 $arctan2()$ 就失效了，即出现了Gimbal Lock。
要处理这种情况，根据情况给 $r$ 或 $y$ 其中一个赋值，然后再根据退化后的旋转矩阵计算另一个角度即可。

2.2.2 约束滚转角

在我的应用场景中，滚转角 $r$ 始终为0，因此欧拉角退化为两个自由度，此时， $s_3=0,c_3=1$，旋转矩阵退化为：
$$R=\left[\begin{array}{ccc}{c}_1{c}_2& -s_1& c_1{s}_2\\ c_2{s}_1& c_1& s_1{s}_2\\ -s_2& 0& c_2\end{array}\right]$$

X轴滚转角(roll):
$$r=0$$
Y轴俯仰角(pitch):
$$p=atan2(-R_{3,1}，R_{3,3})$$
Z轴偏航角(yaw):
$$y=atan2(-R_{1,2}，R_{2,2})$$

3 转换代码（C++)

参考文章：Rotation Matrix To Euler Angles或旋转矩阵与欧拉角互转

3.1 欧拉角–>旋转矩阵

// Calculates rotation matrix given euler angles.
Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
    // Calculate rotation about x axis
    Mat R_x = (Mat_<double>(3,3) <<
               1,       0,              0,
               0,       cos(theta[0]),   -sin(theta[0]),
               0,       sin(theta[0]),   cos(theta[0])
               );
     
    // Calculate rotation about y axis
    Mat R_y = (Mat_<double>(3,3) <<
               cos(theta[1]),    0,      sin(theta[1]),
               0,               1,      0,
               -sin(theta[1]),   0,      cos(theta[1])
               );
     
    // Calculate rotation about z axis
    Mat R_z = (Mat_<double>(3,3) <<
               cos(theta[2]),    -sin(theta[2]),      0,
               sin(theta[2]),    cos(theta[2]),       0,
               0,               0,                  1);
     
    // Combined rotation matrix
    Mat R = R_z * R_y * R_x;
     
    return R;
}

3.2 旋转矩阵–>欧拉角

3.2.1 一般情况

// Checks if a matrix is a valid rotation matrix.
bool isRotationMatrix(Mat &R)
{
    Mat Rt;
    transpose(R, Rt);
    Mat shouldBeIdentity = Rt * R;
    Mat I = Mat::eye(3,3, shouldBeIdentity.type());
     
    return  norm(I, shouldBeIdentity) < 1e-6;
}
 
// Calculates rotation matrix to euler angles
// The result is the same as MATLAB except the order
// of the euler angles ( x and z are swapped ).
Vec3f rotationMatrixToEulerAngles(Mat &R)
{
 
    assert(isRotationMatrix(R));
     
    float sy = sqrt(R.at<double>(0,0) * R.at<double>(0,0) +  R.at<double>(1,0) * R.at<double>(1,0) );
 
    bool singular = sy < 1e-6; // If
 
    float x, y, z;
    if (!singular)
    {
        x = atan2(R.at<double>(2,1) , R.at<double>(2,2));
        y = atan2(-R.at<double>(2,0), sy);
        z = atan2(R.at<double>(1,0), R.at<double>(0,0));
    }
    else//由于欧拉角存在奇异性，当pitch为90°时会出现解不稳定的情况因此需要进行特殊处理
    {
        x = atan2(-R.at<double>(1,2), R.at<double>(1,1));
        y = atan2(-R.at<double>(2,0), sy);
        z = 0;
    }
    return Vec3f(x, y, z);
             
}

3.2.2 约束滚转自由度

// Checks if a matrix is a valid rotation matrix.
bool isRotationMatrix(Mat &R)
{
    Mat Rt;
    transpose(R, Rt);
    Mat shouldBeIdentity = Rt * R;
    Mat I = Mat::eye(3,3, shouldBeIdentity.type());
     
    return  norm(I, shouldBeIdentity) < 1e-6;
}
 
Vec3f rotationMatrixToEulerAngles_RollEquel0(Mat &R)
{
    assert(isRotationMatrix(R));
    double x,y,z;
    x = 0;
    y = atan2(-R.at<double>(2,0), R.at<double>(2,2));
    z = atan2(-R.at<double>(0,1), R.at<double>(1,1));
    return Vec3f(x, y, z);      
}