SLAM基础_从零开始一起学习SLAM | 为啥需要李群与李代数？

深碍√TFBOYSˉ_ 2022-12-24 15:48 285阅读 0赞

2018-10-22 07:30

很多刚刚接触SLAM的小伙伴在看到李群和李代数这部分的时候，都有点蒙蒙哒，感觉突然到了另外一个世界，很多都不自觉的跳过了，但是这里必须强调一点，这部分在后续SLAM的学习中其实是非常重要的基础，不信你看看大神们的论文就知道啦。

关于李群李代数，其实高翔的《视觉SLAM十四讲》里推导什么的挺清楚了，本文就在高博的基础上用比较容易理解的语言讲述一下重点。

首先，假装（也可能是真的）自己是个小白，我们假想对面坐了一个大牛师兄，下面我们开启问答模式。

![76a72c43b4c039035f3e9ac201378e24.png][]

# 为啥需要李代数？ #

小白：师兄，我最近在学习SLAM，看到李群、李代数这一块一直看不懂，不知所云啊，师兄能不能用通俗易懂的方式给我讲解一下？

师兄：好啊，正好这会有空，讲完正好去吃饭。

小白：我请师兄吃烧烤！

师兄：哈哈，那我必须给你讲明白啦！现在开始吧。

小白：好，先问下师兄，我在看高博的书，前面几章挺顺利的，第四章突然跳出来李群和李代数，一堆公式推导，看的我头都大了。

师兄：这部分公式是有点多，不过李群李代数是为了解决SLAM中非常实际的问题的。到后面会用到的。

小白：看来逃不过啊。。。

师兄：是的，这部分必须理解的啊。刚才说到了解决SLAM中实际问题，我展开说下。我们知道SLAM的过程就是不断的估计相机的位姿和建立地图。其中，相机位姿也就是我们所说的变换矩阵T。

小白：嗯嗯，是。上节课《从零开始一起学习SLAM | 三维空间刚体的旋转》中还讲了变换矩阵呢！

师兄：对~下面举个例子说明。比如你拿着相机一边移动一边拍，假设某个时刻相机的位姿是T，它观察到一个在世界坐标系中的一个空间点p，并在相机上产生了一个观测数据z，那么

z = Tp + noise

noise是观测噪声。那么观测误差就是

e = z - Tp

小白：嗯，我 知道，我们的目的就是使得误差最小咯~

师兄：对的，假设我们总共有N个这样的三维点p和观测值z，那么我们的目标就是寻找一个最佳的位姿T，使得整体误差最小化，也就是

![98e2fde4091a8fcbc410705510d603b0.png][]

求解此问题，就是求目标函数J对于变换矩阵T的导数。

小白：嗯，对矩阵求导？第一次 听说啊。。

师兄：听起来确实有点怪。我们先来看看变换矩阵T，我们知道T所在的SE(3)空间，对加法计算并不封闭，也就是说任意两个变换矩阵相加后并不是一个变换矩阵，这主要是因为旋转矩阵对加法是不封闭造成的，它是有约束的。

小白：旋转矩阵对加法不封闭啥意思？

师兄：嗯，这个我一会会细讲，这里你先记住好了。到后面你就知道了

小白：好的，那刚才的问题怎么解决呢？

师兄：这个问题问的好，李代数就是解决这个问题的。我们把大写SE(3)空间的T映射为一种叫做李代数的东西，映射后的李代数我们叫做小se(3)好了。它是由向量组成的，我们知道**向量是对加法封闭的**。这样我们就可以通过对李代数求导来间接的对变换矩阵求导了。

小白：原来如此啊！不过刚才说了那么多概念，都是什么意思啊？

李群怎么理解？

师兄：不急，我一个个说。我先说说李群吧，额，不，先说说群吧。按照数学上定义：群（group）就是一种**集合**加上一种**运算**的代数结构。群有几个运算性质，好像高博说是“凤姐咬你”

小白：（瞪大了眼睛）嗯？

师兄：哦，谐音谐音。。。就是：**封闭性，结合律，幺元，还有逆**。对了，比如旋转矩阵和乘法就构成了旋转矩阵群，变换矩阵和乘法也构成了变换矩阵群。对了，你说，旋转矩阵和加法能构成群吗？

小白：额。。刚才好像说不行吧？

师兄：嗯，不行的 ，他们不满足封闭性。刚才没有细讲，下面仔细解释原因。我们知道旋转矩阵R本身有一定的约束：

![7a1d309805d62e2332e63fe6d84f955a.png][]

两个旋转矩阵R1+R2的结果就不能满足上述约束了，但是R1\*R2满足。此外，旋转矩阵还满足结合律：R1\*R2=R2\*R1，还有**幺元是单位矩阵I**，也有逆矩阵满足R乘以R的逆等于幺元（单位阵）。还有，我们在SLAM里最常说的有两个，一个是特殊正交群SO(3)，也就是旋转矩阵群，还有特殊欧氏群SE(3)，也就是变换矩阵群，3代表是三维的。

小白：嗯嗯，书上看了，我差不多理解群是个什么东东了，那李群呢？

师兄：李群的定义是指连续光滑的群，比如我们前面说的旋转矩阵群SO(3)，你想象你拿个杯子就可以在空间中以某个支点连续的旋转它，所以SO(3)它就是李群。如果你一般旋转一边移动它，也是连续的或者说光滑的运动，所以变换矩阵群SE(3)也是李群。

李代数是李群的亲戚吗？

小白：嗯，师兄，那李代数呢，它和李群都姓李，他们什么关系？

师兄：（一脸黑线）我个人的理解是这样的，就是我们相机在三维空间中是连续的旋转或者变换的嘛，刚才说过，而我们SLAM目的就是优化求解相机的这个最佳的位姿T（变换矩阵），优化方法一般都采用迭代优化的方法，每次迭代都更新一个位姿的增量delta，使得目标函数最小。这个delta就是通过误差函数对T微分得到的。也就是说我们需要对变换矩阵T求微分（导数），我们先以SO(3)空间中的旋转矩阵 R为例来说说吧，你觉得如何对R求微分呢？

小白：矩阵怎么求。。求微分，这个能微分吗？以前没有学过啊

师兄：可以的，李群和李代数都姓李（笑），你还别说，他们之间的确存在某种微分关系。我们先把结论放这里：**李代数对应李群的正切空间，它描述了李群局部的导数**。

小白：也就是说，李代数对应了李群的导数？

师兄：可以这么理解，你可以去看一下十四讲中65-66页那部分的推导，我们只关注两个结论就行了

# 第一个结论：**旋转矩阵的微分** #

看下面的公式，我们发现**旋转矩阵的微分**是一个反对称(也叫斜对称)矩阵左乘它本身，也印证了我前面说的，矩阵是可以微分的。**对于某个时刻的R(t)（李群空间），存在一个三维向量φ=（φ1，φ2，φ3）（李代数空间），用来描述R在t时刻的局部的导数**。

![83e49930a179de57d8a776686b0e5174.png][]

## 反对称矩阵是啥？ ##

小白：等一下，师兄，反对称矩阵是啥？第一次听说啊

师兄：哦哦，忘记解释了。反对称矩阵英文是skew symmetric matrix，有的地方也翻译为斜对称矩阵，其实是一个东西。

小白：这个反对称矩阵是啥意思？

师兄：反对称矩阵其实是将三维向量和三维矩阵建立对应关系。它是这样定义的：如果一个3 X 3的矩阵A满足如下式子

![b948c3b7ba7377b1fced8a658614c81b.png][]

那么A就是反对称矩阵。你看左边有个转置，右边有个负号，叫反对称矩阵，还是挺形象的。

小白：额，好像有点明白，不过这个有啥用啊？

师兄：先别急，先问你一个问题，你觉得反对称矩阵它的元素有什么特点？

小白：啊。。特点啊，我想想（一分钟过去了。。）

师兄：根据它的性质，先想想对角线元素。你看，上式等式左边矩阵A转置后，对角线元素aii是不是还在对角线上？

小白：对哦，师兄好厉害

师兄：额。。别打岔，等式右边，所有元素取负号，那么对于对角线元素aii来说，是不是满足aii=-aii？

小白：是哦，所以aii=0，也就是说反对称矩阵对角线元素都为0？

师兄：bingo！确实是这样。那么非对角线元素还有6个，它们能不能精简呢？

小白：我想想，感觉好像是有重复的，好像可以用更少的元素来表示

师兄：没错！我举个例子，等式左边第2行第1列位置的元素，是矩阵A元素a12转置后到了位置a21，等式右边原来a21变成了 -a21，所以其实对于矩阵A，元素a12 = -a21，所以用一个元素及其负数就可以表示矩阵中这两个元素，同理，其他4个元素也是这样。所以，其实矩阵A中非对角线元素只用3个元素就可以表示。也就是说反对称矩阵A只有3个自由度。

小白：嗯呢，师兄好厉害！不过。。。知道这些有啥用啊？

师兄：这个反对称矩阵只有3个自由度很重要啊，这样我们就可以把一个三维向量和一个三维矩阵建立对应关系。

小白：师兄，感觉还是很抽象啊

师兄：哦哦，那我举个栗子给你看看。我们假设有一个反对称矩阵A的定义如下：

![c6210dce24c3082826ccafbde72c6f8b.png][]

小白：等下，我看看是否满足性质：该矩阵的转置等于该矩阵元素取负数。。

师兄：你看是不是我们前面推算的一致啊，对角线元素为0，只有3个自由度？

小白：是哦，确实没错！师兄继续。。

师兄：我们定义对应的一个三维向量：

![dfe7d9d6f181ca47c22cd18edabb786f.png][]

然后我们用一个上三角符号来表示这个向量α和三维矩阵A的对应关系

![87cdfd685914a260503d9e8c33dad4d0.png][]

小白：这个符号感觉很神奇啊

师兄：是的，通过这个符号，我们把向量和矩阵建立了对应关系。这个在后面非常重要。你再看看前面的第一个结论

![1b5410b29bbfbb17ca6f7efb4e121384.png][]

就好理解很多了。

小白：嗯嗯。确实是呢。师兄继续下一个结论吧。

# 指数映射 #

师兄：好 ，下面说说第二个结论。通过高博一系列辛苦的 计算（笑），我们最终得到下面式子，它的前提是R在原点附近的一阶泰勒展开，我们看到这个向量φ=（φ1，φ2，φ3）反应了R的导数性质，故称它在SO(3)上的原点 φ0 附近的正切空间上。这个φ正是李群大SO(3)对应的李代数小so(3)。

![f110b47f167ed5b10d7e3c284b1a2423.png][]

小白：好晕啊。。

师兄：你这么理解吧，**李代数小so(3)**是三维向量φ的集合，每个向量φi的反对称矩阵都可以表达李群(大SO(3))上旋转矩阵R的导数，而R和φ是一个指数映射关系。也就是说，**李群空间的任意一个旋转矩阵R，都可以用李代数空间的一个向量的反对称矩阵指数来近似**。

小白：好绕的绕口令啊。。

师兄：没事，你只要记得用旋转矩阵表示的话就是李群空间，也是我们熟悉的表示方法。而用向量的反对称矩阵表示的话就是李代数空间，这两个空间建立了联系。

小白：师兄，那这个古怪的式子

![e6e0e0d36fb6c44a4c637bfe1b0559ec.png][]

如何计算呢？

师兄：嗯，这个用大一学的微积分就行。

小白：微积分忘的差不多了。。。

师兄：没事，其实就只用到指数e的泰勒展开

![da4258c212e76740c4dfff2816c5c1ed.png][]

小白：师兄，书上的推导好麻烦啊

师兄：先不管具体推导过程，我们先来看看结论，你说的那个指数形式的古怪的式子通过运用泰勒展开，以及反对称矩阵的性质，我们可以得到如下结果：

![fc04c61f77de72b5f91d1ce6184b620f.png][]

其中：三维向量 φ = θa，a是一个长度为1的方向向量。看到这个式子有没有觉得很神奇？

小白：好像在哪里见过啊

师兄：嗯，这个式子和**罗德里格斯公式**长的一模一样

小白：忘了什么是罗德里格斯（**Rodrigues**）公式了。。。

师兄：你还记得旋转的表示方法吗？有旋转矩阵、旋转向量、欧拉角、四元数，而**罗德里格斯公式是表示从旋转向量到旋转矩阵的转换过程**的

小白：师兄这么一说，我想起来了，旋转向量也有一个旋转角θ，旋转轴也是单位方向向量

师兄：其实旋转向量就是这里的李代数

小白：啊？这怎么会扯上关系？

师兄：你可能有点反应不过来，不过的确，**小so(3)的李代数空间就是由旋转向量组成的的空间，其物体意义就是旋转向量。而前面结论二中的指数映射关系就是罗德里格斯公式，他们在数学上本质是一样**的

小白：真的好神奇啊

师兄：嗯，这样我们可以说旋转矩阵的导数可以由其对应的旋转向量指定，指导如何在旋转矩阵中进行微积分运算。

小白：这样就好理解多了

# 李群李代数之间的 指数对数 映射关系 #

师兄：嗯，反过来，用对数映射也能把大SO(3)李群空间中元素映射到小so(3)李代数空间中去。前面我们都是讲的SO(3)上的映射关系，放到SE(3)上推导类似，也是泰勒展开，旋转矩阵R映射结果和SO(3)一样，平移部分指数映射后会有稍许的不同，它前面多了一个系数矩阵，这些都可以自己证明一下（留作作业）。

小白：嗯嗯，师兄，是不是只要记住高博大神书上的对应关系图就行啦？

![72e082c187b6037f936fccb54c404738.png][]

师兄：这个图要理解透彻

小白：对了，师兄，好像还有一个左扰动，右扰动什么的，这个是干什么用的呀

师兄：这个是用李代数解决求导问题时使用的方法。对了，李代数是对加法封闭的吗？

小白：嗯，李代数是由向量组成的，向量对加法运算是封闭的。

师兄：嗯，学的真快！你说的没错。**李代数求导分两种**：

*  一种是用李代数表示位姿，然后根据李代数加法来对李代数求导。这种方法书中也推导了，结果中有复杂的雅克比公式，不是很方便。
 *  一般都用第二种，就是对李群进行左乘或者右乘微小的扰动，然后对该扰动求导。书上高博也推导了，你看结果还是挺简洁的。

![4c68f7f7ef2cb5865545352a397ab063.png][]

小白：那我们就用扰动模型好啦

师兄：确实实际SLAM问题中，扰动模型比较实用方便。扰动模型的推导一定要自己推一遍哦

小白：嗯，我尽量。。谢谢师兄耐心解答，走，请你吃烧烤去。

以上内容参考了高博的《视觉SLAM十四讲》，SLAM入门必备，优惠购买链接

彩蛋

李群李代数部分有不少推导，其中最难理解的3个推导是SO(3)左扰动模型，SO(3)李代数求导，SE(3)左扰动模型，知识星球：从零开始学习SLAM中关于上面3个过程的推导进行了视频讲解，见下面视频：

如果你还在一个人痛苦的学习SLAM，可以关注下方，一起从零开始，学习更多SLAM知识。

# 本讲练习 #

## 1、**重要****理论推导题** ##

推导李代数小se(3)的指数映射。

我们知道对于大SE(3)，其对应的李代数为小se(3)。其定义如下

![1e1ba6f1ee33b014533a95a7a8a9e18a.png][]

证明1：

![c8a247174457bc7f63d06a5b7fd676f7.png][]

证明2：令ρ=θa，那么

![253e7bb3851259880222981e5cd075bd.png][]

提示：

参考《视觉SLAM十四讲》P68-71页内容。参考SO(3) 的泰勒展开，然后合并奇偶数项级数

## 2、**编程练习** ##

SLAM问题的目标之一就是精确的估计相机运动的轨迹（姿态），如果我们将相机运动的轨迹绘制出来，就可以直观的观察它的运动是否符合预期。给定一个轨迹文件trajectory.txt，该文件的每一行由若干个数据组成，格式为 \[time, tx, ty, tz, qx, qy, qz, qw\],其中 time 为时间，tx,ty,tz 为平移部分，qx,qy,qz,qw 是四元数表示的旋转部分，请完成数据读取部分的代码，绘制部分代码已经给出。

公众号菜单栏回复：“**轨迹**”，即可下载上面代码框架和轨迹数据。

欢迎留言讨论，更多学习视频、文档资料、参考答案等在菜单栏点击“知识星球”，进入《知识星球：从零开始一起学习SLAM》和其他学习SLAM学的伙伴一起学习交流~

[76a72c43b4c039035f3e9ac201378e24.png]: /images/20221120/db7053418c53482d8579a9c9888739db.png
[98e2fde4091a8fcbc410705510d603b0.png]: https://img-blog.csdnimg.cn/img_convert/98e2fde4091a8fcbc410705510d603b0.png
[7a1d309805d62e2332e63fe6d84f955a.png]: https://img-blog.csdnimg.cn/img_convert/7a1d309805d62e2332e63fe6d84f955a.png
[83e49930a179de57d8a776686b0e5174.png]: https://img-blog.csdnimg.cn/img_convert/83e49930a179de57d8a776686b0e5174.png
[b948c3b7ba7377b1fced8a658614c81b.png]: https://img-blog.csdnimg.cn/img_convert/b948c3b7ba7377b1fced8a658614c81b.png
[c6210dce24c3082826ccafbde72c6f8b.png]: https://img-blog.csdnimg.cn/img_convert/c6210dce24c3082826ccafbde72c6f8b.png
[dfe7d9d6f181ca47c22cd18edabb786f.png]: https://img-blog.csdnimg.cn/img_convert/dfe7d9d6f181ca47c22cd18edabb786f.png
[87cdfd685914a260503d9e8c33dad4d0.png]: https://img-blog.csdnimg.cn/img_convert/87cdfd685914a260503d9e8c33dad4d0.png
[1b5410b29bbfbb17ca6f7efb4e121384.png]: https://img-blog.csdnimg.cn/img_convert/1b5410b29bbfbb17ca6f7efb4e121384.png
[f110b47f167ed5b10d7e3c284b1a2423.png]: https://img-blog.csdnimg.cn/img_convert/f110b47f167ed5b10d7e3c284b1a2423.png
[e6e0e0d36fb6c44a4c637bfe1b0559ec.png]: https://img-blog.csdnimg.cn/img_convert/e6e0e0d36fb6c44a4c637bfe1b0559ec.png
[da4258c212e76740c4dfff2816c5c1ed.png]: https://img-blog.csdnimg.cn/img_convert/da4258c212e76740c4dfff2816c5c1ed.png
[fc04c61f77de72b5f91d1ce6184b620f.png]: https://img-blog.csdnimg.cn/img_convert/fc04c61f77de72b5f91d1ce6184b620f.png
[72e082c187b6037f936fccb54c404738.png]: https://img-blog.csdnimg.cn/img_convert/72e082c187b6037f936fccb54c404738.png
[4c68f7f7ef2cb5865545352a397ab063.png]: https://img-blog.csdnimg.cn/img_convert/4c68f7f7ef2cb5865545352a397ab063.png
[1e1ba6f1ee33b014533a95a7a8a9e18a.png]: https://img-blog.csdnimg.cn/img_convert/1e1ba6f1ee33b014533a95a7a8a9e18a.png
[c8a247174457bc7f63d06a5b7fd676f7.png]: https://img-blog.csdnimg.cn/img_convert/c8a247174457bc7f63d06a5b7fd676f7.png
[253e7bb3851259880222981e5cd075bd.png]: https://img-blog.csdnimg.cn/img_convert/253e7bb3851259880222981e5cd075bd.png