视觉SLAM十四讲学习记录 第三讲

之前第二讲的地址

第三讲 三维空间刚体运动

3.1 旋转矩阵

3.1.1 点、向量和坐标系

  我们从最基本的内容讲起：点和向量。点就是空间中的基本元素，没有长度，没有体积。把两个点连接起来，就构成了向量。向量可以看成从某点指向另一点的一个箭头。当我们指定一个三维空间中的某个坐标系时，才可以谈论该向量在此坐标系下的坐标，也就是找到若干个实数对应这个向量。
  在线性代数中，三维空间中的某个点的坐标也可以用${\mathbb{R}}^3$来描述。怎么描述呢？假设在这个线性空间内，我们找到该空间的一组基$(e_1,e_2,e_3)$，那么，任意向量$a$在这组基下就有一个坐标：
          $a=[e_1,e_2,e_3]$$\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]$ = $a_1{e}_1+a_2{e}_2+a_3{e}_3.$
  这里的$(a1,a2,a3{)}^T.$称为$a$再次基下的坐标。坐标的具体取值，一是和向量本身有关，二是和坐标系的选取有关。坐标系一般情况下是由3个正交的坐标轴组成的。根据定义方式的不同，坐标系又分为左手系和右手系。左手系的第三个轴与右手系方向相反。大部分3D程序库使用右手系（如OpenGL、3DMax等），也有部分库使用左手系（如Unity、Direct3D等）。
  线性代数中向量内积的定义：
          $a\cdot b=a^{T}b=\sum _{i=1}^3{a}_i{b}_i=|a||b|cos<a,b>.$
  其中$<a,b>$指向量$a,b$的夹角。內积也可以描述向量间的投影关系。而外积则是：
         $a\times b=\Vert \begin{array}{ccc}{e}_1& e_2& e_3\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\Vert =$$\left[\begin{array}{c}{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{array}\right]$$=$$\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]b\stackrel{def}{=}a$^$b.$
  外积的结果是一个向量，他的方向垂直于这两个向量，大小为$|a||b|sin<a,b>,$是两个向量张成的四边形的有向面积。对于外积运算，我们引入^符号，把$a$写成一个矩阵。事实上是一个反对称矩阵，你可以将^记成一个反对称符号。这样就把外积$a\times b$写成了矩阵与向量的乘法$a$^$b$,把它变成了线性运算。这个符号将在后文经常用到。并且此符号是一个一一映射，意味着任何向量都对应着唯一的一个反对称矩阵，反之亦然：
         $a$^ = $\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right].$
  同时，需要提醒的是，在不知道向量的坐标时，虽然无法通过向量的乘积直接计算，也可以通过长度和夹角来计算二者的內积。所以两个向量的內积结果和坐标系的选取是无关的。

3.1.2 坐标系间的欧式变换

  考虑运动的机器人时，常见的做法是设定一个惯性坐标系（或者称为世界坐标系），可以认为它是固定不动的，例如书中p44的图3-1中的$x_w,y_w,z_w$定义的坐标系。同时，相机和机器人是一个移动坐标系例如$x_C,y_C,z_C$定义的坐标系。读者可能会问：相机视野中某个向量$p$，它在相机坐标系下的坐标为$p_c$，而从世界坐标系下看，它的坐标为$p_w$，那么，这两个坐标之间是如何转换的呢？这时，就需要先得到改点针对机器人坐标系的坐标值，再根据机器人位姿变换到世界坐标系中。我们需要一种数学手段来描述这个变换关系。我们可以用一个矩阵$T$来描述它。
  两个坐标系之间的运动由一个旋转加上一个平移组成，这种运动称为刚体运动。相机的运动便是一个刚体运动。刚体运动的过程中，同一个向量在各个坐标系下的长度和夹角都不会发生变化。就像手机从手上滑下掉在地板上一样，在掉落到地板前，它的形状、大小、各个面的角度等性质不会有任何变化。手机坐标系到世界坐标之间，相差了一个欧式变换（Euclidean Transform)。
  欧式变换由旋转和平移组成。我们首先考虑旋转。设某个正交基$(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 }}{]}^T$。因为向量本身没变，所以根据坐标的定义，有
         $[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]$ ,那么左边的系数就变成了单位矩阵，所以：
         $\left[\begin{array}{c}{a}_1\\ \\ a_2\\ \\ a_3\end{array}\right]$ $=\left[\begin{array}{ccc}{e}_1^T{e}_1^{{}^{\prime }}& e_1^T{e}_2^{{}^{\prime }}& e_1^T{e}_3^{{}^{\prime }}\\ & & \\ e_2^T{e}_1^{{}^{\prime }}& e_2^T{e}_2^{{}^{\prime }}& e_2^T{e}_3^{{}^{\prime }}\\ & & \\ e_3^T{e}_1^{{}^{\prime }}& e_3^T{e}_2^{{}^{\prime }}& e_3^T{e}_3^{{}^{\prime }}\end{array}\right]$$\left[\begin{array}{c}{a}_1{}^{\prime }\\ \\ a_2{}^{\prime }\\ \\ a_3{}^{\prime }\end{array}\right]\stackrel{def}{=}R{a}^{{}^{\prime }}.$
将中间的矩阵拿出来，定义成一个矩阵$R$。这个矩阵由两组基之间的內积组成，刻画了旋转前后同一个向量的坐标变换关系。只要旋转是一样的，这个矩阵就是一样的。可以说，矩阵$R$描述了旋转本身。因此，称为旋转矩阵(Rotation Matrix)。同时，该矩阵各分量是两个坐标系基的內积，由于基向量的长度为1，所以实际上是各基向量夹角的余弦值。所以这个矩阵也叫做方向余弦矩阵(Direction Cosine Matrix)。
  旋转矩阵有一些特别的性质。事实上，它是一个行列式为1的正交矩阵。反之，行列式为1的正交矩阵也是一个旋转矩阵。所以，可以将$n维$旋转矩阵的集合定义如下:
         $SO(n)=\{R\in {\mathbb{R}}^{n\times n}|R{R}^T=I,det(R)=1\}.$
  $SO(n)$为特殊正交群的意思。这个集合由$n$维空间的旋转矩阵组成，特别地，$SO(3)$就是指三维空间的旋转。通过旋转矩阵，我们我们可以直接谈论坐标系之间的旋转变换，而不用再从基开始谈起。
  由于旋转矩阵为正交矩阵，它的逆(即转置)描述了一个相反的旋转。按照上面的定义方式，有
         $a^{{}^{\prime }}=R^{-1}a=R^{T}a$
显然$R^T$刻画了一个相反的旋转。
  在欧式变化中，除了旋转还有平移。考虑世界坐标系中的向量$a$，经过一次旋转(用$R$描述)和一次平移$t$后，得到了$a^{{}^{\prime }}$，那么把旋转和平移合到一起，有
         $a^{{}^{\prime }}=Ra+t.$
其中，$t$称为平移向量。相比于旋转，平移部分只需把平移向量加到旋转之后的坐标上。上式，我们通过一个旋转矩阵$R$和一个平移向量$t$完整地描述了一个欧氏空间的坐标变换关系。实际当中，我们会定义坐标系1、坐标系2，那么向量$a$在这两个坐标系下的坐标为$a_1,a_2$，它们之间的关系应该是:
         $a_1=R_{12}{a}_2+t_{12}.$
这里的$R_{12}$是指“把坐标系2的向量变换到坐标系1”中。由于向量乘在这个矩阵的右边，它的下标是从右读到左的。同理，如果我们要表达“从1到2的旋转矩阵”时，就写成$R_{21}$。请读者务必清楚本书的记法，因为不同书籍里写法不同，有的会记成左上/下标，而本文写在右侧下标。
  关于平移$t_{12}$，它实际对应的是坐标系1原点指向坐标系2原点的向量，在坐标系1下取的坐标，所以读者建议读者把它记作“从1到2的向量”。但是反过来的$t_{21}$,即从2指向1的向量在坐标系2下的坐标，却并不等于$-t_{12}$，而是和两个系的旋转还有关系。所以，“我的坐标”这四个字实际上指的是从世界坐标系指向自己坐标系原点的向量，在世界坐标系下渠道的坐标对应到数学符号上。对应到数学符号上，应该是$t_{WC}$的取值。同理，它也不是$-t_{CW}$。

3.1.3 变换矩阵与齐次坐标

  上述的坐标变换关系不是一个线性关系。假设我们进行了两次变换：$R_1,t_1和R_2,t_2$：
         $b=R_{1}a+t_1,c=R_{2}b+t_2$
那么，从$a$到$c$的变换为
         $c=R_2(R_{1}a+t_1)+t_2$
这样的形式在变换多次之后会显得很啰嗦。因此，我们引入齐次坐标和变换矩阵，
         $\left[\begin{array}{c}{a}^{{}^{\prime }}\\ \\ 1\end{array}\right]$ $=\left[\begin{array}{cc}R& t\\ & \\ 0^T& 1\end{array}\right]$$\left[\begin{array}{c}a\\ \\ 1\end{array}\right]\stackrel{def}{=}T$ $\left[\begin{array}{c}a\\ \\ 1\end{array}\right].$
  这是一个数学技巧：我们在一个三维向量的末尾添加1将其变成了一个，将其变成了四维向量，称为齐次坐标。对于这个四维向量，我们可以把旋转和平移写在一个矩阵里，使得整个关系变成线性关系。该式中，矩阵$T$称为变换矩阵。
  我们暂时用$\tilde{a}$表示$a$的齐次坐标。那么依靠齐次坐标和变换矩阵，两次变换的叠加就可以有很好的形式：
         $\tilde{b}=T_1\tilde{a},\tilde{c}=T_2\tilde{b}\Rightarrow \tilde{c}=T_2{T}_1\tilde{a}$
但是区分齐次和非齐次坐标的符号令我们感到厌烦，因此此处只需要在向量末尾添加1或者去掉1。所以，在不引起歧义的情况下，以后我们就直接把它写成$b=Ta$的样子，默认其中进行了齐次坐标的转换。
  关于变换矩阵$T$，它具有比较特别的结构：左上角为旋转矩阵，右侧为平移向量，左下角为$0$向量，右下角为1.这种矩阵又称为特殊欧式群：
         $SE(3)=\{T=\left[\begin{array}{cc}R& t\\ & \\ 0^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}|R\in SO(3)，t\in {\mathbb{R}}^3\}.$
与$SO(3)$一样，求解该矩阵的逆表示一个反向的变化：
         $T^{-1}=\left[\begin{array}{cc}{R}^T& -R^{T}t\\ & \\ 0^T& 1\end{array}\right].$
  同样，我们用$T_{12}$这样的写法来表示从2到1的变换。并且，为了保持符号的简洁，在不引起歧义的情况下，以后不刻意区别齐次坐标与普通坐标的符号，默认使用的是符合运算法则的那一种例如，当我们写$Ta$时，使用的是齐次坐标(不然没法计算)。而写$Ra$时，使用的是非齐次坐标。

3.2 实践Eigen

  这部分的代码在slambook2/ch3/useEigen/eigenMatrix.cpp中，强烈建议去照着原码码一遍。原码对Eigen库中的许多基础函数进行展示，对入门十分友好。其中有几个点本书作者提到：

1. Eigen提供的矩阵和MATLAB很相似，几乎所有的数据都当做矩阵来处理。不过如果在Eigen中指定矩阵的大小和类型，处理起来会比动态变化大小的矩阵更快。因此，对于旋转矩阵、变换矩阵，完全可在编译时确定它们的大小和数据类型。
2. Eigen矩阵不支持自动类型提升，这和C++的智能性有较大的差异。在C++中，我们可以把一个float数据和double数据相加、相乘，编译器会自动把数据类型转换为最合适的那种。而在Eigen中，出于对性能的考虑，必须显式地对矩阵类型进行转换。否则，Eigen会报编译错误。不过相信一般读者都可以从错误中定位到具体位置。
3. 同理，在计算的过程中也需要保证矩阵维数的正确性，否则就会报"YOU MIXED MATRICES OF DIFFERENT SIZES"错误。对于Eigen的错误信息，可以直接搜寻大写字母的部分，推测出了什么问题。

对于作者问到的代码最后比较了求逆与求QR分解的运行效率（我只学过前者），运行效率截图如下（求逆、QR及ldlt）。

显然三个效率差不多都是倍数关系，看得出后两个的效率提升还是巨大的。

3.3 旋转向量和欧拉角

3.3.1 旋转向量

  有了旋转矩阵来描述旋转，有了变换矩阵描述一个6自由度的三维刚体运动还不够。矩阵表示方式至少有以下两个缺点：

1. $SO(3)$的旋转矩阵有9个量，但一次旋转只有3个自由度。因此这种表达方式是冗余的。同理，变换矩阵用16个量表达了6自由度的变换。那么，是否有更紧凑的表示呢？
2. 旋转矩阵自身带有约束：它必须是个正交矩阵，且行列式为1。变换矩阵也是如此。当想估计或优化一个旋转矩阵或变换矩阵时，这些约束会使得求解变得更困难。
     因此，我们希望有一种方式能够紧凑地描述旋转和平移。例如，用一个三维向量表达旋转，用一个六维向量表达变换，是否可行？事实上，任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量称为旋转向量（或轴角/角轴），只需一个三维向量即可描述旋转。同样，对于变换矩阵，我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的变量维数正好是六维。
     考虑某个用$R$表示的旋转。如果用旋转向量来描述，假设旋转轴为一个单位长度的向量$n$，角度为$\theta$，那么向量$\theta n$也可以描述这个旋转。那这两种表达方式之间有什么区别吗？事实上他们间的转换关系并不难。从旋转向量到旋转矩阵的转换过程由罗德里格斯公式（Rodrigue’s Formula表明，由于推导过程比较复杂，不予赘述，只给出转换结果：

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

  符号^是向量到反对称矩阵的转换符。反之，我们也可以计算从一个旋转矩阵到旋转向量的转换。对于转角$\theta$，取两边的迹，有：
         $\begin{array}{rl}tr(R)& =cos\theta tr(I)+(1-cos\theta )tr(n{n}^T)+sin\theta tr(n\stackrel{ˆ}{})\\ & =3cos\theta +(1-cos\theta )\\ & =1+2cos\theta \end{array}$
  因此：
         $\theta =\arccos \frac{tr(R)-1}{2}$
  关于转轴$n$，旋转轴上的向量在旋转后不发生改变，说明：
         $Rn=n$
  因此，转轴$n$是矩阵$R$特征值1对应的特征向量。求解此方程，再归一化，就得到了旋转轴。

3.3.2 欧拉角

  无论是旋转矩阵还是旋转向量，它们虽然能描述旋转，但对人类来说是非常不直观的。当我们看到一个旋转矩阵或旋转向量时，很难想象出这个旋转究竟是什么样的。当它们变换时，我们也不知道物体是在向哪个方向转动。而欧拉角则直观明显地描述了旋转————它使用了3个分离的转角，把一个旋转分解成3次绕绕不同轴的旋转。这样一来，就很容易理解过程了。但是，由于分解方式有多钟，所以欧拉角也存在着众多不同的、易于混淆的定义方法。例如，先绕$X$,再绕$Y$轴，最后绕$Z$轴旋转，就得到了一个$XYZ$轴的旋转。同理，可以定义$ZYX$的旋转方式。如果细分下去吧，则还需要区分每次是绕固定轴旋转的，还是绕旋转之后的轴旋转的，这也会给出不一样的定义方式。
  在欧拉角当中比较常用的一种，便是用“偏航-俯仰-滚转”3个角度来描述一个旋转。它等价于$ZYX$轴的旋转，因此就以$ZYX$为例。假设一个刚体的前方（朝向我们的方向）为$X$轴，右侧为$Y$轴，上方为$Z$轴，。那么，$ZYX$转角相当于把任意旋转分解成以下3个轴上的转角：

1. 绕物体的$Z$轴旋转，得到偏航角。
2. 绕旋转之后的$Y$轴旋转，得到俯仰角。
3. 绕旋转之后的$X$轴旋转，得到滚转角。

  此时，可以使用$[r,p,y{]}^T$这样一个三维的向量描述任意旋转。这个向量十分直观，我们可以从这个向量想象出旋转的过程。其他的欧拉角也是通过这种方式，把旋转分解到3个轴上，得到一个三维的向量，只不过选用的轴及顺序顺序不一样。这里介绍的$rpy$角是比较常用的一种，只有很少的欧拉角种类会有$rpy$这样脍炙人口的名字。不同的欧拉角是按照旋转轴的顺序来称呼的。例如，$rpy$角的旋转顺序是$ZYX$。同样，也有$XYZ、ZYZ$这样的欧拉角——但是它们就没有专门的称呼。
  欧拉角的一个重大缺点是会碰到著名的万向锁问题（Gimbal Lock）：在俯仰角为$\pm 90°$时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由3次旋转变成了2次旋转）。这被称为奇异性问题，在其他形式的欧拉角中也同样存在。理论上可以证明，只要想用3个实数来表达三维旋转，都会不可避免地碰到奇异性问题。由于这种原理，欧拉角不适用于插值和迭代，往往只用于人机交互中。我们因此也很少能在SLAM程序中直接使用欧拉角表达姿态，同样不会在滤波或优化中使用欧拉角表达旋转（因为奇异性的存在）。不过，若你想验证自己的算法是否有错，转换成欧拉角能够帮你快速分辨结果是否正确。在某些主体主要为2D运动的场合（例如扫地机、自动驾驶车辆），我们也可以把旋转分解为三个欧拉角，然后把其中一个拿出来作为定位信息输出。

3.4 四元数

3.4.1 四元数的定义

  旋转矩阵用9个量描述3自由度的旋转，具有冗余性；欧拉角和旋转向量是紧凑的，但具有奇异性。事实上，我们找不到不带奇异性的三维向量描述方式。这有点类似于用两个坐标表示地球表面（如经度和维度），将必定存在奇异性（维度为$\pm 90°$时经度无意义）。
  回忆以前学习过的复数。我们用复数集$\mathbb{C}$表示复平面上的向量，而复数的乘法则表示复平面上的旋转：例如，乘上复数$i$相当于逆时针把一个复向量旋转$90°$。类似地，在表达三维空间旋转时，也有一种类似于复数的代数：四元数。四元数是Hamilton找到的一种扩展的复数。它既是紧凑的，也没有奇异性。但是缺点就是，四元数不够直观，其运算稍复杂些。
  把四元数与复数类比可以帮助你更快地理解四元数。例如，当我们想要将复平面的向量旋转$\theta$角时，可以给这个复向量乘以$e^{i\theta }$。这是极坐标表示的复数，它也可以写成普通的形式，只要使用欧拉公式即可：
         $e^{i\theta }=cos\theta +isin\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 \mathbb{R},v=[q_1,q_2,q_3{]}^T\in {\mathbb{R}}^3$
这里，$s$称为四元数的实部，而$v$称为它的虚部。如果一个四元数的虚部为0，则称为实四元数；反之，若它的实部为0，则成为虚四元数。
  可以用单位四元数表示三维空间中任意一个旋转，不过这种表达方式和复数有着微妙的不同。在复数中，乘以$i$意味着旋转$90°$。这是否意味着四元数中，乘$i$就是绕$i$轴旋转$90°$？那么，$ij=k$是否意味着，先绕$i$旋转$90°$，就等于绕$k$轴$90°$？显然可以想象一下不是。正确的情况是，乘以$i$对应着旋转$180°$，这样才能保证$ij=k$的性质。而$i^2=-1$，意味着绕$i$轴旋转$360°$后得到一个相反的东西。这个东西要旋转两周才会和它原先的样子相等。

3.4.2 四元数的运算

  常见的有四则运算、共轭、求逆、数乘等。下面分别介绍。
  现有两个四元数$q_a,q_b$，它们的向量表示为$[s_a,v_a{]}^T,[s_b,v_b{]}^T$,或者原始四元数表示为
         $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,q_b$的加减运算为

         $q_a\pm q_b=[s_a\pm s_b,v_a\pm v_b{]}^T.$

  2. $乘法$
   乘法是把$q_a$的每一项与$q_b$的每项相乘，最后相加，虚部要按照式(3.21)进行。整理可得

         $\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_a{x}_b+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{]}^T$

在该乘法的定义下，两个实四元数乘积仍是实的，这与复数是一致的。然而，我们注意到，由于最后一项外积的存在，四元数乘法通常是不可交换的，除非$v_a$和$v_b$在${\mathbb{R}}^3$中共线，此时外积项为零。

  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. $共轭$
   四元数的共轭是把虚部取成相反数：

         $q_a^{\ast }=s_a-x_{a}i-y_{a}j-z_{a}k=[s_a,-v_a{]}^T$

   四元数共轭与其本身相乘，会得到一个实四元数，其实部为模长的平方：

         $q^{\ast }q=q{q}^{\ast }=[s_a^2+v^{T}v,0{]}^T.$

  5. $逆$
   一个四元数的逆为
         $q^{-1}=q^{\ast }/\Vert q{\Vert }^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{]}^T.$

3.4.3 用四元数表示旋转

  我们可以用四元数表达对一个点的旋转。假设有一个空间三维点$p=[x,y,z]\in {\mathbb{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\stackrel{ˆ}{}\end{array}\right],q^{\oplus }=\left[\begin{array}{cc}s& -v^T\\ & \\ v& sI-v\stackrel{ˆ}{}\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\stackrel{ˆ}{}\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\stackrel{ˆ}{}{v}_2\end{array}\right]=q_1{q}_2.$
同理亦可证：
         $q_1{q}_2=q_1^{+}{q}_2=q_2^{\oplus }{q}_1$
  然后，考虑使用四元数对空间点进行旋转的问题。根据前面的说法，有

         $\begin{array}{rl}{p}^{{}^{\prime }}& =qp{q}^{-1}=q^{+}{p}^{+}{q}^{-1}\\ & =q^{+}{q}^{-1\oplus }p.\end{array}$
代入两个符号对应的矩阵，得
         $q^{+}(q^{-1}{)}^{\oplus }=\left[\begin{array}{cc}s& -v^T\\ & \\ v& sI+v\stackrel{ˆ}{}\end{array}\right]\left[\begin{array}{cc}s& v^T\\ & \\ -v& sI+v\stackrel{ˆ}{}\end{array}\right]=\left[\begin{array}{cc}1& 0\\ & \\ 0^T& v{v}^T+s^{2}I+2sv\stackrel{ˆ}{}+(v\stackrel{ˆ}{}{)}^2\end{array}\right].$

因为$p^{{}^{\prime }}$和$p$都是虚四元数，所以事实上该矩阵的右下角即给出了从四元数到旋转矩阵的变换关系：

         $R=v{v}^T+s^{2}I+2sv\stackrel{ˆ}{}+(v\stackrel{ˆ}{}{)}^2.$

为了得到四元数到旋转向量的转换公式，对上式两侧求迹，得

         $\begin{array}{rl}tr(R)& =tr(v{v}^T+3s^2+2s\cdot 0+tr((v\stackrel{ˆ}{}{)}^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{array}$

又由式(3.17)得
         $\begin{array}{rl}\theta & =\arccos \frac{tr(R-1)}{2}\\ & =\arccos (2s^2-1)\end{array}.$

即

         $\cos \theta =2s^2-1=2{\cos }^2\frac{\theta }{2}-1,$

所以：
         $\theta =2\arccos s$

至于旋转轴，如果在式(3.38)中用$q$的虚部代替$p$,易知$q$的虚部组成的向量在旋转时是不动的，即构成旋转轴。于是只要将它除掉它的模长，即得。总而言之，四元数到旋转向量的转换公式如下：

         $\{\begin{array}{l}\theta =2\arccos q_0\\ {\left[\begin{array}{c}{n}_x,n_y,n_z\end{array}\right]}^T={\left[\begin{array}{c}{q}_1,q_2,q_3\end{array}\right]}^T/\sin \frac{\theta }{2}\end{array}.$

  至于如何从其他方式转换到四元数，只需把上述步骤倒过来处理即可。在实际编程中，程序库通常会为我们准备好各种形式的转换。无论是四元数、旋转矩阵还是轴角，它们都可以用来描述同一个旋转。我们应该在实际中选择最方便的形式，而不必拘泥于某种特定的形式。

3.5 *相似、仿射、射影变换

  除了欧氏变换，3D空间还存在其他几种变换方式，只不过欧氏变换是最简单的。它们一部分和测量几何有关，因为在之后的讲解中可能会提到，所以先罗列出来。欧氏变换保持了向量的长度和夹角，相当于我们把一个刚体原封不动地进行了移动或旋转，不改变它自身的样子。其他几种变换则会改变它的外形。它们都拥有类似的矩阵表示。

1. $相似变换$
   相似变换比欧氏变换多了一个自由度，它允许物体进行均匀缩放，其矩阵表示为
            $T_s=\left[\begin{array}{cc}sR& t\\ 0^T& 1\end{array}\right].$
   旋转部分多了一个缩放因子$s$，表示我们在对向量旋转之后，可以在$x,y,z$三个坐标上进行均匀缩放。由于含有缩放，相似变换不再保持图形的面积不变。可以想象一个边长为1的立方体通过相似变换后，变成边长为10的样子。三维相似变换的集合也叫作相似变换群，记作Sim(3)。
2. $仿射变换$
   仿射变换的矩阵形式如下：
            $T_A=\left[\begin{array}{cc}A& t\\ 0^T& 1\end{array}\right].$
   与欧氏变换不同的是，仿射变换只要求$A$是一个可逆矩阵，而不必是正交矩阵。仿射变换也叫正交投影。经过仿射变换之后，立方体就不再是方的了，但是各个面仍然是平行四边形。
3. $射影变换$
   射影变换是最一般的变换，它的矩阵形式为
            $T_p=\left[\begin{array}{cc}A& t\\ a^T& v\end{array}\right].$
   它的左上角为可逆矩阵$A$，右上角为平移$t$,左下角为缩放$a^T$。由于采用了齐次坐标，当$v\ne 0$时，我们可以对整个矩阵除以$v$得到一个右下角为1的矩阵；否则得到右下角为0的矩阵。因此，2D的射影变换一共有8个自由度，3D则共有15个自由度。射影变换是现在讲过的变换中，形式最为一般的。从真实世界到相机照片的变换可以看成一个射影变换。读者可以想象一个原本方形的地板砖，在照片中是什么样子：首先他不再是方形的。由于近大远小的的关系，它甚至不是平行四边形，而是一个不规则的四边形。

  我们之后会讲到，从真实世界到相机照片的变换是一个射影变换。如果相机的焦距为无穷远，那么这个变换为仿射变换。

3.6 实践：Eigen几何模块

3.6.1 Eigen 几何模块的数据演示

  Eigen中对各种形式的表达方式总结如下。注意每种类型都有单精度和双精度两种数据类型，而且和之前一样，不能由编译器自动转换。下面以双精度为例，你可以把最后的d改成f，即得到单精度的数据结构。

• 旋转矩阵（3 × 3）：Eigen::Matrix3d。
• 旋转向量（3 × 1）：Eigen::AngleAxisd。
• 欧拉角（3 × 1）：Eigen::Vector3d。
• 四元数（4 × 1）：Eigen::Quaterniond。
• 欧氏变换矩阵（4 × 4）：Eigen::Isometry3d。
• 仿射变换（4 × 4）：Eigen::Affine3d。
• 射影变换（4 × 4）：Eigen::Projective3d。

  参考代码中对应的CMakeLists即可编译此程序。C++文件为slambook2/ch3/useGeometry/useGeometry.cpp。其中演示了如何在程序中转换这几种表达方式。如果想要进一步了解Eigen的几何模块的读者可以点击Eigen参考。
  请读者注意，程序代码通常和数学表示有一些细微的差别。例如，通过运算符重载，四元数和三维向量可以直接计算乘法，但在数学上则需要先把向量转成虚四元数，再利用四元数乘法进行计算，同样的情况也适用于变换矩阵乘三维向量的情况。总体而言，程序中的用法会比数学公式更灵活。

3.6.2 实际的坐标变换例子

  下面我们举一个小例子来演示坐标变换。
例子 设有小萝卜一号和小萝卜二号位于世界坐标系中。记世界坐标系为$W$,小萝卜们的坐标系为$R_1$和$R_2$。小萝卜一号的位姿为$q_1=[0.35,0.2,0.3,0.1{]}^T,t_1=[0.3,0.1,0.1{]}^T$。小萝卜二号的位姿为$q_2=[-0.5,0.4,-0.1,0.2{]}^T,t_2=[-0.1,0.5,0.3{]}^T$。这里的$q$和$t$表达的是$T_{R_k,W},k=1,2,$也就是世界坐标系到相机坐标系的变换关系。现在，小萝卜一号看到某个点在自身的坐标系下坐标为$p_{R_1}=[0.5,0,0.2{]}^T,$求该向量在小萝卜二号坐标系下的坐标。
  这是一个非常简单，但又具有代表性的例子。在实际场景中你经常需要在同一个机器人的不同部分，或者不同机器人之间转换坐标。代码如slambook2/ch3/examples/coordinateTransform.cpp。
  程序输出的答案是$[-0.0309731,0.73499,0.296108{]}^T,$计算过程也十分简单，只需计算
           $p_{R_2}=T_{R_2,W}{T}_{W,R_1}{p}_{R_1}$
即可。注意四元数使用使用之前需要归一化。

3.7 可视化演示

3.7.1 显示运动轨迹

  如果你是第一次接触旋转和平移这些概念，可能会觉得它们的形式看起来很复杂，因为毕竟每种表达方式都可以与其他方式相互转换，而转换公式有时还比较长。虽然旋转矩阵、变换矩阵的数值可能不够直观，但我们可以很容易地把它们画在窗口里。
  本节我们演示两个可视化的例子。首先，假设我们通过某种方式记录了一个机器人的运动轨迹，现在想把它画到一个窗口中。假设轨迹文件存储于trajectory.txt，每一行用下面的格式存储：

           $time,t_x,t_y,t_z,q_x,q_y,q_z,q_w,$

其中，time指该位姿的记录时间，$t$为平移，$q$为旋转四元数，均是以世界坐标系到机器人坐标系记录。下面我们从文件中读取这些轨迹，并显示到一个窗口中。原则上，如果只是谈论“机器人的位姿”，那么你可以使用$T_{WR}$或者$T_{RW}$，事实上它们也只差一个逆而已，这意味着知道其中一个就可以很轻松地得到另一个。如果你想要存储机器人的轨迹，那么可以存储所有时刻的$T_{WR}$或者$T_{RW}$，这并没有太大的差别。
  在画轨迹的时候，我们可以把“轨迹”画成一系列点组成的序列，这和我们想象中的“轨迹”比较相似。严格说来，这其实是机器人（相机）坐标系的原点在世界坐标系中的坐标。考虑机器人坐标系的原点$O_R$，此时的$O_W$就是这个原点在世界坐标系下的坐标：

           $O_W=T_{WR}{O}_R=t_{WR}.$

这正是$T_{WR}$的平移部分。因此，可以从$T_{WR}$中直接看到相机在何处，这也是我们说$T_{WR}$更为直观的原因。因此因此，在可视化程序里，轨迹文件存储了$T_{WR}$而不是$T_{RW}$。
  最后，我们需要一个支持3D绘图的程序库。有许多库都支持3D绘图，如MATLAB，Python的Matplotlib、OpenGL等。在Linux中，一个常见的库是基于OpenGL的Pangolin库，它在支持OpenGL的绘图操作基础之上还提供一些GUI的功能。本书中，我们使用Git的submodule功能管理本书依赖的第三方库。读者可以进入3rdparty文件夹直接安装所需的库，Git保证了使用的版本是一致的。
  具体的代码在slambook2/ch3/examples/plotTrajectory.cpp，其中演示了如何在Pangolin中画出3D的位姿。我们用红、绿、蓝三种颜色画出每个位姿的三个坐标，然后用黑色线将轨迹连起来。其结果图如下所示：

3.7.2 显示相机的位姿

  除了显示轨迹，我们也可以显示3D窗口中相机的位姿。在slambook2/ch3/visualizeGeometry中，我们以可视化的形式演示相机位姿的各种表达方式。当读者用鼠标操作相机时，左侧的方框里会实时显示相机位姿对应的旋转矩阵、平移、欧拉角和四元数，可以看到数据是如何变化的。不过你在运行的过程中可能会遇到一些bug，建议参考该博主的文章visualizeGeometry的问题解决

  部分解决截图：（还有C++11标准改成C++14）

  相机位姿运行截图：

课后习题

1. 验证旋转矩阵是正交矩阵。
   证明：
     设A为n阶方阵
     正交矩阵$\iff$$A^{-1}=A^T\iff A{A}^T=I$
   $R=\left[\begin{array}{ccc}{e}_1^T{e}_1^{{}^{\prime }}& e_1^T{e}_2^{{}^{\prime }}& e_1^T{e}_3^{{}^{\prime }}\\ & & \\ e_2^T{e}_1^{{}^{\prime }}& e_2^T{e}_2^{{}^{\prime }}& e_2^T{e}_3^{{}^{\prime }}\\ & & \\ e_3^T{e}_1^{{}^{\prime }}& e_3^T{e}_2^{{}^{\prime }}& e_3^T{e}_3^{{}^{\prime }}\end{array}\right]$
            $\begin{array}{rl}R{R}^T& =\left[\begin{array}{ccc}{e}_1^T{e}_1^{{}^{\prime }}& e_1^T{e}_2^{{}^{\prime }}& e_1^T{e}_3^{{}^{\prime }}\\ & & \\ e_2^T{e}_1^{{}^{\prime }}& e_2^T{e}_2^{{}^{\prime }}& e_2^T{e}_3^{{}^{\prime }}\\ & & \\ e_3^T{e}_1^{{}^{\prime }}& e_3^T{e}_2^{{}^{\prime }}& e_3^T{e}_3^{{}^{\prime }}\end{array}\right]\left[\begin{array}{ccc}{e}_1^T{e}_1^{{}^{\prime }}& e_2^T{e}_1^{{}^{\prime }}& e_3^T{e}_1^{{}^{\prime }}\\ & & \\ e_1^T{e}_2^{{}^{\prime }}& e_2^T{e}_2^{{}^{\prime }}& e_3^T{e}_2^{{}^{\prime }}\\ & & \\ e_1^T{e}_3^{{}^{\prime }}& e_2^T{e}_3^{{}^{\prime }}& e_3^T{e}_3^{{}^{\prime }}\end{array}\right]\\ & =\left[\begin{array}{ccc}(e_1^T{e}_1^{{}^{\prime }}{)}^2+(e_1^T{e}_2^{\prime }{)}^2+(e_1^T{e}_3^{\prime }{)}^2& e_1^T{e}_2^T(e_1^{\prime 2}+e_2^{\prime 2}+e_3^{\prime 2})& e_1^T{e}_3^T(e_1^{\prime 2}+e_2^{\prime 2}+e_3^{\prime 2})\\ & & \\ e_1^T{e}_2^T(e_1^{\prime 2}+e_2^{\prime 2}+e_3^{\prime 2})& (e_2^T{e}_1^{{}^{\prime }}{)}^2+(e_2^T{e}_2^{\prime }{)}^2+(e_2^T{e}_3^{\prime }{)}^2& e_2^T{e}_3^T(e_1^{\prime 2}+e_2^{\prime 2}+e_3^{\prime 2})\\ & & \\ e_1^T{e}_3^T(e_1^{\prime 2}+e_2^{\prime 2}+e_3^{\prime 2})& e_2^T{e}_3^T(e_1^{\prime 2}+e_2^{\prime 2}+e_3^{\prime 2})& (e_3^T{e}_1^{{}^{\prime }}{)}^2+(e_3^T{e}_2^{\prime }{)}^2+(e_3^T{e}_3^{\prime }{)}^2\end{array}\right]\\ & =\left[\begin{array}{ccc}(e_1^T{)}^2& e_1^T{e}_2^T& e_1^T{e}_3^T\\ & & \\ e_1^T{e}_2^T& (e_2^T{)}^2& e_2^T{e}_3^T\\ & & \\ e_1^T{e}_3^T& e_2^T{e}_3^T& (e_3^T{)}^2\end{array}\right]\end{array}$
   $\because e_1,e_2,e_3$互相垂直
   $\therefore e_1^T,e_2^T,e_3^T$也互相垂直
   $\therefore$ 互相点乘的结果为0
   $R{R}^T=\left[\begin{array}{ccc}(e_1^T{)}^2& 0& 0\\ & & \\ 0& (e_2^T{)}^2& 0\\ & & \\ 0& 0& (e_3^T{)}^2\end{array}\right]$
   又$\because (e_1,e_2,e_3)$为单位正交基
    $\therefore R{R}^T=\left[\begin{array}{ccc}1& 0& 0\\ & & \\ 0& 1& 0\\ & & \\ 0& 0& 1\end{array}\right]=I$
   得证
2. *寻找罗德里格斯公式的推导过程并加以理解。
   参考CSDN博主默_存的文章
3. 验证四元数旋转某个点后，结果是一个虚四元数（实部为零），所以仍然对应到一个三维空间点。
              $\begin{array}{rl}{p}^{\prime }& =qp{q}^{-1}\\ p& ={\left[\begin{array}{c}0,x,y,z\end{array}\right]}^T={\left[\begin{array}{c}0,v\end{array}\right]}^T\\ q& =q_0+q_{1}i+q_{2}j+q_{3}k={\left[\begin{array}{c}{q}_0,v_q\end{array}\right]}^T\\ q^{-1}& =\frac{q^{\ast }}{\Vert q{\Vert }^2}=\frac{{\left[\begin{array}{c}{q}_0,-v_q\end{array}\right]}^T}{\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}}\\ p^{\prime }& =qp{q}^{-1}={\left[\begin{array}{c}-v_q^{T}v,q_{0}v+v_q\times v\end{array}\right]}^T\frac{{\left[\begin{array}{c}{q}_0,-v_q\end{array}\right]}^T}{\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}}\end{array}$
   可以得到p的实部为

           $\begin{array}{rl}& -v_q^{T}v{q}_0-(q_{0}v+v_q\times v{)}^T(-v_q)\\ & =q_0(v{v}_q-v_q^{T}v)+(v_q\times v{)}^T{v}_q\\ & =(v_q\times v{)}^T{v}_q\\ & =(v_q\times v)v_q\\ & \end{array}$