为什么我们在SLAM中需要李群、李代数

The Beginning of The Beginning写在最开始SLAM中不需要李群、李代数，我说的。李群、李代数初现没有李群、李代数的世界最小二乘问题优化方法高斯牛顿法 姿态估计中的导数旋转向量的指数映射迭代法子问题改写 完备的世界 后记

The Beginning of The Beginning

本文结论先收回了,看到第十章位姿图优化，发现好像还是需要李代数性质的，只能说是naive了。

写在最开始

写完后发现屁话太多了，也没好好整理逻辑。请认真的朋友直接从第4节：没有××××的世界，开始阅读。有闲心的也欢迎从头读起。

SLAM中不需要李群、李代数，我说的。

作为一个数学专业却很惭愧没有学过李群的人，最近对《SLAM十四讲》进行了一些学习。截至动笔写该文位置，博主本人仍未好好研究过李群。所以，如果下文的观点、论证如果出现了错误，也请懂行的各位直接指出，对于错误的地方，博主一定会火速修改。

回到本节题目，显而易见，我的观点就是SLAM不需要李群、李代数！至少，在高翔大大的书里前8章的内容并不需要他们(为什么？因为毕竟我才堪堪看完这么多hhh)。以我对全书脉络的把握，书中剩下的部分应该也不会出现对李群或李代数的需求了。如果现实不幸打脸，那我将把本节标题改为：《SLAM十四讲》前八章不需要李群、李代数。顺便在后面加上SLAM再次需要他们的理由。

李群、李代数初现

废话不多说，先和大家一起回顾一下《SLAM十四讲》的内容。打开书，作者给我们深入浅出地介绍了SLAM的框架，然后我们学会了刚体运动。所谓刚体运动，就是刚体在空间中旋转、平移。旋转可以用旋转矩阵表示，于是我们知道了$\text{SO(n)}$。然后，旋转还可以由一个旋转向量表示，因为两者描述的是同一个旋转，那他们有什么联系呢？作者给我们介绍了罗德里格斯公式： $$R=\cos (\theta I)+(1-\cos \theta )\mathbf{n}{\mathbf{n}}^T+\sin \theta {\mathbf{n}}^{\wedge }.$$ 其中$\theta$为旋转向量的模长，同时也是实际旋转的角度，$n$是旋转向量的旋转轴对应的单位向量。由于旋转对$\theta$是个周期函数，所以上面的变换只是个满射。反过来，我们也有： $$\theta =\arccos \frac{\text{tr}(R)-1}{2}.$$

值得一提的，平移可以用向量唯一表示，然后把平移和旋转融合，我们就有了变换矩阵的概念，变换矩阵融合了平移和旋转的信息，也具有特殊的结构，形成了特殊欧式群： $$\text{SE(3)}=\left\{T=\left[R\mathbf{t};0^{T}1\right]\in {\mathbb{R}}^{4\times 4}|R\in \text{SO(3)},\mathbf{t}\in {\mathbb{R}}^3\right\}.$$ 再往下，我们学到了欧拉角、四元数，他们也都能表示旋转，但是今天的内容用不上他俩，所以暂且不提。

接着，我们来到了艰难的第四章。首先是群，给定集合和运算，集合元素如果满足’封结幺逆’,那就是群，easy！群如果还连续，那就是李群，easy+1。然后$\text{SO(3),SE(3)}$都是李群，也是我们主要研究的目标。

--------下面一段很关键----------------------------------------------------------------------------------- 然后作者从旋转矩阵出发，用其导数引出了李代数$\mathfrak{s}\mathfrak{o}(3)$。李群$\text{SO(3)}$的单位元的切空间的元素就是其对应的李代数，导数是旋转向量$\varphi$对应的反对称矩阵${\varphi }^{\wedge }$。李代数描述了李群的局部性质。有对应关系$R(t)=\exp ({\varphi }_{0}t)$成立。。。。。。 --------书中上面一段其实我没咋看懂------------------------------------------------------------------

到目前为止，我们就已经接触过李代数$\mathfrak{s}\mathfrak{o}(3)$了。李代数也和群一样，要满足一系列性质：给定一个算子（李括号），集合里的元素关于算子满足balabala，那集合加算子再加上数域就构成李代数了。

既然李群、李代数都已经出现过了，那么接下来，欢迎来到没有李群李代数的世界。

没有李群、李代数的世界

我们先跳到后面的章节，理解为什么说需要李群李代数。

最小二乘问题

SLAM的姿态估计方法中涉及一类最小二乘问题： $$\arg \underset{R}{\min }\Vert f(R){\Vert }_2^2.$$ 其中$R$为旋转矩阵，当然，一般情况下实际上是在优化姿态，即变换矩阵$T$。

优化方法

为了求解上面的极小问题，我们需要合适的优化算法。

高斯牛顿法

如果$f(\mathbf{x})$可导，那么我们对其一阶泰勒展开。 $$f(\mathbf{x}+△\mathbf{x})\approx f(\mathbf{x})+J(\mathbf{x}{)}^T△\mathbf{x}.$$

实际上，我们并不需要‘$f(\mathbf{x})$存在导数$J(\mathbf{x})$’及类似的表述。本质上，只需要存在$J(\mathbf{x})$使得上面的近似成立即可。（作为一个没认真学分析的人，觉得这两者应该也是等价的。不过第二种表述是不是更好理解？毕竟我们甚至不需要导数的概念）

在将残差项进行了如上一阶展开之后，问题可以转为求解： $$J(\mathbf{x})J(\mathbf{x}{)}^T△\mathbf{x}=-J(\mathbf{x})f(\mathbf{x}).$$

姿态估计中的导数

回到SLAM，和普通的无约束优化不同，旋转矩阵$R$关于加法不构成群，即两个旋转矩阵的和并不是旋转矩阵，对应的函数导数（或者说一阶近似方式）就更不知道是啥了。

给个基础的SLAM优化问题： $$\arg \underset{R\in \text{SO(3)}}{\min }\Vert R\mathbf{p}{\Vert }_2^2.$$

回顾优化方法、迭代法、高斯牛顿法，我们到底需要什么？ 优化方法要找$R$使目标函数最小，迭代法关注在$R$的基础上找一个$R^{\prime }$使函数值降低，而高斯牛顿法将残差$R\mathbf{p}$在$R$附近一阶近似，然后得增量方程（而且无约束）。

我们注意到$\text{SO(3)}$关于乘法构成群。所以可以把迭代法稍微改写为： $$\arg \underset{\delta R\in \text{SO(3)}}{\min }\Vert (\delta R)R\mathbf{p}{\Vert }_2^2.$$ 又因为旋转向量和旋转矩阵可以互相转化，我们改为求旋转向量$\delta \phi$，使得其对应的旋转矩阵极小化上面的目标函数。

为了公式能顺利推导，下面插入矩阵的指数映射

旋转向量的指数映射

$$\exp ({\varphi }^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n.$$ 将$\varphi$写成模长乘以单位向量，$\varphi =\theta \mathbf{a}$。 因为 $${\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=\mathbf{a}{\mathbf{a}}^t-I,{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=-{\mathbf{a}}^{\wedge }.$$ 所以 $$\exp ({\varphi }^{\wedge })=\exp (\theta {\mathbf{a}}^{\wedge })=\cos (\theta I)+(1-\cos \theta )\mathbf{a}{\mathbf{a}}^T+\sin \theta {\mathbf{a}}^{\wedge }.$$ 正是之前的罗德里格斯公式。

(ps:这里还可以有一种更激进的看法，$\exp ({\varphi }^{\wedge })$可以彻底无视旋转向量的几何背景，而是将其看作一个普通的‘恰好’将三维向量映射为旋转矩阵的满射。我感觉后面其实也只用了这一个性质而已。不过鉴于将其视为旋转向量、然后和旋转矩阵的对应过程中还存在一个共同的实际的旋转，大抵更好理解，所以暂且将其看为旋转向量。）

迭代法子问题改写

好了，现在我们有了确切的从旋转向量到旋转矩阵的表达式了，接下来可以把子优化问题改写为关于旋转向量的问题了。 $$\arg \underset{\delta \phi \in {\mathbb{R}}^3}{\min }\Vert (\exp (\delta {\phi }^{\wedge }))R\mathbf{p}{\Vert }_2^2.$$

记$\phi =\ln (R{)}^{\vee }$，即$R$对应的旋转向量，接下来我们尝试将上面的残项进行一阶展开，即： $$(\exp (\delta {\phi }^{\wedge }))R\mathbf{p}=R\mathbf{p}+J\delta \phi +o(\Vert \delta \phi \Vert ).$$ 显然，如果$J$存在，那么就有: $$J=\underset{\delta \phi \to 0}{\lim }\frac{(\exp (\delta {\phi }^{\wedge }))R\mathbf{p}-R\mathbf{p}}{\delta \phi }.$$ 认真的朋友可能发现了，虽然我在前面口口声声说我们甚至不需要知道导数，但上面其实就是求导的过程了。

将$R$也写成指数映射， $$\underset{\delta \phi \to 0}{\lim }\frac{(\exp (\delta {\phi }^{\wedge }))R\mathbf{p}-R\mathbf{p}}{\delta \phi }=\underset{\delta \phi \to 0}{\lim }\frac{\exp (\delta {\phi }^{\wedge })\exp ({\phi }^{\wedge })\mathbf{p}-\exp ({\phi }^{\wedge })\mathbf{p}}{\delta \phi }.$$ 因为 $$\exp (\delta {\phi }^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}(\delta {\phi }^{\wedge }{)}^n=I+\delta {\phi }^{\wedge }+o(\Vert \delta \phi \Vert ).$$ 所以 $$\begin{gathered} \underset{\delta \phi \to 0}{\lim }\frac{(\exp (\delta {\phi }^{\wedge }))R\mathbf{p}-R\mathbf{p}}{\delta \phi } \\ =\underset{\delta \phi \to 0}{\lim }\frac{\exp (\delta {\phi }^{\wedge })\exp ({\phi }^{\wedge })\mathbf{p}-\exp ({\phi }^{\wedge })\mathbf{p}}{\delta \phi } \\ =\underset{\delta \phi \to 0}{\lim }\frac{(I+\delta {\phi }^{\wedge })\exp ({\phi }^{\wedge })\mathbf{p}-\exp ({\phi }^{\wedge })\mathbf{p}}{\delta \phi } \\ =\underset{\delta \phi \to 0}{\lim }\frac{\delta {\phi }^{\wedge }\exp ({\phi }^{\wedge })\mathbf{p}}{\delta \phi }. \end{gathered}$$ 其中， $$\delta {\phi }^{\wedge }\exp ({\phi }^{\wedge })\mathbf{p}=-(\exp ({\phi }^{\wedge })\mathbf{p}{)}^{\wedge }\delta \phi =-(R\mathbf{p}{)}^{\wedge }\delta \phi$$ 里面用到了${\mathbf{a}}^{\wedge }\mathbf{b}=-{\mathbf{b}}^{\wedge }\mathbf{a}$。 最终： $$\begin{gathered} \underset{\delta \phi \to 0}{\lim }\frac{(\exp (\delta {\phi }^{\wedge }))R\mathbf{p}-R\mathbf{p}}{\delta \phi } \\ =\underset{\delta \phi \to 0}{\lim }\frac{\delta {\phi }^{\wedge }\exp ({\phi }^{\wedge })\mathbf{p}}{\delta \phi } \\ =\underset{\delta \phi \to 0}{\lim }\frac{-(R\mathbf{p}{)}^{\wedge }\delta \phi }{\delta \phi } \\ =-(R\mathbf{p}{)}^{\wedge }. \end{gathered}$$ 即： $$J=-(R\mathbf{p}{)}^{\wedge }.$$ 也就是书中推导出的扰动模型（左乘）。至此，我们完成了大体的推导工作，利用$J$将关于$\delta \phi$的残差函数一阶展开，然后用高斯牛顿法等，可以求解得到一个$\delta \phi$使目标函数尽量小，然后我们对应的更新$R^{\prime }=\exp (\delta {\phi }^{\wedge }))R$，再以$R^{\prime }$为新的迭代点进行迭代即可。

关于变换矩阵$T$的优化问题我拍脑袋觉得应该差不多，就不再细细考究了。至少，到这里可以总结一句，离开了李群、李代数，SLAM还是我们的好朋友（甚至更好了，毕竟烦人的数学概念少了很多）。

完备的世界

至此，我们没有涉及李群、李代数的概念，仅仅根据优化问题的本质，利用从旋转向量（或者单纯的三维向量）到旋转矩阵的一个满射（即$\exp ({\phi }^{\wedge })$)，成功地解决了关于旋转矩阵的最小二乘问题。 换句话说，关于位姿的优化问题，只需要引入上述满射即可避开原始问题形式里的关于变量$R$或$T$的约束，使该优化问题的算法完备。

---

后记

全文的数学推导，基本上是基于我本人的直觉，少有细致的思考和严谨的证明，所以如果其中某个细节不够完善，甚至私下里用到了李代数的若干性质(毕竟在学校的时候写过无数这种类型的伪证！)，欢迎指出。本人很乐意接受大佬们的指点，从修正错误认识中成长。