视觉SLAM十四讲学习笔记-第四讲-李代数求导与扰动模型
专栏系列文章如下：
视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客
视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客
视觉SLAM十四讲学习笔记-第二讲-开发环境搭建_goldqiu的博客-CSDN博客
视觉SLAM十四讲学习笔记-第三讲-旋转矩阵和Eigen库_goldqiu的博客-CSDN博客
视觉SLAM十四讲学习笔记-第三讲-旋转向量、欧拉角、四元数_goldqiu的博客-CSDN博客
视觉SLAM十四讲学习笔记-第三讲-相似、仿射、射影变换和eigen程序、可视化演示_goldqiu的博客-CSDN博客
视觉SLAM十四讲学习笔记-第四讲-李群与李代数基础和定义、指数和对数映射_goldqiu的博客-CSDN博客
李代数求导与扰动模型
BCH公式与近似形式
使用李代数的一大目的是进行优化，在优化过程中导数是非常必要的信息。考虑一个问题，当在SO(3)中完成两个矩阵乘法时，李代数中so(3)上发生了什么改变？反过来说，当so(3)上做两个李代数的加法时，SO(3)上是否对应着两个矩阵的乘积？如果成立，相当于：
如果ϕ1,ϕ2为标量，显然该式成立；但此处计算的是矩阵的指数函数，而非标量的指数。
换言之是在研究下式是否成立：
ln(exp(A)exp(B)) = A + B (这个式子有些问题)
该式在矩阵时并不成立。两个李代数指数映射乘积的完整形式，由 Baker-Campbell-Hausdorff公式（BCH公式）出。
其中 [] 为李括号。
BCH 公式说明，当处理两个矩阵指数之积时，它们会产生一些由李括号组成的余项。特别地，考虑SO(3)上的李代数，当ϕ1或ϕ2为小量时，小于二次以上的项都可以被忽略掉。此时，BCH拥有线性近似表达 ：
以第一个近似为例。该式说明，当对一个旋转矩阵R2（李代数为ϕ2）左乘一个微小旋转矩阵R1（李代数为ϕ1）时，可以近似地看作，在原有的李代数ϕ2上加上了一项Jl(ϕ2)−1ϕ1。同理，第二个近似描述了右乘一个微小位移的情况。于是，李代数在BCH近似下，分成了左乘近似和右乘近似两种，在使用时我们须注意使用的是左乘模型还是右乘模型。
左乘 BCH 近似雅可比
它的逆为：
右乘雅可比仅需要对自变量取负号即可：
BCH近似的意义：假定对某个旋转R，对应的李代数为ϕ。我们给它左乘一个微小旋转，记作∆R，对应的李代数为∆ϕ。那么，在李群上，得到的结果就是∆R·R，而在李代数上，根据BCH近似，为
合并起来，可以写成：
反之，如果我们在李代数上进行加法，让一个ϕ加上 ∆ϕ，可以近似为李群上的左右雅可比的乘法：
这为在李代数上做微积分提供了理论基础。同样，对于SE(3)，亦有类似的BCH近似：
这里的左右雅可比形式比较复杂，是一个6×6的矩阵，可以参考文献[6]。
[6] T. Barfoot, “State estimation for robotics: A matrix lie group approach,” 2016.
SO(3)李代数上的求导
在SLAM中，要估计一个相机的位置和姿态，该位姿是由SO(3)上的旋转矩阵或SE(3) 上的变换矩阵描述的。设某个时刻机器人的位姿为T，它观察到了一个世界坐标位于p的点，产生了一个观测数据z。由坐标变换关系知：
z = Tp + w.
其中w为随机噪声。由于它的存在，z 往往不可能精确地满足z=Tp的关系。所以通常会计算理想的观测与实际数据的误差：
e =z−Tp.
假设一共有N个这样的路标点和观测，于是就有N个上式。那么，对机器人的位姿估计，相当于是寻找一个最优的T，使得整体误差最小化：
求解此问题，需要计算目标函数J关于变换矩阵T的导数。
重点是构建与位姿有关的函数，讨论该函数关于位姿的导数，以调整当前的估计值。然而SO(3),SE(3)上并没有良好定义的加法，它们是群。如果把T当成一个普通矩阵来处理优化，那就必须对它加以约束（旋转矩阵的约束是行列式值唯一，计算复杂）。而从李代数角度来说，由于李代数由向量组成，具有良好的加法运算。
使用李代数解决求导问题的思路分为两种：
-
用李代数表示姿态，然后根据李代数加法来对李代数求导。
-
对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型。
第一种方式对应到李代数的求导模型，而第二种则对应到扰动模型。
李代数求导
考虑SO(3)上的情况。假设对一个空间点p进行了旋转，得到了Rp。计算旋转之后点的坐标相对于旋转的导数，记为 ：
由于SO(3)没有加法，所以该导数无法按照导数的定义进行计算。设R对应的李代数为ϕ，转而计算 ：
按照导数的定义，推导出了旋转后的点相对于李代数的导数：
注：这里并不能按照矩阵微分来定义导数，只是一个记号。
扰动模型（左乘）
另一种求导方式是对R进行一次扰动∆R，看结果相对于扰动的变化率。这个扰动可以乘在左边也可以乘在右边，最后结果会有一点微小的差异。以左扰动为例，设左扰动∆R对应的李代数为φ。对φ求导，即：
相比于直接对李代数求导，省去了一个雅可比矩阵的计算。这使得扰动模型更为实用，在位姿估计当中具有重要的意义。
SE(3)上的李代数求导
书中只给出SE(3)上的扰动模型。假设某空间点p经过一次变换T（对应李代数为ξ），得到Tp 。给T左乘一个扰动∆T = exp(δξ∧)，设扰动项的李代数为 δξ = [δρ,δϕ]T，那么：
把最后的结果定义成一个算符 ，它把一个齐次坐标的空间点变换成一个4×6的矩阵。
（这里不太理解）