李群和李代数（三）——李代数求导和扰动模型

1.求导方式

在SLAM中，我们要估计一个相机的位置和姿态，该位姿是由SO(3)上的旋转矩阵或者SE(3)上的变换矩阵描述。

设置某时刻相机的位姿为T，他观察到了一个世界坐标位于p的点，产生了一个观测数据。那么由坐标变换关系知：
$$z=Tp+w$$
其中w为随机噪声。由于他的存在，z往往不可能精确地满足z=Tp的关系。所以我们通常会计算理想的观测与实际数据的误差：
$$e=z-Tp$$
假设一共由N个这样的路标点和观测，于是就有N个上式。那么对相机进行位姿估计，相当于寻找一个最优的T，使得整体误差最小化：
$$\underset{T}{\min }J(T)=\sum _{i=1}^N||z_i-T{p}_i|{|}_2^2$$
求解此问题，需要计算目标函数$J$关于变换矩阵T的导数。

这里的重点是，我们会经常构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。

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

1.使用李代数表示姿态，然后根据李代数加法对李代数求导，这是李代数的求导模型。

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

2.李代数求导

假设对一个空间点p进行旋转，得到了$Rp$。现在,要计算旋转后点的坐标相当于旋转的导数，记为：
$$\frac{\partial (Rp)}{\partial R}$$
由于SO(3)没有加法，所以该导数无法按照导数的定义进行计算。设R对应的李代数为$\Phi$,有：
$$\frac{\partial (exp({\varphi }^{\wedge })p)}{\partial \Phi }$$
按照导数定义，有：
$$\begin{gathered} \frac{\partial (exp({\varphi }^{\wedge })p)}{\partial \varphi }=\underset{\delta \varphi \to 0}{\lim }\frac{exp((\varphi +\delta \varphi {)}^{\wedge })exp({\varphi }^{\wedge })p}{\delta \varphi } \\ =\underset{\delta \varphi \to 0}{\lim }\frac{exp((J_l\delta \varphi {)}^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\delta \varphi } \\ =\underset{\delta \varphi \to 0}{\lim }\frac{(I+(J_l\delta \varphi {)}^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\delta \varphi } \\ =\underset{\delta \varphi \to 0}{\lim }\frac{((J_l\delta \varphi {)}^{\wedge })exp({\varphi }^{\wedge })p}{\delta \varphi } \\ =\underset{\delta \varphi \to 0}{\lim }\frac{-(exp({\varphi }^{\wedge })p{)}^{\wedge }{J}_l\delta \varphi }{\delta \varphi } \\ =-(Rp{)}^{\wedge }{J}_l \end{gathered}$$
第二行的近似为BCH线性模拟，第三行为泰勒展开舍去高阶项后的近似，第四行至第五行将反对称符号看作叉积，交换后变号。

这里还有形式比较复杂的$J_l$,我们不要希望计算他，扰动模型就更简单。

3.扰动模型（左乘）

另外一种求导方式是对R进行一次扰动$△R$,看结果相对于扰动的变化率。这个扰动可以乘在左边也可以乘在右边，最后结果会有一点微小的误差，下面公式以做扰动为例。设左扰动为$△R$对应的李代数为$\phi$，然后对$\phi$求导，即：
$$\begin{gathered} \frac{\partial (Rp)}{\partial \phi }=\underset{\phi \to 0}{\lim }\frac{exp({\phi }^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\phi } \\ =\underset{\delta \varphi \to 0}{\lim }\frac{(I+{\phi }^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\delta \varphi } \\ =\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }Rp}{\phi }=\underset{\phi \to 0}{\lim }\frac{-(Rp{)}^{\wedge }\phi }{\phi }=-(Rp{)}^{\wedge } \end{gathered}$$
相比于直接对李代数求导，省去了一个雅克比$J_l$的计算，这使得扰动模型更为实用。