文章目录
参考资料
1. 旋转矩阵
1.1 内积、外积
对于
a
,
b
∈
R
3
a,b \in \mathbb{R}^3
a,b∈R3,内积可以写成
内积可以描述向量间的投影关系。而外积呢是这个样子:
外积的方向垂直于这两个向量,大小为
∣
a
∣
∣
b
∣
s
i
n
(
a
,
b
)
|a||b| sin (a,b)
∣a∣∣b∣sin(a,b),是两个向量张成的四边形的有向面积。对于外积,我们引入了^
符号,把a写成一个矩阵,事实上是一个反对称矩阵(Skew-symmetric)。这样就把外积
a
x
b
axb
axb,写成了矩阵与向量的乘法a^b,把它变成了线性运算。这个符号将在后文经常用到,请记住它。外积只对三维向量存在定义,我们还能用外积表示向量的旋转。
为什么外积可以表示旋转呢?
考虑两个不平行的向量a,b,我们要描述从a到b之间是如何旋转的,如图3-1所示。我们可以用一个向量来描述三维空间中两个向量的旋转关系。在右手法则下,我们用右手的四个指头从a转向b,其大拇指朝向就是旋转向量的方向,事实上也是 a × b a\times b a×b的方向。它的大小则由a和b的夹角决定。通过这种方式,我们构造了从a到b的一个旋转向量。这个向量同样位于三维空间中,在此坐标系下,可以用三个实数来描述它。
1.2 坐标变换
-
与向量间的旋转类似,我们同样可以描述两个坐标系之间的旋转关系,再加上平移,统称为坐标系之间的变换关系。
-
一个欧氏变换由一个旋转和一个平移两部分组成。
1.2.1 旋转
首先来考虑旋转。我们设某个单位正交基
(
e
1
,
e
2
,
e
3
)
(e_1,e_2,e_3)
(e1,e2,e3)经过一次旋转,变成了
(
e
1
′
,
e
2
′
,
e
3
′
)
(e'_1, e'_2,e'_3)
(e1′,e2′,e3′)。那么,对于同一个向量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
′
]
T
[a'_1,a'_2,a'_3]^T
[a1′,a2′,a3′]T,根据坐标的定义,有:
为了描述两个坐标之间的关系,我们对上面等式左右同时左乘
[
e
1
,
e
2
,
e
3
]
T
[e_1,e_2,e_3]^T
[e1,e2,e3]T,那么左边的系数变成了单位矩阵,所以:
我们把中间的阵拿出来,定义成一个矩阵R。这个矩阵由两组基之间的内积组成,刻画了旋转前后同一个向量的坐标变换关系。只要旋转是一样的,那么这个矩阵也是一样的。可以说,矩阵R描述了旋转本身。因此它又称为旋转矩阵。
旋转矩阵有一些特别的性质。事实上,它是一个行列式为1的正交矩阵。反之,行列式为1的正交矩阵也是一个旋转矩阵。所以,我们可以把旋转矩阵的集合定义如下:
SO(n)
是特殊正交群(Special Orthogonal Group)的意思。
1.2.2 平移
在欧氏变换中,除了旋转之外还有一个平移。考虑世界坐标系中的向量
a
a
a,经过一次旋转(用R描述)和一次平移
t
t
t后,得到了
a
′
a'
a′,那么把旋转和平移合到一起,有:
其中,
t
t
t称为平移向量。相比于旋转,平移部分只需把这个平移量加到旋转之后的坐标上,显得非常简洁。
1.2.3 变换矩阵与齐次坐标
式(3.8)完整地表达了欧氏空间的旋转与平移,不过还存在一个小问题:这里的变换关系不是一个线性关系。假设我们进行了两次变换:
R
1
,
t
1
R_1,t_1
R1,t1和
R
2
,
t
2
R_2,t_2
R2,t2,满足:
这样的形式在变换多次之后会过于复杂。因此,我们要引入齐次坐标和变换矩阵重写式(3.8):
这是一个数学技巧:我们把一个三维向量的末尾添加1,变成了四维向量,称为齐次坐标。对于这个四维向量,我们可以把旋转和平移写在一个矩阵里面,使得整个关系变成了线性关系。该式中,矩阵T称为变换矩阵(Transform Matrix)。我们暂时用
a
~
\tilde{a}
a~表示a的齐次坐标。
齐次坐标
它是射影几何里的概念。通过添加最后一维,我们用四个实数描述了一个三维向量,这显然多了一个自由度,但允许我们把变换写成线性的形式。在齐次坐标中,某个点 x x x的每个分量同乘一个非零常数k后,仍然表示的是同一个点。因此,一个点的具体坐标值不是唯一的。如 [ 1 , 1 , 1 , 1 ] T [1,1,1,1]^T [1,1,1,1]T和 [ 2 , 2 , 2 , 2 ] T [2,2,2,2]^T [2,2,2,2]T是同一个点。但当最后一项不为零时,我们总可以把所有坐标除以最后一项,强制最后一项为1,从而得到一个点唯一的坐标表示(也就是转换成非齐次坐标):
这时,忽略掉最后一项,这个点的坐标和欧氏空间就是一样的。依靠齐次坐标和变换矩阵,两次变换的累加就可以有很好的形式:
变换矩阵
关于变换矩阵T,它具有比较特别的结构:左上角为旋转矩阵,右侧为平移向量,左下角为0向量,右下角为1。这种矩阵又称为特殊欧氏群(Special Euclidean Group):
与SO(3)一样,求解该矩阵的逆表示一个反向的变换:
最后,为了保持符号的简洁,在不引起歧义的情况下,我们以后不区别齐次坐标与普通的坐标的符号,默认我们使用的是符合运算法则的那一种。
回顾一下我们介绍的内容:首先,我们说了向量和它的坐标表示,并介绍了向量间的运算;然后,坐标系之间的运动由欧氏变换描述,它由平移和旋转组成。旋转可以由旋转矩阵 S O ( 3 ) SO(3) SO(3)描述,而平移直接由一个 R 3 \mathbb{R}^3 R3向量描述。最后,如果将平移和旋转放在一个矩阵中,就形成了变换矩阵 S E ( 3 ) SE(3) SE(3)。
Eigen
是一个C++开源线性代数库。它提供了快速的有关矩阵的线性代数运算,还
包括解方程等功能。许多上层的软件库也使用Eigen进行矩阵运算,包括g2o
,Sophus
等。可自行学习
2. 旋转向量和欧拉角
2.1 旋转向量
旋转矩阵表示方式至少有以下几个缺点:
- SO(3)的旋转矩阵有九个量,但一次旋转只有三个自由度。因此这种表达方式是冗余的。同理,变换矩阵用十六个量表达了六自由度的变换。
- 旋转矩阵自身带有约束:它必须是个正交矩阵,且行列式为1。变换矩阵也是如此。当我们想要估计或优化一个旋转矩阵/变换矩阵时,这些约束会使得求解变得更困难。
任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是,我们可以使用一个向量,其方向与旋转轴一致,而长度等于旋转角。这种向量,称为旋转向量(或轴角, Axis-Angle)。这种表示法只需一个三维向量即可描述旋转。同样,对于变换矩阵,我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的维数正好是六维。
旋转向量和旋转矩阵之间是如何转换的呢?
假设有一个旋转轴为
n
n
n,角度为
θ
\theta
θ的旋转,显然,它对应的旋转向量为
θ
n
\theta n
θn。由旋转向量到旋转矩阵的过程由罗德里格斯公式(Rodrigues’s Formula)表明,由于推导过程比较复杂,我们不作描述,只给出转换的结果:
符号KaTeX parse error: Expected group after '^' at position 1: ^̲是向量到反对称的转换符,见式(3.3)。反之,我们也可以计算从一个旋转矩阵到旋转向量的转换。对于转角
θ
\theta
θ有:
因此,转轴
n
n
n是矩阵
R
R
R特征值1对应的特征向量。求解此方程,再归一化,就得到了旋转轴。读者也可以从“旋转轴经过旋转之后不变”的几何角度看待这个方程。
2.2 欧拉角
无论是旋转矩阵、旋转向量,虽然它们能描述旋转,但对我们人类是非常不直观的。
当我们看到一个旋转矩阵或旋转向量时,很难想象出来这个旋转究竟是什么样的。当它们变换时,我们也不知道物体是向哪个方向在转动。而欧拉角则提供了一种非常直观的方式来描述旋转——它使用了三个分离的转角,把一个旋转分解成三次绕不同轴的旋转。
当然,由于分解方式有许多种,所以欧拉角也存在着不同的定义方法。比如说,当我先绕X轴旋转,再绕Y轴,最后绕Z轴,就得到了一个XYZ轴的旋转。同理,可以定义,ZYZ、 ZYX等等旋转方式。
欧拉角当中比较常用的一种,便是用“偏航-俯仰-滚转”(yaw-pitch-roll)三个角度来描述一个旋转的。由于它等价于ZYX轴的旋转,我们就以ZYX为例。假设一个刚体的前方(朝向我们的方向)为X轴,右侧为Y轴,上方为Z轴,见图3-3。那么,ZYX转角相当于把任意旋转分解成以下三个轴上的转角:
- 绕物体的Z轴旋转,得到偏航角yaw;
- 绕旋转之后的Y轴旋转,得到俯仰角pitch;
- 绕旋转之后的X轴旋转,得到滚转角roll。
欧拉角的一个重大缺点是会碰到著名的万向锁问题(Gimbal Lock):在俯仰角为
±
90
°
\pm90°
±90°时,第一次旋转与第三次旋转将使用同一个轴,使得系统丢失了一个自由度(由三次旋转变成了两次旋转)。这被称为奇异性问题,在其他形式的欧拉角中也同样存在。
3. 四元数
旋转矩阵用九个量描述三自由度的旋转,具有冗余性;
欧拉角和旋转向量是紧凑的,但具有奇异性。
事实上,我们找不到不带奇异性的三维向量描述方式。这有点类似于,当我们想用两个坐标表示地球表面时(如经度和纬度),必定存在奇异性(纬度为 ± 90 ° \pm90° ±90°时经度无意义)。
三维旋转是一个三维流形,想要无奇异性地表达它,用三个量是不够的。
回忆我们以前学习过的复数。我们用复数集C表示复平面上的向量,而复数的乘法则能表示复平面上的旋转:例如,乘上复数i相当于逆时针把一个复向量旋转90度。类似的,在表达三维空间旋转时,也有一种类似于复数的代数:四元数(Quaternion)。四元数是Hamilton找到的一种扩展的复数.它既是紧凑的,也没有奇异性。如果说缺点的话,四元数不够直观,其运算稍为复杂一些。
一个四元数q拥有一个实部和三个虚部:
其中
i
,
j
,
k
i,j,k
i,j,k为四元数的三个虚部。这三个虚部满足关系式:
由于它的这种特殊表示形式,有时人们也用一个标量和一个向量来表达四元数:
这里, s s s称为四元数的实部,而 v v v称为它的虚部。如果一个四元数虚部为0,称之为实四元数。反之,若它的实部为0,称之为虚四元数。
3.1 旋转向量与四元数
假设某个旋转是绕单位向量
n
=
[
n
x
,
n
y
,
n
z
]
T
n = [n_x,n_y,n_z]^T
n=[nx,ny,nz]T进行了角度为0的旋转,那么这个旋转的四元数形式为:
反之,我们亦可从单位四元数中计算出对应旋转轴与夹角:
这式子给我们一种微妙的“转了一半”的感觉。同样,对式(3.19)的0加上2m,我们得到一个相同的旋转,但此时对应的四元数变成了
−
q
-q
−q。因此,在四元数中,任意的旋转都可以由两个互为相反数的四元数表示。同理,取
θ
\theta
θ为0,则得到一个没有任何旋转的实四元数:
3.2 四元数的运算
四元数和通常复数一样,可以进行一系列的运算。常见的有四则运算、数乘、求逆、共轭等等。
现有两个四元数
q
a
,
q
b
q_a,q_b
qa,qb,它们的向量表示为
[
s
a
,
v
a
]
,
[
s
b
,
v
b
]
[s_a,v_a], [s_b,v_b]
[sa,va],[sb,vb],或者原始四元数表示为:
-
加法和减法
四元数 q a , q b q_a,q_b qa,qb的加减运算为:
-
乘法
乘法是把 q a q_a qa的每一项与 q b q_b qb每项相乘,最后相加,虚部要按照式(3.18)进行。
虽然稍为复杂,但形式上是整齐有序的。如果写成向量形式并利用内外积运算,该表达会更加简洁:
在该乘法定义下,两个实的四元数乘积仍是实的,这与复数也是一致的。然而,注意到,由于最后一项外积的存在,四元数乘法通常是不可交换的,除非 v a v_a va和 v b v_b vb在 R 3 \mathbb{R}^3 R3中共线,那么外积项为零。 -
共轭
四元数的共轭是把虚部取成相反数:
四元数共轭与自己本身相乘,会得到一个实四元数,其实部为模长的平方:
-
模长
四元数的模长定义为:
可以验证,两个四元数乘积的模即为模的乘积。这保证单位四元数相乘后仍是单位四元数。
-
逆
一个四元数的逆为:
按此定义,四元数和自己的逆的乘积为实四元数的1:
如果q为单位四元数,逆和共轭就是同一个量。同时,乘积的逆有和矩阵相似的性质:
-
数乘与点乘
和向量相似,四元数可以与数相乘:
点乘是指两个四元数每个位置上的数值分别相乘:
3.3 用四元数表示旋转
我们可以用四元数表达对一个点的旋转。假设一个空间三维点 p = [ r , y , z ] ∈ R 3 p=[r,y,z] \in \mathbb{R}^3 p=[r,y,z]∈R3,以及一个由轴角 n , θ n,\theta n,θ指定的旋转。三维点 p p p经过旋转之后变成为 p ′ p' p′。如果使用矩阵描述,那么有 p ′ = R p p'=Rp p′=Rp。如果用四元数描述旋转,它们的关系如何来表达呢?
首先,把三维空间点用一个虚四元数来描述:
这相当于我们把四元数的三个虚部与空间中的三个轴相对应。然后,参照式(3.19),用四元数
q
q
q表示这个旋转:
那么,旋转后的点
p
′
p'
p′即可表示为这样的乘积:
可以验证,计算结果的实部为0,故为纯虚四元数。其虚部的三个分量表示旋转后3D点的坐标
3.4 四元数与旋转矩阵
设四元数
q
=
q
0
+
q
1
i
+
q
2
j
+
q
3
k
q=q_0+q_1i+q_2j+q_3k
q=q0+q1i+q2j+q3k,对应的旋转矩阵R为:
反之,由旋转矩阵到四元数的转换如下。假设矩阵为
R
=
m
i
j
,
i
,
j
∈
[
1
,
2
,
3
]
R= {m_{ij}},i,j\in [1,2,3]
R=mij,i,j∈[1,2,3],其对应的四元数
q
q
q由下式给出:
值得一提的是,由于 q q q和 − q -q −q表示同一个旋转,事实上一个R对应的四元数表示并不是惟一的。同时,除了上面给出的转换方式之外,还存在其他几种计算方法,这里不再赘述。
4. 相似、仿射、射影变换
4.1 相似变换
相似变换比欧氏变换多了一个自由度,它允许物体进行均匀的缩放,其矩阵表示为:
注意到旋转部分多了一个缩放因子s,表示我们在对向量旋转之后,可以在
x
,
y
,
z
x,y,z
x,y,z三个坐标上进行均匀的缩放。由于含有缩放,相似变换不再保持图形的面积不变。可以想象一个边长为1的立方体通过相似变换后,变成边长为10的样子(但仍然是立方体)。
4.2 仿射变换
仿射变换矩阵形式如下:
与欧氏变换不同的是,仿射变换只要求A是一个可逆矩阵,而不必是正交矩阵。仿射变换也叫正交投影。经过仿射变换之后,立方体就不再是方的了,但是各个面仍然是平行四边形。
4.3 射影变换
射影变换是最一般的变换,矩阵形式为: