旋转矩阵转欧拉角 opencv 旋转矩阵转换为欧拉角

简 介： 对于欧拉角与旋转矩阵之间的转换公式和程序实现进行了测试。也显示了这其中的转换关系的复杂性，来自于欧拉角的方向、范围、转换顺序。这在实际应用中需要特别的关注。关键词： 欧拉角，旋转矩阵

欧拉角

目 录 Contents

基本概念

旋转顺序

旋转矩阵

由欧拉角到旋转矩阵

任意轴旋转

相互转换

转换公式

转换代码

对比代码

总 结

• 欧拉角和旋转矩阵之间的转换

  在机器人视觉应用中，经常会遇到旋转矩阵、旋转向量、四元素、欧拉角之间的相互转换。其中最容易出错的是旋转矩阵与欧拉角之间的相互转换。

▲ 图1 坐标系旋转变换

 

§01 欧拉角

---

1.1 基本概念

1.1.1 欧拉角的名称

• 欧拉角的叫法不固定，跟坐标轴的定义强相关。
• 在图1中，假设X是车头，Y是车左方，Z是车上方，那么绕X轴旋转得到的是roll，绕Y轴旋转得到的是pitch，绕Z轴得到的是yaw。
• 在图1中，假设Y是车头，X是车右方，Z是车上方，那么绕X轴旋转得到的是pitch，绕Y轴旋转得到的是roll，绕Z轴得到的是yaw。

1.1.2 欧拉角方向

• 如果是右手系，旋转轴正方向面对观察者时，逆时针方向的旋转是正、顺时针方向的旋转是负。
• 亦可这样描述：使用右手的大拇指指向旋转轴正方向，其他4个手指在握拳过程中的指向便是正方向。
• 如图1中的三次旋转都是正向旋转。

▲ 图1.1.1 三次旋转的过程

1.1.3 欧拉角范围

• 这个要具体问题具体对待。
• 假如是车体坐标系（x-前，y-左，z-上），那么roll和pitch应该定义在（-90°，+90°），yaw应该定义在（-180°，+180°）。
• 假如是飞机坐标系，那么roll、pitch和yaw都应该定义在（-180°，+180°）。
• Eigen中的默认范围roll、pitch和yaw都是（-180°，+180°）。

1.2 旋转顺序

1.2.1 旋转顺序和旋转轴

• 对于x,y,z三个轴的不同旋转顺序一共有(x-y-z,y-z-x,z-x-y,x-z-y,z-y-x,y-x-z)六种组合，在旋转相同的角度的情况下不同的旋转顺序得到的姿态是不一样的。
• 比如，先绕x轴旋转alpha，再绕y轴旋转beta；先绕y轴旋转beta，再绕x轴旋转alpha。这两种顺序得到的姿态是不一样的。### 内旋和外旋

1.2.2 内旋和外旋

• 每次旋转是绕固定轴（一个固定参考系，比如世界坐标系）旋转，称为外旋。
• 每次旋转是绕自身旋转之后的轴旋转，称为内旋。
• 下图说明了内旋和外旋的区别。

▲ 图1.2.1 内在旋转

▲ 图1.2.2 外在旋转

  按照内旋方式，Z-Y-X旋转顺序（指先绕自身轴Z，再绕自身轴Y，最后绕自身轴X），可得旋转矩阵（内旋是右乘）

$R_1 = R_z \left( \gamma \right) \cdot R_y \left( \beta \right) \cdot R_x \left( \alpha \right)$

  按照外旋方式，X-Y-Z旋转顺序（指先绕固定轴X，再绕固定轴Y，最后绕固定轴Z），可得旋转矩阵（外旋是左乘）：

$R_2 = R_z \left( \gamma \right) \cdot R_y \left( \beta \right) \cdot R_x \left( \alpha \right)$

  故R1=R2，具体不在此证明，记住即可。这个结论说明ZYX顺序的内旋等价于XYZ顺序的外旋。

 

§02 旋转矩阵

---

2.1 由欧拉角到旋转矩阵

$\alpha ,\beta ,\gamma$，则三次旋转的旋转矩阵计算方法如下：

▲ 图2.1 三个旋转矩阵

$R_x \left( \theta \right) = \begin{bmatrix} \begin{matrix} 1 & 0 & 0\\0 & {\cos \theta } & { - \sin \theta }\\0 & {\sin \theta } & {\cos \theta }\\\end{matrix} \end{bmatrix}$

$R_y \left( \theta \right) = \begin{bmatrix} \begin{matrix} {\cos \theta } & 0 & {\sin \theta }\\0 & 1 & 0\\{ - \sin \theta } & 0 & {\cos \theta }\\\end{matrix} \end{bmatrix}$

$R_z \left( \theta \right) = \begin{bmatrix} \begin{matrix} {\cos \theta } & { - \sin \theta } & 0\\{\sin \theta } & {\cos \theta } & 0\\0 & 0 & 1\\\end{matrix} \end{bmatrix}$

2.2 任意轴旋转

$U = \left( {U_x ,U_y ,U_z } \right)$，旋转$\theta$角度，那么对应的旋转矩阵为：$R_U \left( \theta \right) = I + \left( {\sin \theta } \right) \cdot S + \left( {1 - \cos \theta } \right) \cdot S^2$

$I$是单位矩阵；

$S = \begin{bmatrix} \begin{matrix} 0 & { - U_z } & {U_y }\\{U_z } & 0 & { - U_x }\\{ - U_y } & {U_x } & 0\\\end{matrix} \end{bmatrix}$

▲ 图2.2 旋转角与旋转矩阵

 

§03 相互转换

---

3.1 转换公式

▲ 图3.1.1 欧拉角与转换过程

3.1.1 从欧拉角到旋转矩阵

  根据 Euler Angle Formulas 给出了不同参数下测转换矩阵。

▲ 图3.2.1 从欧拉角到旋转矩阵

▲ 图3.2.2 欧拉角

3.2 转换代码

3.2.1 Python代码

  Rotation matrix to Euler angles Python code example

（1）旋转拒转→欧拉角

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

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

（2）欧拉角→旋转矩阵

def euler_to_rotVec(yaw, pitch, roll):
    Rmat = euler_to_rotMat(yaw, pitch, roll)

    theta = math.acos(((Rmat[0, 0] + Rmat[1, 1] + Rmat[2, 2]) - 1) / 2)
    sin_theta = math.sin(theta)
    if sin_theta == 0:
        rx, ry, rz = 0.0, 0.0, 0.0
    else:
        multi = 1 / (2 * math.sin(theta))
        rx = multi * (Rmat[2, 1] - Rmat[1, 2]) * theta
        ry = multi * (Rmat[0, 2] - Rmat[2, 0]) * theta
        rz = multi * (Rmat[1, 0] - Rmat[0, 1]) * theta
    return rx, ry, rz

def euler_to_rotMat(yaw, pitch, roll):
    Rz_yaw = np.array([
        [np.cos(yaw), -np.sin(yaw), 0],
        [np.sin(yaw),  np.cos(yaw), 0],
        [          0,            0, 1]])
    Ry_pitch = np.array([
        [ np.cos(pitch), 0, np.sin(pitch)],
        [             0, 1,             0],
        [-np.sin(pitch), 0, np.cos(pitch)]])
    Rx_roll = np.array([
        [1,            0,             0],
        [0, np.cos(roll), -np.sin(roll)],
        [0, np.sin(roll),  np.cos(roll)]])
    rotMat = np.dot(Rz_yaw, np.dot(Ry_pitch, Rx_roll))
    return rotMat

  通过测试可以知道：

• yaw： z 轴
• pitch： y 轴
• roll： x 轴

3.2.2 SciPy

  scipy.spatial.transform.Rotation.from_euler

import sys,os,math,time
import matplotlib.pyplot as plt
from numpy import *
from scipy.spatial.transform import Rotation as R

r = R.from_euler('x', 90, degrees=True)
print("r.as_quat(): {}".format(r.as_quat()))

r.as_quat(): [0.70710678 0.         0.         0.70710678]

  R产生的对象包含有以下对象：

'apply',
'as_dcm',
'as_euler',
'as_quat',
'as_rotvec',
'from_dcm',
'from_euler',
'from_quat',
'from_rotvec',
'inv',
'match_vectors',

3.3 对比代码

x = 30
y = 45
z = 60

r = R.from_euler('zyx', [-z, -y, -x], degrees=True)
print("r.as_dcm():\n{}".format(r.as_dcm()))

rm = euler_to_rotMat(z*pi/180, y*pi/180, x*pi/180)
print("rm.T:\n{}".format(rm.T))

r.as_dcm():
[[ 0.35355339  0.61237244 -0.70710678]
 [-0.5732233   0.73919892  0.35355339]
 [ 0.73919892  0.28033009  0.61237244]]
rm.T:
[[ 0.35355339  0.61237244 -0.70710678]
 [-0.5732233   0.73919892  0.35355339]
 [ 0.73919892  0.28033009  0.61237244]]

  可以看到， 在上述；定义中，关于方向的角度正负定义，在两种函数中时有区别的。输出的矩阵也呈现转置的情况。

rm = euler_to_rotMat(z*pi/180, y*pi/180, x*pi/180)
print("rm.T:\n{}".format(rm.T))

r = R.from_dcm(rm.T)
print("r.as_euler('zyx')*180/pi: {}".format(r.as_euler('zyx')*180/pi))

rm.T:
[[ 0.35355339  0.61237244 -0.70710678]
 [-0.5732233   0.73919892  0.35355339]
 [ 0.73919892  0.28033009  0.61237244]]
r.as_euler('zyx')*180/pi: [-60. -45. -30.]

 

※ 总

---

对于欧拉角与旋转矩阵之间的转换公式和程序实现进行了测试。也显示了这其中的转换关系的复杂性，来自于欧拉角的方向、范围、转换顺序。这在实际应用中需要特别的关注。

#!/usr/local/bin/python
# -*- coding: gbk -*-
#============================================================
# TEST4.PY                     -- by Dr. ZhuoQing 2021-12-31
#
# Note:
#============================================================

from headm import *                 # =
from scipy.spatial.transform import Rotation as R
import numpy as np

#------------------------------------------------------------
def euler_to_rotVec(yaw, pitch, roll):
    Rmat = euler_to_rotMat(yaw, pitch, roll)

    theta = math.acos(((Rmat[0, 0] + Rmat[1, 1] + Rmat[2, 2]) - 1) / 2)
    sin_theta = math.sin(theta)
    if sin_theta == 0:
        rx, ry, rz = 0.0, 0.0, 0.0
    else:
        multi = 1 / (2 * math.sin(theta))
        rx = multi * (Rmat[2, 1] - Rmat[1, 2]) * theta
        ry = multi * (Rmat[0, 2] - Rmat[2, 0]) * theta
        rz = multi * (Rmat[1, 0] - Rmat[0, 1]) * theta
    return rx, ry, rz

def euler_to_rotMat(yaw, pitch, roll):
    Rz_yaw = np.array([
        [np.cos(yaw), -np.sin(yaw), 0],
        [np.sin(yaw),  np.cos(yaw), 0],
        [          0,            0, 1]])
    Ry_pitch = np.array([
        [ np.cos(pitch), 0, np.sin(pitch)],
        [             0, 1,             0],
        [-np.sin(pitch), 0, np.cos(pitch)]])
    Rx_roll = np.array([
        [1,            0,             0],
        [0, np.cos(roll), -np.sin(roll)],
        [0, np.sin(roll),  np.cos(roll)]])
    rotMat = np.dot(Rz_yaw, np.dot(Ry_pitch, Rx_roll))
    return rotMat

#------------------------------------------------------------
x = 30
y = 45
z = 60

r = R.from_euler('zyx', [-z, -y, -x], degrees=True)
printt(r.as_dcm()\)

rm = euler_to_rotMat(z*pi/180, y*pi/180, x*pi/180)
printt(rm.T\)

#------------------------------------------------------------
rm = euler_to_rotMat(z*pi/180, y*pi/180, x*pi/180)
printt(rm.T\)

r = R.from_dcm(rm.T)
printt(r.as_euler('zyx')*180/pi:)






#------------------------------------------------------------
#        END OF FILE : TEST4.PY
#============================================================

---