解读《视觉SLAM十四讲》，带你一步一步入门视觉SLAM—

本文主要是介绍解读《视觉SLAM十四讲》，带你一步一步入门视觉SLAM—— 第 3 讲三维刚体运动（下），希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

陪女朋友为期五天的农村调研终于结束，终于可以回到大魔都享受城市生活了。在高铁上闲来无事再写点吧！（由于我肩部肌肉劳损，太疼了，所以本篇博客是我口述，女朋友代写的，感谢她的鼎力相助）
　　
　　在上一篇博客中我们介绍了三维刚体运动的旋转矩阵和变换矩阵，下面我要带你解读这一讲的下半部分内容：

旋转向量、欧拉角、四元数；

解读

这一讲中的很多概念都比较抽象，我尽量给大家总结的通俗一些，但是大家还是要自己仔细去理解和体会这些几何变换的意义，有些东西如果实在难以理解，你也别着急，咱们先学会用，慢慢的就理解了。
　　
　　前面我们介绍了旋转矩阵和变换矩阵，它们的矩阵描述分别为：
$\left[\begin{matrix}a_{11}&a_{12}&a_{13}\\ a_{21}&a_{22}&a_{23}\\a_{31}&a_{32}&a_{33} \end{matrix}\right] \tag0$

$\left[\begin{matrix}R_{3*3}&t_{3*1}\\ 0^T&1\end{matrix}\right] \tag1$
　　我们观察（0）式可以发现，在旋转矩阵中一共有9个变量，也就是说旋转矩阵有9个自由度，而实际上我们空间当中的一次旋转变换只需要3个自由度，也就是3个变量，所以我们使用旋转矩阵表示旋转变换时，额外引入了6个变量，就会造成表达的冗余。
　　
　　我们再观察（１）式，在变换矩阵中一共有16个量，实际上一次変换只有6个自由度，也就只需要6个变量，那么变换矩阵实际也是冗余的。
　　
　　另外一方面，旋转矩阵和变换矩阵都是正交矩阵，并且行列式的值为1，这在运算和优化中就会引入额外的限制，因为我们的优化结果必须要满足旋转矩阵和变换矩阵的形式。
　　
　　于是我们就想能不能有更好的表达方法去表达旋转和变换，能够一方面变量很紧凑，另一方面不会引入额外的约束。那么我们就要探索下面的一些方法。

旋转向量

因为前面所述的问题，所以作者在书中提出了旋转向量。旋转向量实际上并不难理解的，一个旋转轴 $\vec n=[a_1,a_2,a_3]$ 加上一个旋转角度 $\theta$ 就可以达到旋转的效果。关于旋转向量，它和旋转矩阵一样也表示一种旋转变换，那么它们之间一定可以相互转换，转换可以使用罗德里格斯公式：
$cos\theta \vec I +(1-cos\theta)\vec n \vec n^T+sin\theta \left[\begin{matrix}0&-a_3&a_{2}\\ a_{3}&0&-a_{1}\\-a_{2}&a_{1}&0 \end{matrix}\right]$
同样，旋转矩阵也可以转换为旋转向量，转换公式大家可以参考书中的公式。
　　
　　尽管旋转向量已经比旋转矩阵直观了很多，但是还是不够直观，于是又提出了欧拉角。

欧拉角

我们可以想象一下：对于一个三维坐标系，变换它的姿态，我们可以分为三步，第一步，我们先选择任意一个轴，以它为旋转轴进行旋转，第二步，在剩下的两个轴中选择一个轴作为旋转轴，然后旋转一定的角度，第三步，再以最后一根轴为旋转轴进行旋转，这样的过程就可以把一个坐标系旋转为另外的姿态。
　　
　　欧拉角虽然很直观，但是有很大的局限性，它常常会发生万向锁问题，也称为奇异性问题。当我们选择XYZ轴不同顺序进行依次旋转时常常发生万向锁问题，所以在SLAM中欧拉角的应用并不多，这里不再细说，大家看书就可以。关于万向锁问题大家可以参考这个视频了解一下《“欧拉角旋转”产生“万向锁”的来源，以及如何避免万向锁》
　　
　　我们不妨总结一下前面说的几种表示旋转的方法：欧拉角虽然直观，但是会导致万向锁问题，它很可能就不可用了，所以它在SLAM问题中很少使用；旋转矩阵参数冗余而且还会引入额外的约束，虽然使用会带来一些麻烦，但是至少不会失效；旋转向量使用旋转轴加上旋转角，好像还挺好的，没有带来什么不便和问题。那么还有更好的表示旋转的方法吗？答案是有的！

四元数

我是在学习SLAM的时候，才第一次接触到四元数，当时第一次接触也是一头雾水，尽管现在也没有完全理解，但是算是会用了。
　　
　　前面我们说到的一些表示旋转的方法，或多或少都存在一些问题，所以为了解决这些问题，就有人想到了四元数。这个东西我确实没有深入的去了解，所以靠我现在的理解，还是很难讲清楚，大家还是以《十四讲》为主要去理解四元数。
　　
　　我有一些认识在这里和大家分享一下：我们在学习一些几何的时候总是想直观的去理解一些定理或概念，多数情况下这样是行得通的，但是在四元数上若想直观的理解它的旋转还是有些困难的，如果你实在不能理解四元数，那就暂且将它放一边，就把它当做一个工具使用就行，记住它的一些性质和用法，对于它的理解你就边用边深入。关于四元数我推荐一个英文教程你可以阅读一下《四元数数学基础》提取码：q2x3

最后，无论是四元数、旋转矩阵还还是旋转向量，它们都可以用来描述同一个旋转。我们应该在实际中选择最为方便的形式，而不必拘泥于某种特定的形式。《视觉SLAM十四讲从理论到实践》

扩展阅读：关于的旋转矩阵、旋转向量（轴角）、欧拉角、四元数的优缺点，可以看一下这篇博客：《矩阵、欧拉角、轴-角对、四元数随笔》

实践

我建议你按照书中的代码把实践部分内容多打几遍，不要嫌麻烦，一开始写代码都是这样先重复，再模仿，最后就能自己写了。

代码部分有两个地方还是值得你去注意的：

注意四元数在Eigen中的表示顺序，在书本理论部分四元数的实部在前，形式是(w, x, y, z)，而在Eigen库中形式是(x, y, z, w)。你要注意一下。我在公司实习的时候，有一个工程师就因为这个顺序搞反了，导致一个BUG找了一星期。
有一行代码v_rotated = q*v;，这个是用四元数计算旋转，而书上理论部分使用的计算公式是： $p'=qpq^{-1}$ ，但是在代码中直接用q*v，因为Eigen库重载了*号，所以你以后使用的时候，直接用*就能表示四元数的旋转了。