视觉SLAM十四讲读书笔记：Ch04李群与李代数（2）

Posted on Edited on In Views:

这是我系列博客《视觉SLAM十四讲》的读书笔记中的一篇，总结了第四章“李群与李代数”中有关李代数求导、扰动模型等的内容

一、BCH公式与李代数加法近似

在李群中，我们希望有如下理想等式： 但这是不成立的。准确地说，其近似成立仅在 或 是小量时。这个不等价现象由BCH公式（Baker–Campbell–Hausdorff）描述，其完整形式为： 在 上，如果我们令 表示李代数，则有： 当为小量当为小量 其中：

  • 是左雅可比矩阵
  • 是右雅可比矩阵

左雅可比 的表达式

设 ，其中 是旋转角度， 是单位旋转轴，则有： 其逆为：

二、李群扰动模型与加法法则

李代数加法的李群映射

基于以上的分析，我们重新阐述在BCH在SLAM下的意义：对于旋转 ，若左乘一个小扰动 ，有： 因此我们可以将李群上的“加法”定义为： 对称地：

拓展到

SE(3) 中，扰动 ，其李群逼近类似：

中左雅可比 公式

设 ，则有： 其中 ，而：

详式较复杂，见 Barfoot《State Estimation for Robotics》附录。

三、SO(3) 李代数求导与扰动推导

当位姿为T、观测到世界坐标系中位于p的点产生的观测数据z： 其中w是噪声，我们会计算误差 ​ 当有N个观测的时候，我们可以最小化误差 在此动机上，我们最好能了解某个函数关于位姿的导数。如果我们将T当作矩阵，就会得到一个带约束的优化问题。但是李代数上有良好的加法运算，可以从下面两个角度来使用李代数解决求导问题

  1. 用李代数表示姿态
  2. 对李群左乘/右乘微小扰动，对该扰动求导

我们关注函数 关于 的导数：

Note：严格来说只能求行向量关于列向量的导数才能得到矩阵。但是这里写成列向量关于列向量的导数（为了简单），可以理解为先对分子部分进行转置，最后再将结果转置。在这种意义下，可以理解为

因此：

四、扰动模型法求导（左扰动）

令 ，为左扰动，有： 相对于李代数求导来说更加简单，更加具有实用意义

五、扰动模型法求导（右扰动）

与上面的式子类似的： 求导过程如下：

六、SE(3) 求导与扰动模型

设 ，。则变换为 。

左扰动模型

令 ，有：

Note：注意为了表示方便，公式的前三行中p为齐次坐标，后面的不是齐次坐标

关于使用到的矩阵/向量求导相关的内容，参考Wiki : Matrix-Calculus

对于运算，《十四讲》的作者读做“咚”

右扰动模型

推导如下：

总结：

内容 推导关键式 工具矩阵
李群加法近似 左/右雅可比
李代数求导 李代数扰动
左扰动导数 无雅可比
右扰动导数 无雅可比
SE(3)左扰动
SE(3)右扰动