学习视觉SLAM需要一些基本知识,大致有:三维空间刚体运动、李群与李代数、矩阵论、优化方法、概率论。基础不牢地动山摇,而且没有数学基础的理解是片面而感性的,所以下面对视觉SLAM中所需要用到的基础知识进行分别介绍:
(1)三维空间的刚体运动
1、旋转矩阵以及变换矩阵
一个向量可以在不同的坐标系下进行描述,假设有一个向量 a a a,坐标系1和2(两个坐标系原点重合,有一定的旋转)的单位向量分别为 ( 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 a可以表示为 ( e 1 , e 2 , e 3 ) ∗ ( a 1 , a 2 , a 3 ) T (e_1,e_2,e_3) *(a_1,a_2,a_3)^T (e1,e2,e3)∗(a1,a2,a3)T 或者 或者 或者 ( e 1 ′ , e 2 ′ , e 3 ′ ) ∗ ( a 1 ′ , a 2 ′ , a 3 ′ ) T (e_1^{'},{e_2}^{'},{e_3}^{'})*({a_1}^{'},{a_2}^{'},{a_3}^{'})^T (e1′,e2′,e3′)∗(a1′,a2′,a3′)T,二者表示同一个向量,所以有相等关系,并且可以推得:
其中
R
R
R为旋转矩阵,它表示一个坐标系和另外一个坐标系之间的旋转关系,该矩阵可通过数学证明为一个单位正交阵,由正交矩阵可以推得
R
T
=
R
−
1
R^T=R^{-1}
RT=R−1,则每行或者每列都是单位向量,且不同行或列之间正交,旋转矩阵为正交矩阵则表明对向量的变换为正交变换,正交变换可以保证变换之后的向量长度,向量之间的内积、距离和夹角等很多性质不变。(其中单位阵的含义是矩阵的行列式为单位1,根据线性代数的几何意义,线性变换是对一个向量进行线性的变换,变换前后不会产生弯曲,若变换前是直线,则变换后也是直线,矩阵的行列式表示对直线的缩放系数,由于旋转不改变向量的长度,所以旋转矩阵的行列式为1。)
所以旋转矩阵对向量做的是一种正交变换,满足正交变换的矩阵(单位正交阵)也为旋转矩阵。
一般的刚体运动往往由旋转和平移组成,根据欧拉定理:刚体在三维空间里的一般运动,可分解为刚体上方某一点的平移,以及绕经此点的旋转轴的转动,则刚体运动可表示为坐标系原点间的平移和绕原点间的旋转表示,用公式可表现为:
a
′
=
R
a
+
t
a^{'}=Ra+t
a′=Ra+t,这种表达方式对于单个变换还行,但是对于复合情况有很大的不便之处,例如:
b
=
R
1
a
+
t
1
c
=
R
2
b
+
t
2
c
=
R
2
(
R
1
a
+
t
1
)
+
t
2
b=R_1a+t_1\\ c=R_2b+t_2\\ c=R_2(R_1a+t_1)+t_2
b=R1a+t1c=R2b+t2c=R2(R1a+t1)+t2
为了复合变换时的方便性,将其写为齐次的形式,将旋转矩阵和平移向量放在一起,形成变换矩阵,变换矩阵的形式为:
其中 ( a , 1 ) T (a,1)^T (a,1)T为齐次坐标(用n+1维的向量表示n维空间中的点)。
2、旋转向量和欧拉角
一个旋转只有三个自由度,但是用旋转矩阵有9个参数(三个向量分别为单位向量自由度-3,两两垂直自由度-3),所以旋转矩阵的表达是冗余的,表达效率较低,那有没有表达效率稍微高一点的呢?效率较高的有旋转向量和欧拉角,他们都只用三个量就完成了旋转的表达。
①旋转向量
旋转向量用方向表示转轴,用长度表示转过的角度,也被称为角轴或者轴角。该表示方法只有三个变量,并且其三个量无约束,同时也更加直观,具有一定的几何意义。由于旋转向量和旋转矩阵是同一个东西的两种表达形式,它们之间有一定的换算方法,将旋转向量转化为旋转矩阵可以通过罗德里格斯公式:
其中 R R R为旋转矩阵, I I I为单位矩阵, n n n为旋转向量方向的单位向量, θ \theta θ为旋转角度。
将旋转矩阵转换为旋转向量的公式为:
②欧拉角
旋转也可以分解为三个方向上的转动,转动的顺序可以不同,例如按 Z − Y − X Z-Y-X Z−Y−X的顺序转动,轴也可以分为定轴和动轴,不同的领域的习惯有所不同。例如绕 Z Z Z轴旋转可以得到偏航角 y a w yaw yaw,绕旋转之后的 Y Y Y轴旋转可以得到俯仰角 p i t c h pitch pitch,绕旋转之后的 X X X轴旋转得到横滚角 r o l l roll roll。
欧拉角也是用三个变量表示一个旋转,但是对于上述的欧拉角的表示形式,当绕 y y y轴旋转90度的时候会出现万向锁的问题,此时自由度会减去1,此时欧拉角无法正确表达一个旋转。由于万向锁的问题,欧拉角不适合用于插值或者迭代,由于其较好的几何意义,常用于人机交互。并且使用三个实数表达旋转时,会不可避免的存在奇异性的问题(旋转向量也具有奇异性的问题,当转角超过360度时具有,旋转向量的长度具有周期性)。
3、四元数
由于使用欧拉角或者旋转向量都会不可避免的遇到万向锁的问题,使用旋转矩阵又太过冗余,因此可以用一个更加紧凑且不会出现奇异性的方式来表达旋转——四元数。
在二维的情况下,可以使用单位复数来表达一个旋转,三维情况下,四元数可以作为复数的一个扩充,四元数有一个实部和三个虚部: 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,其中三个虚部自己和自己的运算像复数,自己和别人的运算像叉乘。
既然四元数能表示一个旋转,那么它是如何对空间中一个点 ( x , y , z ) (x,y,z) (x,y,z)进行旋转的呢?
首先将点坐标转化为一个纯虚四元数 p p p,假设其经过单位四元数 q q q的旋转,则旋转后的坐标用四元数可以表示为: p ′ = q p q − 1 p^{'}=qpq^{-1} p′=qpq−1 ,可以证明 ,可以证明 ,可以证明 p ′ p^{'} p′为纯虚四元数,其三个坐标为旋转之后的点的坐标。并且数学上可以证明,四元数和旋转向量之间有以下的转换关系:
四元数相对于旋转矩阵的优点是紧凑性,相对于旋转向量和欧拉角的优点是无奇异性,因此是一种有效的旋转表达方法,虽然其不太直观。