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

本文主要是介绍解读《视觉SLAM十四讲》，带你一步一步入门视觉SLAM—— 第４讲李群与李代数 (下)，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

在上一篇解读中《解读《视觉SLAM十四讲》，带你一步一步入门视觉SLAM—— 第４讲李群与李代数 (上)》，我们先介绍了李群的定义，知道了我们前面介绍的旋转矩阵集合就是一个李群，然后我们通过一些推导得到了 $exp(\boldsymbol\phi^{\wedge})$ ，知道了旋转矩阵可以用李代数（向量）的形式去表示。
　　
　　这一讲我将带你解读李群和李代数的指数和对数运算，以及李代数的求导与扰动模型。

解读

指数映射与对数映射

前面我们经过不懈的努力找到了旋转矩阵另外的表示方法， $exp(\boldsymbol\phi^{\wedge})$ ，但是我们并不知道这个 $exp(\boldsymbol\phi^{\wedge})$ 应该怎么计算。下面我们就跟着作者的思路一起学习一下。
　　
　　我要再次跟你强调一下 $\boldsymbol\phi$ 是一个四元数，它实际上也是一个三维向量，这一点请你别忘记了！同时你也别忘记 $\boldsymbol\phi^{\wedge}$ 表示的是反对称矩阵，虽然前面已经说过了，但是再强调一下。
　　首先我们对 $exp(\boldsymbol\phi^{\wedge})$ 写成泰勒级数的方式： $exp(\boldsymbol\phi^{\wedge})=\sum_{n=0}^{\infty} \frac 1 {n!}(\boldsymbol\phi^{\wedge})^n\tag 0$ 因为 $\boldsymbol\phi$ 是一个向量，我们可以把它写成 $\boldsymbol\phi=\theta\vec a$ ， $\vec a$ 是和 $\boldsymbol\phi$ 方向相同的单位向量， $\theta$ 是 $\boldsymbol\phi$ 的模长。
　　书中说到，对于 $\vec a$ ，有两条性质， $\vec a^{\wedge}*\vec a^{\wedge} = \vec a * \vec a^T - I \tag1$ $\vec a^{\wedge}*\vec a^{\wedge}*\vec a^{\wedge} = -\vec a^{\wedge} \tag2$ 然后我们就可以把（０）式进行展开了，并且应用上（１）（２）的性质 $exp(\boldsymbol\phi^{\wedge})=exp(\theta\vec a^{\wedge})=\sum_{n=0}^{\infty} \frac 1 {n!}(\theta\vec a^{\wedge})^n \tag3$ 上式（３）拆开之后进行化简（具体过程请看书本），可以得到如下式子： $exp(\boldsymbol\phi^{\wedge})=exp(\theta\vec a^{\wedge})=cos\theta I+(1-cos\theta)\vec a\vec a^T+sin\theta\vec a^{\wedge}\tag4$ 握草，（４）式不就是我们第三讲说的罗德里格斯公式吗！？怎么搞了一大圈又回到原点了！？
　　
　　我们上面的推导没有问题啊！那我们来仔细回味一下这些推导过程到底得到了什么结论。我们首先把旋转矩阵转换成了对应的李代数形式，即就是 $exp(\boldsymbol\phi^{\wedge})$ ，然后求得了这个指数映射关系实际上就是罗德里格斯公式。我们上一讲运用罗德里格斯公式的时候，主要是用来把旋转向量转换成旋转矩阵。我们仔细观察（４）式， $\theta\vec a$ 不就等于是旋转向量嘛！那是不是可以这样认为，旋转矩阵对应的李代数实际上就是旋转向量组成的空间？答案是：肯定的！
　　
　　上面的思维转换有一点难，我们不妨再不厌其烦的总结一下：我们一开始从 $exp(\boldsymbol\phi^{\wedge})$ 入手，等式左边的 $R$ 是李群，等式右边的 $\boldsymbol\phi$ 是它对应的李代数，然后去探索这个指数映射的解法。惊奇的发现这个指数映射其实就是通过罗德里格斯公式变换过去的。而罗德里格斯公式也是将旋转向量转换成旋转矩阵的公式。所以我们可以得出一个结论，旋转矩阵群对应的李代数，实际上就是旋转向量组成的集合。也就是说旋转矩阵的李代数就是旋转向量。哇，看来前面这么多内容只是将第三讲的内容升华到了更高的理论层上，实际上也并没有什么新的东西产生。于是乎，我们得到了一个结论，第四讲一大部分东西都是忽悠人的，现在看来确实是这样的，哈哈，开个玩笑。
　　
　　当我们明白了旋转矩阵的指数映射之后，理解变换矩阵 $T$ 的指数映射似乎也就顺理成章了，所以不再解读，大家看书即可！如果上面的内容你还觉得思维转不过来，请结合书本多看几遍。
　　
　　既然有旋转向量到旋转矩阵的指数映射，那也就有旋转矩阵到旋转向量的对数映射，书中直接给出了对数映射的公式，大家请参考课本《十四讲》！

BCH公式与近似形式

还记得我们一开始说的，为什么要引入李群和李代数吗？目的就是为了后面在优化过程中就行求导。
　　
　　我们说旋转矩阵不满足加法封闭，那我们通过指数映射 $exp(\boldsymbol\phi^{\wedge})$ 变换到李代数的形式，在李代数的形式下，我们是否可以求导呢？让我们来探讨一下：
　　
　　如果我们想要能求导，那就必须能够对加法运算封闭，如果我们旋转矩阵相乘对应到李代数是相加，那我们就可以通过李代数的方式进行求导。也就是说要下式(5)成立 $exp(\boldsymbol\phi_1^{\wedge})exp(\boldsymbol\phi_2^{\wedge})=exp(\boldsymbol\phi_1^{\wedge}+\boldsymbol\phi_2^{\wedge})\tag5$ $ln(exp(\boldsymbol\phi_1^{\wedge})exp(\boldsymbol\phi_2^{\wedge}))=\boldsymbol\phi_1^{\wedge}+\boldsymbol\phi_2^{\wedge}\tag6$ 显然对于标量，（５）（６）式是成立的，但是对于矩阵（５）（６）式不成立。
　　等式（６）在对矩阵进行运算的时候，是满足 BCH公式， $ln(exp(\boldsymbol\phi_1^{\wedge})exp(\boldsymbol\phi_2^{\wedge}))=\boldsymbol\phi_1^{\wedge}+\boldsymbol\phi_2^{\wedge}+\frac 1 2[\boldsymbol\phi_1^{\wedge},[\boldsymbol\phi_1^{\wedge},\boldsymbol\phi_2^{\wedge}]]+...$ ，BCH告诉我们，在做矩阵运算时会产生余项。
　　考虑到，SLAM是实际问题，所以我们可以做一些近似，我们把小量中二次以上的项都忽略掉。此时（6）式可以近似等于如下表达： $ln(exp(\boldsymbol\phi_1^{\wedge})exp(\boldsymbol\phi_2^{\wedge}))^{\vee}\approx \begin{cases}J_l(\boldsymbol\phi_2)^{-1}\boldsymbol\phi_1+\boldsymbol\phi_2 当\boldsymbol\phi_1为小量\\J_r(\boldsymbol\phi_1)^{-1}\boldsymbol\phi_2+\boldsymbol\phi_1 当\boldsymbol\phi_２为小量\end{cases}\tag 7$ 通过（７）式我们就可以得到，李群乘法（旋转矩阵乘法）与李代数加法（旋转向量）的关系了。
　　现在我们来考虑一个实际情况，某一个旋转 $Ｒ$ ，对应的李代数 $\boldsymbol\phi$ ，如果在它左边乘上一个微小的旋转 $\Delta R$ ， $\Delta R$ 对应的李代数是 $\Delta\boldsymbol\phi$ 。根据（７）式中的第一个等式，可以得到： $exp(\Delta\boldsymbol\phi)exp(\boldsymbol\phi)=exp((\boldsymbol\phi+J_l^{-1}\Delta\boldsymbol\phi)^{\wedge})\tag8$ 上式（８）就告诉我们了，旋转向量加法和旋转矩阵乘法的关系。注意：我并没有介绍 $J$ 怎么计算，请看书中给出的公式。
　　反过来，当我们做李代数的加法时，对应到李群上面是怎样的运算呢？可以由下式给出： $exp((\boldsymbol\phi+\Delta\boldsymbol\phi)^{\wedge})=exp((J_l^{-1}\Delta\boldsymbol\phi)^{\wedge})exp(\boldsymbol\phi^{\wedge})=exp(\boldsymbol\phi^{\wedge})exp((J_r^{-1}\Delta\boldsymbol\phi)^{\wedge})\tag{9}$

对位姿有关函数的求导问题

对于旋转矩阵和变换矩阵，它们对加法都没有良好的定义，所以对姿态有关的函数求导，只能通过李代数进行。关于用李代数解决求导问题，有两种思路：

用李代数表示姿态，然后根据李代数加法对李代数进行求导；
对李群左乘或者右乘微小扰动，然后对该扰动求导。

我们分别来看这两种求导方法的区别：

李代数的求导

假设我们要对前后两个时刻的姿态求导数，因为旋转矩阵没有加法，所以我们就对旋转矩阵的李代数就行求导，如下式： $\frac {\partial(exp(\boldsymbol\phi^{\wedge}))}{\partial\boldsymbol\phi}\tag{10}$ 上式根据导数定义进行求导： $\frac{\partial(exp(\boldsymbol\phi^{\wedge}))}{\partial\boldsymbol\phi} = \lim_{\delta\boldsymbol\phi \to 0}\frac{exp((\boldsymbol\phi+\delta\boldsymbol\phi)^{\wedge})-exp(\boldsymbol\phi^{\wedge})}{\delta\boldsymbol\phi}$ 对上式使用BCH线性近似，得到： $=\lim_{\delta\boldsymbol\phi \to 0}\frac{exp((J_l\delta\boldsymbol\phi)^{\wedge})exp(\boldsymbol\phi^{\wedge})-exp(\boldsymbol\phi^{\wedge})}{\delta\boldsymbol\phi}$ 对上式的 $exp((J_l\delta\boldsymbol\phi)^{\wedge})$ 进行泰勒展开取常数项和一次项，得到下式： $\approx\lim_{\delta\boldsymbol\phi \to 0}\frac{exp(I+(J_l\delta\boldsymbol\phi)^{\wedge})exp(\boldsymbol\phi^{\wedge})-exp(\boldsymbol\phi^{\wedge})}{\delta\boldsymbol\phi}$ $=\lim_{\delta\boldsymbol\phi \to 0}\frac{(J_l\delta\boldsymbol\phi)^{\wedge}exp(\boldsymbol\phi^{\wedge})}{\delta\boldsymbol\phi}$ 上式中，将反对称符号看做是叉积，交换两项的叉积顺序，然后变号： $=\lim_{\delta\boldsymbol\phi \to 0}\frac{-(exp(\boldsymbol\phi^{\wedge}))^{\wedge}J_l\delta\boldsymbol\phi}{\delta\boldsymbol\phi}$ $=-(R)^{\wedge}J_l$ 通过上面的过程我们得到了旋转矩阵相对于李代数的求导： $\frac {\partial R}{\partial\boldsymbol\phi}=\frac {\partial(exp(\boldsymbol\phi^{\wedge}))}{\partial\boldsymbol\phi}=-(R)^{\wedge}J_l$ 书中说到 $J_l$ 的计算很复杂，所以求导时候，并不采用这种方法，而是采用下面介绍的扰动模型的方法。

扰动模型（左乘）

以左乘为例，我们在对 $R$ 左乘一个微小量 $\Delta R$ ，就可以得到下一个姿态为 $\Delta R*R$ ，我们设这左扰动的李代数为 $\boldsymbol\varphi$ ，然后再次使用导数定义对 $\frac {\partial R}{\partial\boldsymbol\varphi}$ 进行求导：
$\frac {\partial R}{\partial\boldsymbol\phi} = \lim_{\boldsymbol\varphi \to 0} \frac {exp(\boldsymbol\varphi^{\wedge})*exp(\boldsymbol\phi^{\wedge}) - exp(\boldsymbol\phi^{\wedge})} {\boldsymbol\varphi}$
与前面的思路一样，先对 $exp(\boldsymbol\varphi^{\wedge})$ 进行泰勒展开，然后保留低阶项，最终可以得到如下结论：
$\frac {\partial R}{\partial\boldsymbol\phi} =-(R)^{\wedge} \tag{11}$
两种求导的方法，至于哪个精度更高，说实话还得就实际情况才能知道，但是扰动模型，显然计算量上要少非常多。
　　
　　同样的，变换矩阵的李代数求导，也可以类比获得，直接看书上的结论就可以了！
注意:不管是李代数求导还是扰动模型，都是旋转矩阵对李代数的求导，请你仔细观察一下！

实践

实践部分，请注意一定要使用作者Github上提供的Sophus库，不要使用Sophus官方的库，因为现在Sophus已经是模板类的方法编写的了，书中的例子已经不能直接使用最新版本的Sophus。

题外话:说实话这一讲，有一点儿难，特别是其中的证明特别多，而且概念之间的跳跃也很大，理解上来说是需要时间的，我已经尽量省去了很多不必要的内容，但是还是显得有点乱，你在学习的时候，如果实在不能理解证明过程，请先记住结论，但是在记住结论的同时，你要知道这个公式是用来干啥的。就像我们不知道电脑是怎么生产出来的，但是我们会用就行了。