转换矩阵、平移矩阵、旋转矩阵关系以及python实现旋转矩阵、四元数、欧拉角之间转换

文章目录

• 1. 转换矩阵、平移矩阵、旋转矩阵之间的关系
• 2. 缩放变换、平移变换和旋转变换
• 2. python实现旋转矩阵、四元数、欧拉角互相转化

由于在平时总是或多或少的遇到平移旋转的问题，每次都是现查资料，然后查了忘，忘了继续查，这次弄明白之后干脆写一篇文章，给人方便同时于己方便，后续如有扩充或变动也方便添加。

1. 转换矩阵、平移矩阵、旋转矩阵之间的关系

假设有两个向量

a

1

=

(

x

1

,

y

1

,

z

1

)

a_1 = (x_1, y_1, z_1)

a1​=(x1​,y1​,z1​)和

a

2

=

(

x

2

,

y

2

,

z

2

)

a_2 = (x_2, y_2, z_2)

a2​=(x2​,y2​,z2​)，它们的转换关系为：

a

1

=

R

∗

a

2

+

T

a_1 = R * a_2 + T

a1​=R∗a2​+T

这里

R

R

R就是它的旋转矩阵，

T

T

T就是它的平移矩阵。使用齐次方式表示如下：

(

a

1

1

)

=

(

R

T

0

1

)

∗

(

a

2

1

)

\begin{pmatrix} a_1\\ 1 \end{pmatrix}= \begin{pmatrix} R&T\\ 0&1 \end{pmatrix}* \begin{pmatrix} a_2\\1 \end{pmatrix}

(a1​1​)=(R0​T1​)∗(a2​1​)

使用元素值替换后，表示如下：

(

x

1

y

1

z

1

1

)

=

(

r

11

r

12

r

13

t

1

r

21

r

22

r

23

t

2

r

31

r

32

r

33

t

3

0

0

0

1

)

∗

(

x

2

y

3

z

2

1

)

\begin{pmatrix} x_1\\y_1\\z_1\\1 \end{pmatrix}= \begin{pmatrix} r_{11}&r_{12}&r_{13}&t_{1}\\ r_{21}&r_{22}&r_{23}&t_{2}\\ r_{31}&r_{32}&r_{33}&t_{3}\\ 0&0&0&1 \end{pmatrix}* \begin{pmatrix} x_2\\y_3\\z_2\\1 \end{pmatrix}

​x1​y1​z1​1​
​=
​r11​r21​r31​0​r12​r22​r32​0​r13​r23​r33​0​t1​t2​t3​1​
​∗
​x2​y3​z2​1​
​

在仿射变换中的转换矩阵表示先线性变换再平移。在这里转换矩阵表示如下：

转换矩阵

=

(

r

11

r

12

r

13

t

1

r

21

r

22

r

23

t

2

r

31

r

32

r

33

t

3

0

0

0

1

)

转换矩阵= \begin{pmatrix} r_{11}&r_{12}&r_{13}&t_{1}\\ r_{21}&r_{22}&r_{23}&t_{2}\\ r_{31}&r_{32}&r_{33}&t_{3}\\ 0&0&0&1 \end{pmatrix}

转换矩阵=
​r11​r21​r31​0​r12​r22​r32​0​r13​r23​r33​0​t1​t2​t3​1​
​

平移矩阵表示如下：

平移矩阵

T

=

(

t

1

t

2

t

3

)

平移矩阵T=\begin{pmatrix} t_{1}\\ t_{2}\\ t_{3}\\ \end{pmatrix}

平移矩阵T=
​t1​t2​t3​​
​

旋转矩阵表示如下：

旋转矩阵

R

=

(

r

11

r

12

r

13

r

21

r

22

r

23

r

31

r

32

r

33

)

旋转矩阵R=\begin{pmatrix} r_{11}&r_{12}&r_{13}\\ r_{21}&r_{22}&r_{23}\\ r_{31}&r_{32}&r_{33} \end{pmatrix}

旋转矩阵R=
​r11​r21​r31​​r12​r22​r32​​r13​r23​r33​​
​

2. 缩放变换、平移变换和旋转变换

如果理解以上知识点之后，缩放变换、平移变换和旋转变换的特殊情况也迎刃而解。

• 缩放变换

缩放变换只是在尺度上进行改变，所以它的变换形式如下：

• 平移变换

平移变换的时候，角度不发生改变，也就是旋转矩阵R为单位矩阵，所以它的变换形式如下：

• 旋转变换

当空间内的物体绕着 x 轴，y 轴或者 z 轴旋转的时候，变换矩阵为：

对于一般性的旋转问题，可以用简单的旋转描述复杂的旋转。用 x 轴，y 轴和 z 轴上的旋转来定义旋转：

这三个角就被称作欧拉角（Euler angles）。

2. python实现旋转矩阵、四元数、欧拉角互相转化

在应用中，我们往往会遇到旋转矩阵、四元数和欧拉角之间的互相转换，在这里，我们只使用python代码来实现它们之间互相转换。

from scipy.spatial.transform import Rotation as R

def quaternion2euler(quaternion):
    r = R.from_quat(quaternion)
    euler = r.as_euler('xyz', degrees=True)
    return euler

def euler2quaternion(euler):
    r = R.from_euler('xyz', euler, degrees=True)
    quaternion = r.as_quat()
    return quaternion

def euler2rotation(euler):
    r = R.from_euler('xyz', euler, degrees=True)
    rotation_matrix = r.as_matrix()
    return rotation_matrix

def quaternion2rotation_matrix(quaternion):
    r = R.from_quat(quaternion)
    rotation_matrix = r.as_matrix()
    return rotation_matrix

def rotation_matrix2euler(rotation_matrix):
    r = R.from_matrix(rotation_matrix)
    euler = r.as_euler('xyz', degrees=True)
    return euler
    

def rotation_matrix2quaternion(rotation_matrix):
    r = R.from_matrix(rotation_matrix)
    quaternion = r.as_quat()
    return quaternion

if __name__ == '__main__':
    # 四元数=>欧拉角
    quaternion = [0.71934025092983234, -1.876085535681999e-06, -3.274841213980097e-08, -0.69465790385533299]
    euler = quaternion2euler(quaternion) # [-9.20000743e+01  1.52039496e-04 -1.52039496e-04]
    print(f'euler: {euler}')
    
    # 四元数=>旋转矩阵
    rotation_matrix = quaternion2rotation_matrix(quaternion)
    print(f'rotation_matrix: {rotation_matrix}')
    
    # 欧拉角=>四元数
    quaternion = euler2quaternion(euler)
    print(f'quaternion: {quaternion}') # [-7.19340251e-01  1.87608554e-06  3.27484122e-08  6.94657904e-01]
    
    # 欧拉角=>旋转矩阵
    rotation_matrix = euler2rotation(euler)
    print(f'rotation_matrix: {rotation_matrix}')
    
    # 旋转矩阵=>欧拉角
    euler = rotation_matrix2euler(rotation_matrix)
    print(f'euler: {euler}')
    
    # 旋转矩阵=>四元数
    quaternion = rotation_matrix2quaternion(rotation_matrix)
    print(f'quaternion: {quaternion}')