说明:转发请注明作者和出处;以下文章内容为精简版,详细版以pdf格式的幻灯片形式呈现,请见链接:视觉SLAM_02_旋转矩阵.pdf
0.目录
1.点、向量和坐标系
设
R
3
\mathbb{R}^3
R3空间内的一组基为
(
e
1
,
e
2
,
e
3
)
(\boldsymbol e_1, \boldsymbol e_2, \boldsymbol e_3)
(e1,e2,e3),则任意向量
a
\boldsymbol{a}
a在这组基下有一个坐标:
a
=
[
e
1
,
e
2
,
e
3
]
[
a
1
a
2
a
3
]
=
a
1
e
1
+
a
2
e
2
+
a
3
e
3
(1)
\boldsymbol{a} = [\boldsymbol e_1, \boldsymbol e_2, \boldsymbol e_3] \begin{bmatrix} a_1 \\ a_2 \\ a_3 \end{bmatrix} = a_1 \boldsymbol e_1 + a_2 \boldsymbol e_2 + a_3 \boldsymbol e_3 \tag{1}
a=[e1,e2,e3]⎣⎡a1a2a3⎦⎤=a1e1+a2e2+a3e3(1)
这里
(
a
1
,
a
2
,
a
3
)
T
(a_1, a_2, a_3)^T
(a1,a2,a3)T称为
a
\boldsymbol{a}
a在此基下的坐标。外积如下定义:
a
×
b
=
∥
e
1
e
2
e
3
a
1
a
2
a
3
b
1
b
2
b
3
∥
=
[
a
2
b
3
−
a
3
b
2
a
3
b
1
−
a
1
b
3
a
1
b
2
−
a
2
b
1
]
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
b
=
d
e
f
a
^
b
(2)
\boldsymbol{a} \times \boldsymbol{b} = \begin{Vmatrix} \boldsymbol e_1 & \boldsymbol e_2 & \boldsymbol e_3 \\ a_1 & a_2 & a_3 \\ b_1 & b_2 & b_3 \end{Vmatrix} = \begin{bmatrix} a_2 b_ 3 - a_3 b_2 \\ a_3 b_1 - a_1 b_3 \\ a_1 b_2 - a_2 b_1 \end{bmatrix} = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix} \boldsymbol{b} \overset{def}{=} \boldsymbol{a} \hat{\,} \boldsymbol{b} \tag{2}
a×b=∥∥∥∥∥∥e1a1b1e2a2b2e3a3b3∥∥∥∥∥∥=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b=defa^b(2)
a
\boldsymbol{a}
a的反对称矩阵:
a
^
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
(3)
\boldsymbol{a} \hat{\,} = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix} \tag{3}
a^=⎣⎡0a3−a2−a30a1a2−a10⎦⎤(3)
2.坐标系间的欧氏变换
刚体运动过程中,物体坐标系到世界坐标系之间,相差了一个欧氏变换。欧氏变换由旋转和平移组成。
设某个单位正交基
(
e
1
,
e
2
,
e
3
)
(\boldsymbol e_1, \boldsymbol e_2, \boldsymbol e_3)
(e1,e2,e3)经过一次旋转变成了
(
e
1
′
,
e
2
′
,
e
3
′
)
(\boldsymbol e^{'}_1, \boldsymbol e^{'}_2, \boldsymbol e^{'}_3)
(e1′,e2′,e3′),对于同一个向量
a
\boldsymbol{a}
a它在两个坐标系下的坐标为
[
a
1
,
a
2
,
a
3
]
T
[a_1, a_2, a_3]^T
[a1,a2,a3]T和
[
a
1
′
,
a
2
′
,
a
3
′
]
[a^{'}_1, a^{'}_2, a^{'}_3]
[a1′,a2′,a3′],因为向量本身没有变,所以
[
e
1
,
e
2
,
e
3
]
[
a
1
a
2
a
3
]
=
[
e
1
′
,
e
2
′
,
e
3
′
]
[
a
1
′
a
2
′
a
3
′
]
(4)
[\boldsymbol e_1, \boldsymbol e_2, \boldsymbol e_3] \begin{bmatrix} a_1 \\ a_2 \\ a_3 \end{bmatrix} = [\boldsymbol e^{'}_1, \boldsymbol e^{'}_2, \boldsymbol e^{'}_3] \begin{bmatrix} a^{'}_1 \\ a^{'}_2 \\ a^{'}_3 \end{bmatrix} \tag{4}
[e1,e2,e3]⎣⎡a1a2a3⎦⎤=[e1′,e2′,e3′]⎣⎡a1′a2′a3′⎦⎤(4)
整理有:
[
a
1
a
2
a
3
]
=
[
e
1
T
e
1
′
e
1
T
e
2
′
e
1
T
e
3
′
e
2
T
e
1
′
e
2
T
e
2
′
e
2
T
e
3
′
e
3
T
e
1
′
e
3
T
e
2
′
e
3
T
e
3
′
]
=
d
e
f
R
a
′
(5)
\begin{bmatrix} a_1 \\ a_2 \\ a_3 \end{bmatrix} = \begin{bmatrix} \boldsymbol e^{T}_1 \boldsymbol e^{'}_1 & \boldsymbol e^{T}_1 \boldsymbol e^{'}_2 & \boldsymbol e^{T}_1 \boldsymbol e^{'}_3 \\ \boldsymbol e^{T}_2 \boldsymbol e^{'}_1 & \boldsymbol e^{T}_2 \boldsymbol e^{'}_2 & \boldsymbol e^{T}_2 \boldsymbol e^{'}_3 \\ \boldsymbol e^{T}_3 \boldsymbol e^{'}_1 & \boldsymbol e^{T}_3 \boldsymbol e^{'}_2 & \boldsymbol e^{T}_3 \boldsymbol e^{'}_3 \end{bmatrix} \overset{def}{=} \boldsymbol R \boldsymbol a^{'} \tag{5}
⎣⎡a1a2a3⎦⎤=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤=defRa′(5)
其中,
R
=
[
e
1
T
e
1
′
e
1
T
e
2
′
e
1
T
e
3
′
e
2
T
e
1
′
e
2
T
e
2
′
e
2
T
e
3
′
e
3
T
e
1
′
e
3
T
e
2
′
e
3
T
e
3
′
]
\boldsymbol R = \begin{bmatrix} \boldsymbol e^{T}_1 \boldsymbol e^{'}_1 & \boldsymbol e^{T}_1 \boldsymbol e^{'}_2 & \boldsymbol e^{T}_1 \boldsymbol e^{'}_3 \\ \boldsymbol e^{T}_2 \boldsymbol e^{'}_1 & \boldsymbol e^{T}_2 \boldsymbol e^{'}_2 & \boldsymbol e^{T}_2 \boldsymbol e^{'}_3 \\ \boldsymbol e^{T}_3 \boldsymbol e^{'}_1 & \boldsymbol e^{T}_3 \boldsymbol e^{'}_2 & \boldsymbol e^{T}_3 \boldsymbol e^{'}_3 \end{bmatrix}
R=⎣⎡e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎦⎤
即为旋转矩阵(方向余弦矩阵),刻画了旋转前后同一个向量的坐标变换关系。
向量
a
\boldsymbol{a}
a在坐标系1和坐标系2下的坐标为
a
1
\boldsymbol{a_1}
a1,
a
2
\boldsymbol{a_2}
a2,则:
a
1
=
R
12
a
2
+
t
12
(6)
\boldsymbol{a_1} = \boldsymbol R_{12} \boldsymbol{a_2} + \boldsymbol{t_{12}} \tag{6}
a1=R12a2+t12(6)
这里的
R
12
\boldsymbol R_{12}
R12是指“把坐标系2中的向量变换到坐标系1中表示”。
t
12
\boldsymbol t_{12}
t12实际是坐标系1原点指向坐标系2原点的向量在坐标系1下取的坐标。
3.变换矩阵与齐次坐标
假设进行了两次变换:
R
1
,
t
1
\boldsymbol R_1, \boldsymbol t_1
R1,t1和
R
2
,
t
2
\boldsymbol R_2, \boldsymbol t_2
R2,t2:
b
=
R
1
a
+
t
1
,
c
=
R
2
b
+
t
2
\boldsymbol b = \boldsymbol R_1 \boldsymbol a + \boldsymbol t_1, c = \boldsymbol R_2 \boldsymbol b + \boldsymbol t_2
b=R1a+t1,c=R2b+t2
则从
a
\boldsymbol a
a变到
c
\boldsymbol c
c为:
c
=
R
2
(
R
1
a
+
t
1
)
+
t
2
\boldsymbol c = \boldsymbol R_2 (\boldsymbol R_1 \boldsymbol a + \boldsymbol t_1) + \boldsymbol t_2
c=R2(R1a+t1)+t2
这种形式多次变换之后显得很罗嗦,因此引入齐次坐标表示方法:
[
a
′
1
]
=
[
R
t
0
T
1
]
[
a
1
]
=
d
e
f
T
[
a
1
]
\begin{bmatrix} \boldsymbol a^{'} \\ 1 \end{bmatrix} = \begin{bmatrix} \boldsymbol R & \boldsymbol t \\ \boldsymbol 0^T & 1 \end{bmatrix} \begin{bmatrix} \boldsymbol a \\ 1 \end{bmatrix} \overset{def}{=} \boldsymbol T \begin{bmatrix} \boldsymbol a \\ 1 \end{bmatrix}
[a′1]=[R0Tt1][a1]=defT[a1]
矩阵
T
\boldsymbol T
T称为变换矩阵,暂时用
a
~
\tilde{\boldsymbol a}
a~表示
a
\boldsymbol a
a的齐次坐标,两次变换的叠加:
b
~
=
T
1
a
~
,
c
~
=
T
2
b
~
⇒
c
~
=
T
2
T
1
a
~
\tilde{\boldsymbol b} = \boldsymbol T_1 \tilde{\boldsymbol a}, \tilde{\boldsymbol c} = \boldsymbol T_2 \tilde{\boldsymbol b} \Rightarrow \tilde{\boldsymbol c} = \boldsymbol T_2 \boldsymbol T_1 \tilde{\boldsymbol a}
b~=T1a~,c~=T2b~⇒c~=T2T1a~
4.参考文献
- 高翔等. 视觉SLAM十四讲:从理论到实践第二版. 北京:电子工业出版社,2019.8.