视觉SLAM十四讲第三讲笔记

视觉SLAM十四讲第三讲笔记

这一讲主要是讲三维空间刚体运动:
主要是理解旋转矩阵，变换矩阵，欧拉角和四元数。

一、旋转矩阵

相机的位置是指相机在空间中的哪个地方，姿态是指相机的朝向。如何用数学语言表示呢？

1. 点和向量，坐标系

书中给出了内积和外积的公式:

内积:
$$a\cdot b=a^{T}b=\sum _{i=1}^3{a}_i{b}_i=|a||b|cos⟨a,b⟩$$
内积可以描述向量间的关系。

外积：

外积的方向垂直于这两个向量，大小为$|a||b|sin⟨a,b⟩$，是两个向量张成的四边形的有向面积对于外积，我们引入了$\wedge$符号，把$a$写成一个矩阵。事实上是一个反对称矩阵(Skew-symmetric)，你可以将$\wedge$记成一个反对称符号。这样就把外积$a\times b$，写成了矩阵与向量的乘法$a\wedge b$，把它变成了线性运算。我们还能用外积表示向量的旋转。

2.坐标系间的欧式变换

相机运动是一个刚体运动，它保证了同一个向量在各个坐标系下的长度和夹角都不会发生变化。这种变换称为欧氏变换。欧式变换由一个旋转和一个平移两部分组成。

(1) 旋转

首先来考虑旋转。 我们设某个单位正交基$(e_1,e_2,e_3)$经过一次旋转变成了$(e_1^{\prime },e_2^{\prime },e_3^{\prime })$。那么对于同一个向量$a$，它在两个坐标系下的坐标为$[a_1,a_2,a_3{]}^T$和$[a_1^{\prime },a_2^{\prime },a_3^{\prime }]$。根据坐标的定义:

$$[e_1,e_2,e_3]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=[e_1^{\prime },e_2^{\prime },e_3^{\prime }]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]$$
同时左乘$\left[\begin{array}{c}{e}_1^T\\ e_2^T\\ e_3^T\end{array}\right]$，就得到了:

中间的矩阵，定义为矩阵$R$,描述了旋转本身。因此它又称为旋转矩阵。旋转矩阵有一些特别的性质。事实上，它是一个行列式为1的正交矩阵。反之，行列式为1的正交矩阵也是一个旋转矩阵。 所以，我们可以把旋转矩阵的集合定义如下:
S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } SO(n)=\{R \in \R^{n \times n} | RR^T = I, det(R)=1 \} SO(n)={R∈Rn×n∣RRT=I,det(R)=1}

$SO(n)$是特殊的正交群(special orthogonal group)。 这个集合由$n$维空间的旋转矩阵组成，特别的，SO(3)就是三维空间的旋转了。 旋转矩阵可以描述相机的旋转 。

由于旋转矩阵为正交阵，它的逆(即转置)描述了一个相反的旋转。
$$a^{\prime }=R^{-1}a=R^{T}a$$

(2)平移

再来考虑平移。
$$a^{\prime }=Ra+t$$
这里的 向量$t$就是平移向量。相比于旋转，平移部分只需把这个平移量加到旋转之后的坐标上。

二、变换矩阵

上面的式子完整的表达了欧式空间的旋转和平移，但是这里的变换关系不是线性关系。因此我们引入了齐次坐标和变换矩阵。
$$\left[\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{c}Rt\\ 0^{T}1\end{array}\right]\left[\begin{array}{c}a\\ 1\end{array}\right]\triangleq T\left[\begin{array}{c}a\\ 1\end{array}\right]$$

我们把一个三维向量的末尾添加1，变成了四维向量，称为齐次坐标。 对于这个四维向量，我们可以把旋转和平移写在一个矩阵里面，使得整个关系变成了线性关系。该式中，矩阵$T$称为变换矩阵(Transform Matrix)。我们暂时用 $\tilde{a}$表示$a$的齐次坐标。转换成齐次坐标:
$$\tilde{x}=[x,y,z,w{]}^T=[x/w,y/w,z/w,1{]}^T$$

变换矩阵T，又称为特殊欧式群(special euclidean group)。

$$SE(3)=\left\{\begin{array}{c}T=\left[\begin{array}{c}Rt\\ 0^{T}1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|R\in SO(3),t\in {\mathbb{R}}^3\end{array}\right\}$$

$T$矩阵的逆表示一个反向变换:
$$T^{-1}=\left[\begin{array}{c}{R}^T-R^{T}t\\ 0^{T}1\end{array}\right]$$

如果将平移和旋转放在一个矩阵中，就形成了变换矩阵SE(3)。

三、欧拉角

变换矩阵描述一个六自由度的三维刚体运动，但矩阵表示方式至少有以下几个缺点:

1. SO (3) 的旋转矩阵有九个量， 但一次旋转只有三个自由度。 因此这种表达方式是冗余的。同理， 变换矩阵用十六个量表达了六自由度的变换。
2. 旋转矩阵自身带有约束：它必须是个正交矩阵，且行列式为1。变换矩阵也是如此。当我们想要估计或优化一个旋转矩阵/变换矩阵时，这些约束会使得求解变得更困难。

任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量，称为旋转向量(或轴角，Axis-Angle)。这种表示法只需一个三维向量即可描述旋转。

旋转向量和旋转矩阵的转换关系:

旋转向量到旋转矩阵，罗德里格斯公式(Rodrigues’s Formula)

$$R=cos\theta I+(1-cos\theta )n{n}^T+sin\theta n^{\wedge }$$

符号$\wedge$ 是向量到反对称的转换符。

旋转矩阵到旋转向量的转化:

$$\begin{array}{rl}tr(R)& =cos\theta tr(I)+(1-cos\theta )tr(n{n}^T)+sin\theta tr(n^{\wedge })\\ & =3cos\theta +(1-cos\theta )\\ & =1+2cos\theta \end{array}$$
所以

$$\theta =arccos(\frac{tr(R)-1}{2})$$

而欧拉角则提供了一种非常直观的方式来描述旋转——它使用了三个分离的转角，把一个旋转分解成三次绕不同轴的旋转。

1. 绕物体的Z轴旋转，偏航角yaw
2. 绕物体的Y轴旋转，俯仰角pitch
3. 绕物体的X轴旋转，翻滚角roll

此时，我们可以使用$[r,p,y{]}^T$这样一个三维的向量描述任意旋转。欧拉角的一个重大缺点是会碰到著名的万向锁问题(Gimbal Lock)：在俯仰角为$\pm 90^{\circ }$时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度(由三次 旋转变成了两次旋转)。这被称为奇异性问题，在其他形式的欧拉角中也同样存在。 在SLAM中不直接使用欧拉角表达姿态，但若你想验证自己算法是否有错时，转换成欧拉角能够快速辨认结果的正确与否。

四、四元数

旋转矩阵用九个量描述三自由度的旋转，具有冗余性。欧拉角和旋转向量是紧凑的，但具有奇异性。在表达三维空间旋转时，有一种类似于复数的代数：四元数(Quaternion)。它既是紧凑的，也没有奇异性。 一个四元数$q$拥有一个实部和三个虚部。
$$q=q_0+q_{1}i+q_{2}j+q_{3}k$$
其中$i,j,k$是四元数的三个虚部。满足下列关系式:

$$\{\begin{array}{rl}& i^2=j^2=k^2=-1\\ & ij=k,ji=-k\\ & jk=i,kj=-i\\ & ki=j,ik=-j\end{array}$$
有时人们也用一个标量和一个向量来表达四元数:
$$q=[s,v],s=q_0\in \mathbb{R},v=[q_1,q_2,q_3{]}^T\in {\mathbb{R}}^3$$

这里，$s$称为四元数的实部，而$v$称为它的虚部。如果一个四元数虚部为0，称之为实四元数。反之， 若它的实部为0，称之为 虚四元数。
在复数中，乘以$i$应该对应着旋转180度，这样才能保证$ij=k$的性质。而 $i^2=-1$，意味着绕$i$轴旋转360度后，你得到了一个相反的东西。这个东西要旋转两周才会和它原先的样子相等。

旋转向量和四元数的关系:
$$q=[cos\frac{\theta }{2},n_{x}sin\frac{\theta }{2},n_{y}sin\frac{\theta }{2},n_{z}sin\frac{\theta }{2}{]}^T$$

四元数转换为旋转向量和夹角:
$$\{\begin{array}{rl}& \theta =2arccos{q}_0\\ & [n_x,n_y,n_z{]}^T=[q_1,q_2,q_3{]}^T/sin\frac{\theta }{2}\end{array}$$

四元数转换为旋转矩阵:

反之，四元数转换为旋转向量和夹角:
$$q_0=\frac{\sqrt{tr(R)+1}}{2},q_1=\frac{m_{23}-m_{32}}{4q_0},q_2=\frac{m_{31}-m_{13}}{4q_0},q_3=\frac{m_{12}-m_{21}}{4q_0}$$

在四元数中，任意的旋转都可以由两个互为相反数的四元数表示。

下面是四元数的计算:
假设有两个四元数$q_a,q_b$ 表示为$[s_a,v_a],[s_b,v_b]$
$q_a=s_a+x_{a}i+y_{a}j+z_{a}k,q_b=s_b+x_{b}i+y_{b}j+z_{b}k$

1. 加法

$$q_a\pm q_b=[s_a\pm s_b,v_a\pm v_b]$$

2. 乘法

$$\begin{array}{rl}{q}_a{q}_b=& s_a{s}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b\\ & +(s_a{x}_b+x_a{s}_b+y_a{z}_b-z_a{y}_b)i\\ & +(s_a{y}_b-x_a{z}_b+y_a{s}_b+z_a{x}_b)j\\ & +(s_a{z}_b+x_a{y}_b-y_b{x}_a+z_a{s}_b)k\end{array}$$

如果写成向量形式并利用内外积运算:
$$q_a{q}_b=[s_a{s}_b-v_a^T{v}_b,s_a{v}_b+s_b{v}_a+v_a\times v_b]$$

3.共轭

四元数的共轭是把虚部取成相反数:
$$q_a^{\ast }=s_a-x_{a}i-y_{a}j-z_{a}k=[s_a,-v_a]$$

四元数共轭与自己本身相乘，会得到一个实四元数，其实部为模长的平方:
$$q^{\ast }q=q{q}^{\ast }=[s_a^2+v^{T}v,0]$$

4.模长

四元数的模长定义为:
$$||q_a||=\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}$$

可以验证，两个四元数乘积的模即为模的乘积。这保证单位四元数相乘后仍是单位四元数。
$$||q_a{q}_b||=||q_a||||q_b||$$

5.逆

一个四元数的逆为:
$$q^{-1}=q^{\ast }/||q|{|}^2$$
四元数和自己的逆的乘积为实四元数的1
$$q{q}^{-1}=q^{-1}q=1$$
如果$q$为单位四元数，逆和共轭就是同一个量。

$$(q_a{q}_b{)}^{-1}=q_b^{-1}{q}_a^{-1}$$

6.数乘和点乘

数乘:
$$kq=[ks,kv]$$

点乘:
$$q_a\cdot q_b=s_a{s}_b+x_a{x}_{b}i+y_a{y}_{b}j+z_a{z}_{b}k$$

7.用四元数表示旋转

把三维空间点用一个虚四元数来描述:
$$p=[0,x,y,z]=[0,v]$$
用四元数q表示这个旋转:
$$q=[cos\frac{\theta }{2},nsin\frac{\theta }{2}]$$

那么旋转之后的点$p^{\prime }$可以表示为:
$$p^{\prime }=qp{q}^{-1}$$

五、相似、仿射、射影变换

1. 相似变换
   $$T_s=\left[\begin{array}{rl}& sRt\\ & 0^{T}1\end{array}\right]$$
   其中$s$是缩放因子。

2. 仿射变换
   $$T_A=\left[\begin{array}{rl}& At\\ & 0^{T}1\end{array}\right]$$
   仿射变换只要求A是一个可逆矩阵，而不必是正交矩阵。仿射变换也叫正交投影。经过仿射变换之后，立方体就不再是方的了，但是各个面仍然是平行四边形。

3. 射影变换
   $$T_P=\left[\begin{array}{rl}& At\\ & a^{T}v\end{array}\right]$$
   它左上角为可逆矩阵A，右上为平移t，左下缩放$a^T$。

最后各种变换总结如下: