视觉SLAM十四讲学习笔记-第三讲-旋转矩阵和Eigen库

专栏系列文章如下：

视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-开发环境搭建_goldqiu的博客-CSDN博客

第三讲：三维空间刚体运动

这章主要是讲一个刚体在三维空间中的运动是如何描述的。同时还介绍线性代数库Eigen。它提供了C++中的矩阵运算，并且它的 Geometry 模块还提供了四元数等刚体运动的描述。

三维空间刚体运动描述方法之一：旋转矩阵

三维空间由三个轴组成，所以一个空间点的位置可以由三个坐标指定。考虑刚体，它不光有位置，还有自身的姿态。相机可以看成三维空间的刚体，相机位置是指相机在空间中的哪个地方，而姿态则是指相机的朝向，合起来称位姿。

点和向量，坐标系：

点：点是空间中的基本元素，没有长度，没有体积。

向量：把两个点连接起来，就构成了向量。向量可以看成从某点指向另一点的一个箭头。不要把向量与它的坐标两个概念混淆。一个向量是空间当中的一样东西，并不需要和若干个实数相关联的。只有当我们指定这个三维空间中的某个坐标系时，才可以谈论该向量在此坐标系下的坐标，也就是找到若干个实数对应这个向量。（就是向量没有坐标系也在那，有了坐标系向量就有了坐标）

用线性代数的知识来说，三维空间中的某个点的坐标也可以用线性空间R3来描述。假设在这个线性空间内，有该空间的一组基 (e1,e2,e3)，那么，任意向量a在这组基下就有一个坐标，这里 (a1,a2,a3)T 称为a在此基下的坐标。公式如下：

注：1.这里基就是张成这个空间的一组线性无关的向量。

2.坐标的具体取值，一是和向量本身有关，二是和坐标系（基）的选取有关。

3.坐标系通常由3个正交的坐标轴组成，非正交的很少见。通常使用右手系，给定x和y轴时，z 轴就可以通过右手法则由x × y定义出来。左手系的第3个轴与右手系的方向相反。

向量内积：

公式如下：

注：内积可以描述向量间的投影关系。指向量间夹角。

向量外积：

外积的结果是一个向量，它的方向垂直于这两个向量，大小为 |a||b|sin〈a,b〉，是两个向量张成的四边形的有向面积。

对于外积运算，我们引入∧符号，把a写成一个矩阵，事实上是一个反对称矩阵（Skew-symmetric matrix），你可以将∧记成一个反对称符号。这样就把外积a × b写成了矩阵与向量的乘法a∧b，把它变成了线性运算。这意味着任意向量都对应着唯一的一个反对称矩阵，反之亦然。

注：1.反对称矩阵满足AT = -A

2.向量和加减法、内外积，即使在不谈论它们的坐标时也可以计算。例如，虽然内积在有坐标时，可以用两个向量的分量乘积之和表达，但是即使不知道它们的坐标时，也可以通过长度和夹角来计算二者的内积。所以两个向量的内积结果和坐标系的选取是无关的。

坐标系间的欧氏变换：

考虑运动的机器人，常见的做法是设定一个惯性坐标系（或者叫世界坐标系），可以认为它是固定不动的。相机或机器人则是一个移动坐标系。相机视野中某个向量p，它在相机坐标系下的坐标为pc，而在世界坐标系下看，它的坐标为pw，那么，这两个坐标之间是如何转换的呢？需要先得到该点针对机器人坐标系的坐标值，再根据机器人位姿变换到世界坐标系中。

两个坐标系之间的运动由一个旋转加上一个平移组成，这种运动称为刚体运动。相机运动就是一个刚体运动。刚体运动过程中，同一个向量在各个坐标系下的长度和夹角都不会发生变化。此时，我们说手机坐标系到世界坐标之间，相差了一个欧氏变换（Euclidean Transform）。对于同一个向量 p，它在世界坐标系下的坐标 pw 和在相机坐标系下的坐标 pc 是不同的。这个变换关系由变换矩阵T来描述。

欧氏变换由旋转和平移组成。首先考虑旋转，设某个单位正交基 (e1, e2, e3) 经过一次旋转变成了 (e1', e2', e3') ，那么，对于同一个向量 a（该向量并没有随着坐标系的旋转而发生运动），它在两个坐标系下的坐标为[a1, a2, a3]T和[a1', a2', a3']T。因为向量本身没变，根据坐标的定义，有：

为了描述两个坐标之间的关系，我们对上述等式的左右两边同时左乘

那么左边的系数就变成了单位矩阵，把中间的矩阵拿出来，定义成一个矩阵R。

这个矩阵由两组基之间的内积组成，刻画了旋转前后同一个向量的坐标变换关系。只要旋转是一样的，那么这个矩阵也是一样的。可以说，矩阵R描述了旋转本身。因此称为旋转矩阵（Rotation matrix）。该矩阵各分量是两个坐标系基的内积，由于基向量的长度为1，所以实际上是各基向量的夹角之余弦。所以这个矩阵也叫方向余弦矩阵（Direction Cosine matrix）。

旋转矩阵有一些特别的性质。它是一个行列式为1的正交矩阵。反之，行列式为1的正交矩阵也是一个旋转矩阵。以把 n 维旋转矩阵的集合定义如下：

SO(n) 是特殊正交群（Special Orthogonal Group）的意思。这个集合由n维空间的旋转矩阵组成，特别地，SO(3) 就是指三维空间的旋转。通过旋转矩阵，我们可以直接谈论两个坐标系之间的旋转变换，而不用再从基开始谈起。

注：由于旋转矩阵为正交矩阵，它的逆（即转置）描述了一个相反的旋转。

在欧氏变换中，除了旋转之外还有平移。考虑世界坐标系中的向量a，经过一次旋转（用R描述）和一次平移t后，得到了a′，把旋转和平移合到一起，有：a′ = Ra + t. t称为平移向量。平移部分只需把平移向量加到旋转之后的坐标上。

通过上式，我们用一个旋转矩阵R和一个平移向量t完整地描述了一个欧氏空间的坐标变换关系。定义坐标系1、坐标系2，那么向量a在两个系下坐标为 a1, a2 ，完整的写法：a1 = R12a2 + t12.

R12 是指把坐标系2的向量变换到坐标系1中。同理，如果我们要表达从1到2的旋转矩阵时，就写成R21。关于平移t12，实际对应的是坐标系1原点指向坐标系2原点的向量，在坐标系1下取的坐标，把它记作从1到2的向量。但是反过来的t21，即从2指向1的向量在坐标系2下的坐标，并不等于−t12，这里和两个系的旋转还有关系。从向量层面来看，它们确实是反向的关系，但这两个向量的坐标值并不是相反数。

注：1.正交矩阵即逆为自身转置的矩阵。

2.行列式为1是人为定义的，实际上只要求它的行列式为 ±1，但行列式为 −1 的称为瑕旋转，即一次旋转加一次反射。

变换矩阵与齐次坐标：

上面的变换关系不是一个线性关系。这样的形式在变换多次之后会显得很繁杂。因此，我们引入齐次坐标和变换矩阵，

这是一个数学技巧：我们在一个三维向量的末尾添加 1，将其变成了四维向量，称为齐次坐标。对于这个四维向量，我们可以把旋转和平移写在一个矩阵里面，使得整个关系变成线性关系。该式中，矩阵T称为变换矩阵（Transform Matrix）。依靠齐次坐标和变换矩阵，两次变换的叠加就可以有很好的形式：

b = T1a, c = T2b ⇒ c = T2T1a.

变换矩阵T具有比较特别的结构：左上角为旋转矩阵，右侧为平移向量，左下角为0 向量，右下角为 1。这种矩阵又称为特殊欧氏群（Special Euclidean Group），

与 SO(3) 一样，求解该矩阵的逆表示一个反向的变换：

同样，我们用T12 这样的写法来表示从2到1的变换。

齐次坐标和非齐次坐标之间的转换非常容易，在C++程序中可以使用运算符重载来完成这个功能，保证在程序中看到的运算是统一的。

总结：

坐标系之间的运动由欧氏变换描述，它由平移和旋转组成。旋转可以由旋转矩阵 SO(3) 描述，而平移直接由一个R3向量描述。最后，如果将平移和旋转放在一个矩阵中，就形成了变换矩阵 SE(3)。

实践：Eigen

代码在 slambook2/ch3/useEigen

链接：

https://github.com/gaoxiang12/slambook​github.com/gaoxiang12/slambook

Eigen 是一个 C++ 开源线性代数库。它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。

安装：

sudo apt−get install libeigen3−dev

这个库由头文件和库文件组成。Eigen 头文件的默认位置在“/usr/include/eigen3/”中。如果不确定，可以输入以下命令查找：

sudo update db 
locate eigen3

官方主页：

Eigen​eigen.tuxfamily.org/index.php?title=Main_Page

相比于其他库，Eigen 的特殊之处在于，它是一个纯用头文件搭建起来的库。这意味着你只能找到它的头文件，而没有.so 或.a 那样的二进制文件。在使用时，只需引入 Eigen 的头文件即可，不需要链接库文件。

作者给的代码里有很详细的注释，可以去阅读。

注：1.Kdevelop 可能不会提示 C++ 成员运算，这是它做得不够完善导致的。Clion 则会完整地给出提示。

2. Eigen 提供的矩阵和 MATLAB 很相似，几乎所有的数据都当作矩阵来处理。但是，为了实现更好的效率，在 Eigen 中需要指定矩阵的大小和类型。对于在编译时期就知道大小的矩阵，处理起来会比动态变化大小的矩阵更快一些。因此，像旋转矩阵、变换矩阵这样的数据，完全可在编译时期确定它们的大小和数据类型。

3. Eigen 矩阵不支持自动类型提升，这和 C++ 的内建数据类型有较大差异。在 C++ 程序中，我们可以把一个 float 数据和 double 数据相加、相乘，编译器会自动把数据类型转换为最合适的那种。而在 Eigen 中，出于性能的考虑，必须显式地对矩阵类型进行转换。而如果忘了这样做，Eigen 会提示你一个“YOU MIXED DIFFERENT NUMERIC TYPES ...”的编译错误。你可以尝试找一下这条信息出现在错误提示的哪个部分。如果错误信息太长最好保存到一个文件里再找。

4. 同理，在计算过程中也需要保证矩阵维数的正确性，否则会出现“YOU MIXED MATRICES OF DIFFERENT SIZES”错误。若发现 Eigen 出错，你可以直接寻找大写的部分，推测出了什么问题。

5. 可以阅读 Eigen 官网教程：

Eigen: Chapters​eigen.tuxfamily.org/dox-devel/modules.html

来学习更多的Eigen知识。

6. 例程最后一段代码中比较了求逆与求QR分解的运行效率，两种方法比较下差异。