视觉SLAM十四讲笔记-3-02

视觉SLAM十四讲笔记-3-02

参考链接： link，
高翔,张涛,等. 视觉 SLAM 十四讲:从理论到实践[M]. 电子工业出版社, 2019.

3.3 旋转向量和欧拉角

3.3.1 旋转向量

前面 3.1 节、 3.2 节已经介绍了 旋转矩阵、变换矩阵。那描述一个6自由度的三维刚体运动是不是已经足够了尼?
矩阵表示至少存在以下两个缺点：
1.SO(3) 的旋转矩阵有 9 个量，但一次旋转只有 3 个自由度，因此这种表达方式是冗余的。同理，变换矩阵用 16 个量表示 6 自由度的变换， 存在冗余。
2.旋转矩阵自身带有约束，必须是一个正交矩阵，且行列式为1。变换矩阵也是如此。当想估计或者优化一个旋转矩阵或者变换矩阵时，这些约束会使得求解变得很困难。
因此，希望有一种方式能够紧凑地描述旋转和平移。例如，用一个三维向量表达旋转，用一个六维向量表达变换，可行吗？事实上，任意旋转都可以用一个旋转轴和一个旋转角来刻画。
于是，可以使用一个向量，方向与旋转轴一致，长度等于旋转角。这种向量称为旋转向量。同样，对于变换矩阵，可以使用一个旋转向量和一个平移向量即可表达一次变换。这样的变量维数正好是六维。
考虑某个用 R 表示的旋转。如果用旋转向量来表示，假设旋转轴为一个单位长度的向量n，角度为 $\theta$，那么向量 $\theta n$ 也可以描述这个旋转。那么 R 和 $\theta n$ 之间有什么关系吗？
从旋转向量到旋转矩阵的转换过程用罗德里格斯公式表明。
$$R=\cos \theta I+(1-\cos \theta )n{n}^T+\sin \theta n^{\wedge }$$
^为上一节中介绍的反对称符号。
对式子两边取迹，得：
$$\begin{array}{rl}\mathrm{tr}(R)& =\cos \theta \mathrm{tr}(I)+(1-\cos \theta )\mathrm{tr}\left(n{n}^T\right)+\sin \theta \mathrm{tr}\left(n^{\wedge }\right)\\ & =3\cos \theta +(1-\cos \theta )\\ & =1+2\cos \theta \end{array}$$
因此有：
$$\theta =\arccos \frac{\mathrm{tr}(R)-1}{2}$$
关于转轴 n，旋转轴上的向量在旋转后不发生改变，说明：
$$Rn=n$$
因此，转轴 n 是矩阵 R 特征值 1 对应到特征向量。求解此方程，再归一化，就得到了转轴。
这里的两个方程在后续仍将出现，会发现这正是 SO(3)上李群和李代数的对应关系。

3.3.2 欧拉角

无论是旋转矩阵还是旋转向量，它们虽然能描述旋转，但是对于人类来说是非常不直观的。当人们看到一个旋转矩阵或者旋转向量时，很难想象出来这个旋转究竟是什么样的。当它们变换时，也不知道物体时在哪个方向上转动，而欧拉角提供了一种非常直观的方式来描述旋转，使用了3个分离的转角，把每次的旋转分解为3次绕不同轴的旋转。人们很容易理解绕单个轴的旋转过程。
但是，分解的方式有很多种。但是在特定领域内，欧拉角通常有统一的定义方式。欧拉角当中比较常用的一种是使用 “偏航-俯仰-滚转(yaw-pitch-roll)” 3 个角度来描述一个旋转。它等价于ZYX方向的旋转。如下图所示，假设一个刚体的前方(朝向我们的方向为 X 轴，右侧为 Y轴， 上方为 Z 轴)。那么， ZYX转角相当于把任意旋转分解为以下3个轴上的转角：
1.绕物体的 Z 轴旋转，得到偏航角 yaw；
2.绕旋转之后的 Y 轴旋转，得到俯仰角 pitch；
3.绕旋转之后的 X 轴旋转，得到滚转角 roll

下图参考链接： link

此时，可以使用 $[r,p,y{]}^T$ 这样一个三维的向量描述任意旋转。这个向量十分直观，可以从这个向量中想象出旋转的过程。其他的欧拉角也是通过这种方式，把旋转分解到3个轴上，得到一个三维的向量，只不过选用的轴及顺序不一样。
缺点：万向锁问题：在俯仰角为 $\pm 90$ 时 (即第二次旋转时)，第一次旋转和第三次旋转将使用同一个轴，使得系统丢失了一个自由度。(由三次旋转变为两次旋转)。这种称为奇异性问题。在其他形式的欧拉角中也同样存在。
理论上可以证明，只要想用三个实数来表达三维旋转，都会不可避免的碰到奇异值问题。由于这种原理，欧拉角不适用于插值和迭代，往往只用于人机交互中。通常也很少在 SLAM 中直接使用欧拉角表示姿态，同样不会在滤波或者优化中使用欧拉角表达旋转(因为有奇异性)。不过，若想验证自己的算法是否有错，转换成欧拉角能够帮你快速分辨结果是否正确。

3.4 四元数

3.4.1 四元数的定义

旋转矩阵用 9 个量描述 3 自由度的旋转，具有冗余性；欧拉角和旋转向量是紧凑的，但具有奇异性。事实上，我们找不到不带奇异性的三维向量描述方式。
回忆以前学习过的复数。用复数集 C 表示复平面上的向量，而复数的乘法则表示复平面上的旋转。例如，乘上复数 i 相当于逆时针把一个复向量旋转 $90^{\circ }$。类似地，在表达三维空间旋转时，也有一种类似于复数的代数：四元数。四元数既是紧凑的，也没有奇异性。但是四元数不够直观，且运算稍微复杂一些。
欧拉公式：
$$e^{i\theta }=\cos \theta +i\sin \theta$$
四元数的形式：一个四元数 q 拥有一个实部和三个虚部。如：
$$q=q_0+q_{1}i+q_{2}j+q_{3}k$$
其中, i, j, k, 为四元数的三个虚部。这三个虚部满足以下关系式：
$$\{\begin{array}{l}{i}^2=j^2=k^2=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\end{array}$$
如果把 i, j, k 看成三个坐标轴，则它们与自己的乘法和复数是一样的，相互之间的乘法和外积是一样的。也可以用一个标量和一个向量来表达四元数：
$$q=[s,v{]}^T,s=q_0\in R,v=[q_1,q_2,q_3{]}^T$$
s 称为四元数的实部，v称为四元数的虚部。如果一个四元数的虚部为0，则称为实四元数；若实部为0称为虚四元数。
讲了四元数，但是如何用四元数表示三维空间中任意一个旋转。

3.4.2 四元数的运算

四元数和通常的复数一样，可以进行一些列的运算。常见的有四则运算、共轭、求逆、数乘等。
现有两个四元数 $q_0$ 和 $q_1$， 它们的向量表示为 $[s_a,v_a]$，$[s_b,v_b]$，或者原始四元数表示为:
$$\begin{gathered} 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 \end{gathered}$$
那么，其运算可以表示如下：
1.加减法：
$${\mathbf{q}}_a\pm {\mathbf{q}}_b=\left[s_a\pm s_b,{\mathbf{v}}_a\pm {\mathbf{v}}_b\right]$$
2.乘法
乘法是把 $q_a$ 的每一项与 $q_b$ 的每一项相乘，最后相加。
$$\begin{array}{rl}{q}_a{q}_b=& s_a{s}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b\\ & +\left(s_a{x}_b+x_a{s}_b+y_a{z}_b-z_a{y}_b\right)i\\ & +\left(s_a{y}_b-x_a{z}_b+y_a{s}_b+z_a{x}_b\right)j\\ & +\left(s_a{z}_b+x_a{y}_b-y_a{x}_b+z_a{s}_b\right)k\end{array}$$
写成向量的形式，用内外积表示：
$$q_a{q}_b={\left[s_a{s}_b-v_a^T{v}_b,s_a{v}_b+s_b{v}_a+v_a\times v_b\right]}^T$$
由于最后一项外积的存在，通常四元数乘法是不可以交换的。
3.模长
$$\Vert q_a\Vert =\sqrt{s_a^2+x_a^2+y_a^2+z_a^2}$$
两个四元数乘积的模即模的乘积。
$$\Vert q_a{q}_b\Vert =\Vert q_a\Vert \Vert q_b\Vert$$
4.共轭
$${\mathbf{q}}_a^{\ast }=s_a-x_{a}i-y_{a}j-z_{a}k=\left[s_a,-{\mathbf{v}}_a\right]$$
四元数共轭和其本身相乘，会得到一个实四元数，其模等于模长的平方
$$q^{\ast }q=q{q}^{\ast }={\left[s_a^2+v^{T}v,0\right]}^T$$
5.逆
$$q^{-1}=q^{\ast }/{\Vert q_a\Vert }^2$$
$$q{q}^{-1}=q^{-1}q=1$$
若 q 为单位四元数，则有 $q^{-1}=q^{\ast }$。
乘积的逆具有和矩阵的逆相似的性质：
$${\left(q_a{q}_b\right)}^{-1}=q_b^{-1}{q}_a^{-1}$$
6.数乘
$$k\mathbf{q}=[ks,k\mathbf{v}]$$

3.4.3 用四元数表示旋转

假设有一个空间三维点 $p=[x,y,z]\in R^3$，以及一个由单位四元数 q 指定的旋转。三维点 p 经过旋转之后变为 $p^{\prime }$。如果使用矩阵描述，那么有 $p^{\prime }=Rp$。而如果使用四元数描述旋转，它们的关系该如何表达尼？
首先，把三维空间点用一个虚四元数来描述：
$$p=[0,x,y,z{]}^T=[0,v{]}^T$$
相当于把四元数的3个虚部与空间中的3个轴相对应。那么，旋转后的点 $p^{\prime }$ 可以表示为这样的乘积：
$$p^{\prime }=qp{q}^{-1}$$
这里的乘法均为四元数乘法，结果也是四元数。最后把 $p^{\prime }$ 的虚部取出，即得旋转之后点的坐标，并且可以验证，计算结果的实部为0，故为纯虚四元数。

3.4.4 四元数到其他旋转表示的转换

任意单位四元数描述了一个旋转，该旋转也可用旋转矩阵或者旋转向量来描述。现在考察四元数与旋转向量、旋转矩阵之间的转换关系。
四元数乘法可以写成一种矩阵的乘法。设$q=[s,v{]}^T$，那么，定义如下的符号 + 和 $\oplus$ 为：
$$q^{+}=\left[\begin{array}{cc}s& -v^T\\ v& sI+v^{\wedge }\end{array}\right]$$
$$q^{\oplus }=\left[\begin{array}{cc}s& -v^T\\ v& sI-v^{\oplus }\end{array}\right]$$
这两个符号将四元数映射成为了一个 4*4 的矩阵。于是，四元数乘法可以写成矩阵的形式：
$$q_1^{+}{q}_2=\left[\begin{array}{cc}{s}_1& -v_1^T\\ v_1& s_{1}I+v_1^{\wedge }\end{array}\right]\left[\begin{array}{c}{s}_2\\ v_2\end{array}\right]=\left[\begin{array}{c}-v_1^T{v}_2+s_1{s}_2\\ s_1{v}_2+s_2{v}_1+v_1\wedge v_2\end{array}\right]=q_1{q}_2$$
同理可证：
$$q_1{q}_2=q_1^{+}{q}_2=q_2^{\oplus }{q}_1$$
然后，考虑使用四元数对空间点进行旋转的问题。根据前面得到的关系，可以得：
$$p^{\prime }=qp{q}^{-1}=q^{+}p{q}^{-1}=q^{+}{p}^{+}{q}^{-1}=q^{+}{q^{-1}}^{\oplus }p$$
带入两个符号对应的矩阵：
$$q^{+}{q}^{-1^{\oplus }}=\left[\begin{array}{cc}1& 0\\ 0^T& v{v}^T+s^{2}I+2s{v}^{\wedge }+{\left(v^{\wedge }\right)}^2\end{array}\right]$$
这个式子的右下角给出了从四元数到旋转矩阵的变换关系：
$$R=v{v}^T+s^{2}I+2s{v}^{\wedge }+{\left(v^{\wedge }\right)}^2$$
为了得到四元数到旋转向量的转换公式，对上述两侧求迹，得：
$$\begin{gathered} tr(R)=tr(v{v}^T+3s^2+2s.0+tr(v\wedge {)}^2) \\ =v_1^2+v_2^2+v_3^2+3s^2-2(v_1^2+v_2^2+v_3^2) \\ =(1-s^2)+3s^2-2(1-s^2) \\ =4s^2-1 \end{gathered}$$
上一节中推导出旋转向量和旋转矩阵的转换关系：
$$tr(R)=1+2cos\theta$$
则：
$$\theta =\arccos \left(2s^2-1\right)$$
即：
$$\theta =2\arccos s$$
四元数到旋转向量的转换关系：
$$\{\begin{array}{l}\theta =2\arccos q_0\\ {\left[n_x,n_y,n_z\right]}^T=\left[q_1,q_2,q_3\right]/\sin \left(\frac{\theta }{2}\right)\end{array}$$

3.5 相似、仿射、射影变换

除了欧氏变换，3D 空间还存在其他几种变换方式，只不过欧氏变换是最简单的。欧氏变换保持了向量的长度和夹角，相当于把一个刚体原封不动地进行了移动或者旋转，不改变自身的样子。其他的几种变换则会改变它的外形。
1.相似变换
相似变换比欧氏变换多了一个自由度，它允许物体进行均匀缩放，矩阵表示为：
$$T_S=\left[\begin{array}{ll}sR& t\\ 0^T& 1\end{array}\right]$$
缩放因子允许对向量旋转之后，可以在 x,y,z 三个坐标上进行均匀缩放。三维相似变换的集合也叫做相似变换群，记做 $Sim(3)$。
2.仿射变换
$$T_S=\left[\begin{array}{cc}A& t\\ 0^T& 1\end{array}\right]$$
只要求 A 是一个可逆矩阵，而不要求是正交矩阵。
3.射影变换
$$T_S=\left[\begin{array}{cc}A& t\\ a^T& v\end{array}\right]$$
摄影变换形式最为一般。
下表参考： link