李代数求导和扰动模型

前一篇大概介绍了李群李代数的相关性质，这里主要介绍李代数在SLAM中的作用。

BCH公式及其近似形式

李代数在SLAM中的作用主要是进行优化，优化过程就不可避免地需要涉及到求导的过程。
与标量不同，在矩阵中指数的运算法则并不适用，即是说在矩阵中：$$ln(exp(A)exp(B))\ne A+B$$
取而代之的时BCH公式：$$ln(exp(A)exp(B))=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]+...$$
其中的$“[,]”$为李括号，也就是说，两个矩阵指数之积会由这两个矩阵的和与一系列由李括号组成的余项组成。对于SO(3)，当${\varphi }_1$或${\varphi }_2$为小量时，小量二次以上的项都可以被忽略，即是有以下关系：

以第一个近似为例，当对一个旋转向量$R_2$（对应的李代数为${\varphi }_2$）左乘一个微小旋转矩阵$R_1$，可以近似看作在原有李代数${\varphi }_2$的基础上加上了$J_l({\varphi }_2{)}^{-1}{\varphi }_1$（第二个近似同理，但是是右乘）。
李代数在BCH近似下分为左乘和右乘两种，注意区分。
这里的$J$就是在上一篇关于李群李代数里提到的式子：

右乘的话只需要改变自变量的符号即可：$$J_r(\varphi )=J_l(-\varphi )$$
现在对于一个旋转$R$(对应李代数为$\varphi$)，给之左乘一个微小旋转$△R$(对应于$△\varphi$)，我们就有了如下关系：

反之，当我们在李代数上做加法运算时则有：

对于SE(3)，也有类似的关系：

这里的$ȷ$是一个$6\times 6$的矩阵。

SO(3)上的李代数的求导

在SLAM中要估计一个相机的位置和位姿，我们是通过SO(3)上的旋转矩阵或者SE(3)上的变换矩阵描述的。举个例子，在某时刻相机位姿为$T$，其观察到世界坐标位于$p$的一点，产生一个观测数据$z$，那么我们就有如下关系：$$z=Tp+\omega$$其中的$\omega$为随机噪音。
由于噪音的存在我们无法获得精准的$z-p$对应关系，所以现在我们就要对这个噪音产生的误差进行计算，即：$$e=z-Tp$$
假设一共有$N$个这样的路标点和观察，即是有$N$个上式，我们现在要预计该相机的位姿，就是要求出一组$T$，使整体误差最小化：$$\underset{T}{\min }J(T)=\sum _{i=1}^N||z_i-T{p}_i|{|}_2^2$$
要求解这个问题，就要求$J$对$T$的导数，然而因为群SO(3)和SE(3)没有加法，所以我们一般转而处理它们的李代数（因为李代数是向量的集合，对加法封闭），因此思路一般分为两种：

1. 用李代数表示位姿，然后对李代数进行求导；
2. 对李群左乘或者右乘微小扰动，然后对这个扰动进行求导，称为左扰动和右扰动模型

第一种对应李代数求导模型，第二种对应扰动模型。
以下分别进行说明。

李代数求导

（其实这部分可不看，因为最后会发现扰动模型更适合）
现在处理SO(3)的情况。假设对空间一点$p$进行旋转，得到$Rp$，我们要计算旋转后的点坐标相对于旋转的角度，即是：$$\frac{\partial (Rp)}{\partial R}$$
由于SO(3)没有加法，所以我们转而求其对应的李代数，即是：$$\frac{\partial (exp({\varphi }^{\wedge })p)}{\partial \varphi }$$
根据导数的定义计算：

第二行的近似为BCH近似，第三行为在0附近的泰勒展开舍去高阶项后的近似，第四到第五行将反对称符号看作叉积，交换后变号。于是我们可以得出以下式子：

这里简单解释下反对称矩阵和叉积的关系，也就是上面第四到第五行的变换：
$$\begin{gathered} a=[a_1,a_2,a_3] \\ b=[b_1,b_2,b_3] \\ a\times b=\left[\begin{array}{ccc}i& j& k\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\right] \\ =\left[\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ -(a_1{b}_3-a_3{b}_1)\\ a_1{b}_2-a_2{b}_1\end{array}\right] \\ =\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]\left[\begin{array}{c}{b}_1\\ b_2\\ b_3\end{array}\right] \end{gathered}$$
所以，$a$ 与$b$的叉乘即是等于$a$的反对称矩阵乘以$b$，同时叉乘满足如下反交换律：$$a\times b=-b\times a$$
回到原题，在上图中，由于结果式子含有形式比较复杂的$J_l$，不容易计算，所以我们一般不采用上面的方法，而用下面的扰动模型的方法。

扰动模型（左乘）

扰动模型的求导方法是对$R$进行一次扰动$△R$，这个微小扰动可以左乘，也可以右乘，最后结果会有一点微小差异，这里以左乘为例。设左扰动$△R$对应的李代数为$\phi$，然后对$\phi$进行求导：

相比直接对李代数求导，这里省去了一个雅可比$J_l$的计算，更为简便。

SE(3)上的李代数求导

假设空间一点$p$经过一次变换$T$(对应的李代数为$\xi$)，得到$Tp$(这里的$p$注意用齐次坐标)。现给$T$左乘一个扰动$△T=exp(\delta {\xi }^{\wedge })$，设扰动项的李代数为$\delta \xi =[\delta \rho ,\delta \varphi {]}^T$，可得：


把最后的结果定义成一个算符$\odot$，它把一个齐次空间坐标变换成一个$4\times 6$的矩阵。
最后这里简单解释下矩阵求导的顺序，设$a,b,x,y$都是列向量，其求导顺序为：

注：本文主要参考自《视觉SLAM十四讲》