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