旋转矩阵与欧拉角的相互转换及代码

这篇博客将会分享旋转矩阵和欧拉角的相互转换。

三维旋转矩阵有三个自由度，旋转能够使用多种方法表示（旋转矩阵，欧拉角，四元数，轴角，李群与李代数），比如一个3x3的矩阵，比如四元数，甚至可以将旋转表示为三个数字，即绕三个轴x,y,z的旋转角度。在原始的欧拉角公式中，旋转是通过围绕Z轴，Y轴，X轴分别旋转得到的。它分别对应于偏航，俯仰和横滚。

当定义一个旋转时，可能还会引起歧义，比如对于一个给定的点(x,y,z),可以把这个点视为行向量（x,y,z）或者列向量量$[x,y,z{]}^T$，如果使用行向量，那么就必须后乘旋转矩阵（旋转矩阵在后），如果使用列向量，就要左乘旋转矩阵（旋转矩阵在前）。

在MATLAB中，rotm2euler.m能够实现旋转矩阵到欧拉角的转换。下文中将要叙述的转换代码参考于MATLAB中的rotm2euler.m实现。不同于MATLAB实现的是，它的旋转顺序是Z-Y-X，而下面的实现是X-Y-Z。

在计算将旋转矩阵转换成欧拉角之前，先介绍一下欧拉角转换为旋转矩阵的方法。

欧拉角转换为旋转矩阵

假如已知旋转角，绕着X-Y-Z三个轴的角度分别为 ${\theta }_x{\theta }_y{\theta }_z$那么三个旋转矩阵可以表示如下：

如果旋转顺序为Z-Y-X的顺序，那么旋转矩阵可表示如下：

关于上面的旋转矩阵，以z轴旋转为例：
第一种物理意义，坐标系的旋转。 其应用场景有SLAM，机械臂运动学等。

如上图所示， P点不变，坐标系 O-XYZ旋转 α ,得到新的坐标系O-X’Y’Z’，在新坐标系下，P点的坐标变为P’ ，则有：

第二种物理意义，向量的旋转，其应用场景有机器人的姿态估计等。

如上图所示，坐标系 O-XYZ不变， P’点旋转α ,得到新的点P （我也想反过来，但用的教材上的图，没办法），则有：

对应于上述旋转顺序的C++代码如下：

C++

// 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;
 
}

Python

# Calculates Rotation Matrix given euler angles.
def eulerAnglesToRotationMatrix(theta) :
     
    R_x = np.array([[1,                  0,                   0],
                    [0, math.cos(theta[0]), -math.sin(theta[0])],
                    [0,  math.sin(theta[0]), math.cos(theta[0]) ]])
                           
    R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1])],
                    [0,                  1,                  0],
                    [-math.sin(theta[1]), 0, math.cos(theta[1])]])
                 
    R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
                    [math.sin(theta[2]), math.cos(theta[2]),  0],
                    [0,                  0,                   1]])
                     
                     
    R = np.dot(R_z, np.dot( R_y, R_x ))
 
    return R

在OpenCV中将旋转矩阵转换为欧拉角

将旋转矩阵转换为欧拉角就不是那么容易，不同的旋转顺序对应着不同的旋转角度。使用上面的代码，即使欧拉角看起来非常不同，您也可以验证与欧拉角[0.1920、2.3736、1.1170]（或[[11，136，64]度）和[-2.9496，0.7679，-2.0246]（或[-169，44，-116]度）相对应的旋转矩阵实际上是相同的。下面的代码显示了在给定旋转矩阵的情况下找到欧拉角的方法。以下代码的输出应与MATLAB的rotm2euler的输出完全匹配，但x和z的顺序会互换。

C++

// 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
    {
        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);   
}

Python

# Checks if a matrix is a valid rotation matrix.
def isRotationMatrix(R) :
    Rt = np.transpose(R)
    shouldBeIdentity = np.dot(Rt, R)
    I = np.identity(3, dtype = R.dtype)
    n = np.linalg.norm(I - shouldBeIdentity)
    return n < 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 ).
def rotationMatrixToEulerAngles(R) :

    assert(isRotationMatrix(R))
    
    sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])
    
    singular = sy < 1e-6

    if  not singular :
        x = math.atan2(R[2,1] , R[2,2])
        y = math.atan2(-R[2,0], sy)
        z = math.atan2(R[1,0], R[0,0])
    else :
        x = math.atan2(-R[1,2], R[1,1])
        y = math.atan2(-R[2,0], sy)
        z = 0

    return np.array([x, y, z])