Learn OpenCV----旋转矩阵转欧拉角

阅读原文

概述

本篇博客，主要给出如何将3x3的旋转矩阵转换成欧拉角的讨论和代码。
什么是旋转矩阵和欧拉角呢？

欧拉角

我们应该知道，对于一个三维空间，我们可以建立一个三维坐标轴，而这个三维坐标轴由X轴，Y轴和Z轴组成。当三维物体进行旋转时，可以看成该物体依次绕着这三个轴进行某种角度的旋转，这些角度被称为欧拉角。值得注意的是，在行业中，欧拉角饶轴旋转的循序是Z-Y-X，因为这样对应着yaw-pitch-roll。

yaw：偏航角，指物体绕着Z轴旋转
pitch：俯仰角，指物体绕着Y轴旋转
roll：滚转角，指物体绕着X轴旋转

欧拉角看起来比较简单，但是它的一个重大缺点是会出现万向锁问题：在俯仰角为$\pm 90^{°}$时，第一次旋转与第三次旋转将使用同一个轴，这使得系统丢失了一个自由度。

旋转矩阵

旋转矩阵的几何意义也很明确，把一个三维空间点(x,y,z)看为一个三维向量[x,y,z]，然后将该向量与一个矩阵相乘，得到一个变换后的三维向量。值得注意的一点是，如果你将三维空间点看成一个行向量，那旋转矩阵是在乘号后边，即VM。但是如果使用列向量表示，那旋转矩阵是在乘号前边，即MV。而这两种情况下的旋转矩阵M是不相同的(他们是彼此的转置)

将欧拉角转为旋转矩阵

考虑三维旋转最简单的方式是轴角形式。因为任何旋转都可以由旋转轴和描述旋转量的角度来定义。
由上述的讨论可知，每个轴都有其对应的旋转矩阵。下面分别给出X轴，Y轴和Z轴的旋转矩阵。



其中，${\theta }_x,{\theta }_y,{\theta }_z$对应为每个轴的旋转角度，即欧拉角。
为了得到最后的旋转矩阵$R$，我们需要依次将物体在这些矩阵上进行连续运算，因此，最后的$R$可以表示为如下的矩阵乘法。

从欧拉角转换为旋转矩阵的函数片段如下：

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

将旋转矩阵转为欧拉角

将旋转矩阵转换为欧拉角有点困难，因为在大多数情况下，解决方案不是唯一的。下面代码的输出完全与Matlab的rotm2euler函数一致。

首先，先对旋转矩阵进行检查

# 检查一个旋转矩阵是否有效
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

完整的转换代码

# 这部分的代码输出与Matlab里边的rotm2euler一致
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([z, y, x])

总结

三维旋转是一个十分常见的需求，这些代码都可以直接使用，非常的nice。如果对完整代码有需求的小伙伴，可以去原网站下载，嫌麻烦，也可以下载我添加上注释后的版本