【视觉SLAM十四讲学习笔记】第四讲——李代数求导与扰动模型

专栏系列文章如下：
【视觉SLAM十四讲学习笔记】第一讲——SLAM介绍
【视觉SLAM十四讲学习笔记】第二讲——初识SLAM
【视觉SLAM十四讲学习笔记】第三讲——旋转矩阵
【视觉SLAM十四讲学习笔记】第三讲——旋转向量和欧拉角
【视觉SLAM十四讲学习笔记】第三讲——四元数
【视觉SLAM十四讲学习笔记】第三讲——Eigen库
【视觉SLAM十四讲学习笔记】第四讲——李群与李代数基础
【视觉SLAM十四讲学习笔记】第四讲——指数映射

BCH公式与近似形式

使用李代数的一大动机是进行优化，而在优化过程中导数是非常重要的信息。虽然我们已经清楚了SO(3)和SE(3)上的李群与李代数关系，但是当在SO(3)中完成两个矩阵乘法时，李代数中so(3)上发生了什么改变呢？反过来说，当so(3)上做两个李代数的加法时，SO(3)上是否对应着两个矩阵的乘积？

如果成立，相当于：

如果?_1,?_2为标量，那么显然该式成立；但此处我们计算的是矩阵的指数函数，而非标量的指数。换言之，我们在研究下式是否成立：

很遗憾，该式在矩阵时并不成立。两个李代数指数映射乘积的完整形式，由BCH公式给出。由于其完整形式较复杂，我们只给出其展开式的前几项，其中[ ]为李括号：

上面的BCH公式告诉我们，当处理两个矩阵指数之积时，它们会产生一些由李括号组成的余项。特别地，考虑SO(3)上的李代数ln(exp(?_1^)exp(?_2 ^))∨，当?_1或?_2为小量时，小量二次以上的项都可以被忽略。此时，BCH拥有线性近似表达：

以第一个近似为例。该式告诉我们，当对一个旋转矩阵R_2（李代数为?_2）左乘一个微小旋转矩阵R_1（李代数为?_1）时，可以近似地看作，在原有的李代数?_2上加上了一项J_l(?_2)^-1?_1。同理，第二个近似描述了右乘一个微小位移的情况。于是，李代数在BCH近似下，分成了左乘近似和右乘近似两种。而右乘雅可比仅需要对自变量取负号即可：

这样，我们就可以谈论李群乘法和李代数加法的关系了。为了方便理解，我们重新叙述BCH近似的意义。假定对于某个旋转R，对应的李代数为?。我们给他左乘一个微小旋转，记作ΔR，对应的李代数为Δ?。那么，在李群上，得到的结果就是ΔR·R，而在李代数上，根据BCH近似，为J_l^-1(?) Δ?+?。合并起来，可以简单地写成：

反之，如果我们在李代数上进行加法，让一个?加上Δ?，那么可以近似为李群上带左右雅可比的乘法：

这就为之后李代数上做微积分提供了理论基础。同样地，对于SE(3)，也有类似的BCH近似：

SO(3)上的李代数求导

在SLAM中，要估计一个相机的位置和姿态，该位姿是由SO(3)上的旋转矩阵或SE(3)上的变换矩阵描述的。设某个时刻机器人的位姿为T，它观察到了一个世界坐标位于p的点，产生了一个观测数据z。由坐标变换关系知：

其中w为随机噪声。由于它的存在，z 往往不可能精确地满足z=Tp的关系。所以通常会计算理想的观测与实际数据的误差：
假设一共有N个这样的路标点和观测，于是就有N个上式。那么，对机器人的位姿估计，相当于是寻找一个最优的T，使得整体误差最小化：

求解此问题，需要计算目标函数J关于变换矩阵T的导数。重点是构建与位姿有关的函数，讨论该函数关于位姿的导数，以调整当前的估计值。然而SO(3),SE(3)上并没有良好定义的加法，它们是群。如果把T当成一个普通矩阵来处理优化，那就必须对它加以约束（旋转矩阵的约束是行列式值唯一，计算复杂）。而从李代数角度来说，由于李代数由向量组成，具有良好的加法运算。

因此，使用李代数解决求导问题的思路分为两种：

1. 用李代数表示姿态，然后根据李代数加法来对李代数求导。

2. 对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型。

第一种方式对应到李代数的求导模型，而第二种则对应到扰动模型。

李代数求导

首先，考虑SO(3)上的情况。假设对一个空间点p进行了旋转，得到了Rp。计算旋转之后点的坐标相对于旋转的导数，我们非正式地记为 ：

由于SO(3)没有加法，所以该导数无法按照导数的定义进行计算。设R对应的李代数为?，转而计算：

按照导数的定义，推导出了旋转后的点相对于李代数的导数：

不过，由于这里仍然含有形式比较复杂的雅可比式，我们不太希望计算它。而下面的扰动模型则提供了更简单的导数计算方式。

扰动模型（左乘）

另一种求导方式是对R进行一次扰动ΔR，看结果相对于扰动的变化率。这个扰动可以乘在左边也可以乘在右边，最后结果会有一点微小的差异，我们以左扰动为例。设左扰动ΔR对应的李代数为φ。然后对φ求导，即：

相比于直接对李代数求导，省去了一个雅可比矩阵的计算。这使得扰动模型更为实用，在位姿估计当中具有重要的意义。

SE(3)上的李代数求导

最后，我们给出SE(3)上的扰动模型，而直接李代数上的求导就不再介绍了。假设某空间点p经过一次变换T（对应李代数为ξ），得到TP。

现在，给T左乘一个扰动?T = exp(δξ∧)，设扰动项的李代数为 δξ = [δρ,δ?]^T，那么：

我们把最后的结果定义成一个算符，它把一个齐次坐标的空间点变换成一个4×6的矩阵。