python旋转矩阵与欧拉角互转

 学习链接：Rotation Matrix To Euler Angles | LearnOpenCV #

                       旋转矩阵与欧拉角互转

先上代码说明如何实现python旋转矩阵与欧拉角互转：

欧拉角 ——> 旋转矩阵

import math
import numpy as np

def eulerAngles2rotationMat(theta, format='degree'):
    """
    Calculates Rotation Matrix given euler angles.
    :param theta: 1-by-3 list [rx, ry, rz] angle in degree
    :return:
    RPY角，是ZYX欧拉角，依次 绕定轴XYZ转动[rx, ry, rz]
    """
    if format is 'degree':
        theta = [i * math.pi / 180.0 for i in 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

print(eulerAngles2rotationMat([120,0,0]))

旋转矩阵——> 欧拉角

# 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])
 
 

知识拓展：什么是欧拉角？

        用一句话说，欧拉角就是物体绕坐标系三个坐标轴(x,y,z轴）的旋转角度。

        在这里，坐标系可以是世界坐标系，也可以是物体坐标系，旋转顺序也是任意的，可以是xyz,xzy,yxz,zxy,yzx,zyx中的任何一种，甚至可以是xyx,xyy,xzz,zxz等等等等。。。。。。

所以说欧拉角多种多样。欧拉角可分为两种情况：

1，静态：即绕世界坐标系三个轴的旋转，由于物体旋转过程中坐标轴保持静止，所以称为静态。

2，动态：即绕物体坐标系三个轴的旋转，由于物体旋转过程中坐标轴随着物体做相同的转动，所以称为动态。

对于分别绕三个坐标轴旋转的情况，下述定理成立：

物体的任何一种旋转都可分解为分别绕三个轴的旋转，但分解方式不唯一。如：

假设绕y轴旋转为heading，绕x轴旋转为pitch，绕z轴旋转为bank，则先heading45°再pitch90°等价于先pitch90°再bank45°。

前面曾提到过，heading-pitch-bank系统不是惟一的欧拉角系统，绕任意三个互相垂直轴的任意旋转序列都能定义一个方位。所以，多种选择导致了欧拉角约定的多样性：
     1）heading-pitch-bank系统有两个名称，当然，不同的名字并不代表不同的约定，这其实并不重要，一组常用的术语是roll-pitch-yaw，其中的roll对应与bank，yaw对应于heading，它定义了从物体坐标系到惯性坐标系的旋转顺序
     2）任意三个轴都能作为旋转轴，不一定必须是笛卡尔轴，但是用笛卡尔轴最有意义
     3）也可以选用右手坐标规则
     4）旋转可以以不同的顺序进行

3，优点：1）容易使用；2）表达简洁；3）任意三个角都是合法的

4，缺点：1）给定方位的表达方式不唯一；2）两个角度间求插值非常困难

采用限制欧拉角的方法来避免以上问题的出现：heading限制在+-180，pitch为+-90。以上为欧拉角的定义。旋转的方法如下：

从欧拉角矢量转换很容易，困难的部分是转换回来。XNA提供了一个方法可以创建旋转矩阵，但它并没有提供转换回来的方法，因此我们将不得不自己实现。

首先，我们转换为旋转矩阵，Matrix.CreateFromYawPitchRoll()方法可以做到这一点。如果这里使用欧拉角，我们需要以以下顺序提供坐标：

Yaw（偏航）：欧拉角向量的y轴
Pitch（俯仰）：欧拉角向量的x轴
Roll（翻滚）： 欧拉角向量的z轴
想象一下飞机，yaw指水平方向的机头指向，它绕y轴旋转。Pitch指与水平方向的夹角，绕x轴旋转。Roll指飞机的翻滚，绕z轴旋转。

除欧拉角以外，常用的还有四元素法和旋转矩阵法。简而言之，三种方法的特点如下：

1）欧拉角最直观、最容易理解、存储空间少，但是欧拉角存在万向节死锁现象、插值速度不均匀等缺点，而且不可以在计算机中直接运算；

2）四元素不存在万向节死锁问题、利用球面插值可以获得均匀的转速、存储空间也较少，但是不好理解、不直观；

3）旋转矩阵法是最便于计算机处理的，但不可以直接插值、冗余信息多、费存储空间，同样不直观。所以，在机器人学中，一般人机交互端会用欧拉角，插值等用四元素，正逆运动学运算中用矩阵表示法。

拓展知识转载链接：什么是欧拉角/姿态角？