李代数的求导及扰动模型

1.引言

李代数的求导以及扰动模型有什么用？当然就是求导的用，但在什么情况下需要求导，在机器人领域和3d视觉领域当然是函数的变量是旋转矩阵或变换矩阵，多发生在优化问题上！
举个例子，比如说在PnP问题中，如果使用基于优化的方法进行求解需要我们求解公式(1.1)：

$$T^{\ast }=argmi{n}_T\frac{1}{2}\sum _{i=1}^n{\Vert u_i-\frac{1}{s_i}KT{P}_i\Vert }^2(1.1)$$

当然非线性优化的方法有很多最速下降法、牛顿法、共轭梯度法、拟牛顿法、信赖区域法和最小二乘法等等，但是不管使用什么方法大多数都逃不掉求雅可比矩阵或者是Hession矩阵，而这些导数是关于R，T的，所以我们需要学习李代数的求导。

2.BCH公式及近似模型

BCH公式实质是在讨论李群的乘法与李代数的加法之间的关系，这个讨论的问题如果写成公式就是(2.1):

$$exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge })=exp(({\varphi }_1+{\varphi }_2{)}^{\wedge })?(2.1)$$

该等式是否成立呢？如果不成立，它们之间又是什么关系呢？
公式(2.2)即为BCH近似公式：
$$ln(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{c}{J}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2,当{\varphi }_1为小量，\\ J_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1，当{\varphi }_2为小量\end{array}(2.2)$$

以第一个近似式为例。该式告诉我们，当对一个旋转矩阵$R_2$(李代数为${\varphi }_2$)左乘一个微小旋转矩阵$R_1$(李代数为${\varphi }_1$)时，可以近似的看作，在原有李代数${\varphi }_2$上加上一项$J_l({\varphi }_2{)}^{-1}{\varphi }_1$。同样第二个近似描述了右乘一个微小位移的情况。使用时需注意左乘还是右乘，之后的解析大多数以左乘为例。
左乘BCH近似雅可比$J_l$为公式(2.3):
$$J_l=J=\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })a{a}^T+\frac{1-cos\theta }{\theta }{a}^{\wedge }(2.3)$$
它的逆为:
$$J_l^{-1}=\frac{\theta }{2}cot\frac{\theta }{2}I+(1-\frac{\theta }{2}cot\frac{\theta }{2})a{a}^T-\frac{\theta }{2}{a}^{\wedge }(2.4)$$
而右乘雅可比仅需要对自变量取负号即可:
$$J_r(\varphi )=J_l(-\varphi )(2.5)$$

为了方便进行求导和引出扰动模型，我们重新叙述BCH近似的意义。假定对某个旋转$R$，对应的李代数为$\varphi$。给他左乘一个微小旋转，记作$\Delta R$,对应的李代数为$\Delta \varphi$。那么在李群上，得到的结果就是$\Delta R\cdot R$。而在李代数上，根据BCH近似，为$J_l(\varphi {)}^{-1}\Delta \varphi +\varphi$。因此可以写成(2.6)
$$exp(\Delta {\varphi }^{\wedge })exp({\varphi }^{\wedge })=exp((J_l^{-1}(\varphi )\Delta \varphi +\varphi {)}^{\wedge })(2.6)$$

根据式子2.7近似为李群上带左右雅可比的乘法

在视觉slam上直接截的图，4.36=2.8,4.37=2.9,参考文献[6]为 T. Barfoot,State estimation for robotics:A matrix lie group approach,2016.

使用李代数进行求导的思路分为两种：

1. 用李代数表示姿态，然后根据李代数加法对李代数求导。
2. 对李群左乘或右乘微小扰动，然后对该扰动求导，成为左扰动和右扰动模型。

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

假设我们对一个空间点$p$进行了旋转,得到$Rp$。现在要计算旋转之后点的坐标相对于旋转的导数，我们非正式的记为(3.1)（这里为什么说是非正式的呢？因为李群上无法定义加法，所以无法定义传统意义上的导数）：
$$\frac{\partial (Rp)}{\partial R}(3.1)$$

由于SO(3)没有加法，所以该导数无法按照导数的定义进行计算。设$R$对应的李代数为$\varphi$，我们转而计算：
$$\frac{\partial (exp({\varphi }^{\wedge })p)}{\partial \varphi }(3.2)$$

下面将根据导数的定义进行手写推导：

不过式(3.3)仍然含有形式比较复杂的$J_l$,我们不太希望计算它。所以下面讲解了扰动模型，提供了更为简单的俄导数计算方式。

4.SO(3)上的扰动模型(左乘)

另一种求导方式是对R进行一次扰动$\Delta R$,看结果相对于扰动的变化率。这个扰动可以乘在左边也可以乘在右边，最后结果会有一点微小的差异，我们以左乘为例。
设左扰动$\Delta R$对应的李代数为$\phi$,然后对$\phi$进行求导：
$$\frac{\partial (Rp)}{\partial \phi }=\underset{\phi \to 0}{\lim }\frac{exp({\phi }^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\phi }(4.1)$$

公式(4.1)的求导比公式(3.3)更简单，相信如果看了(3.3)的手写推导，这个(4.2)一下就能看明白，所以不具体讲解了，把公式列出来：
$$\frac{\partial (Rp)}{\partial \phi }=\underset{\phi \to 0}{\lim }\frac{exp({\phi }^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\phi }$$
$$=\underset{\phi \to 0}{\lim }\frac{(I+{\phi }^{\wedge })exp(\varphi )p-exp({\varphi }^{\wedge })p}{\phi }=\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }Rp}{\phi }$$
$$=\underset{\phi \to 0}{\lim }\frac{-(Rp{)}^{\wedge }\phi }{\phi }=-(Rp{)}^{\wedge }(4.2)$$

相比于直接对李代数求导，省去了一个雅可比J_{l}的计算。这使得扰动模型更为实用。

4.SE(3)上的扰动模型(左乘)

最后，我们给出了SE(3)上的扰动模型，而直接李代数上的求导就不再介绍了。
假设某空间点$p$经过一次变换$T$(对应李代数为$\xi$)，得到$Tp$。现在，给$T$左乘一个扰动$\Delta T=exp(\delta {\xi }^{\wedge })$。
根据se(3)的定义:
$$se(3)=\left\{\xi =\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]\in {\text{R}}^6,\rho \in {\text{R}}^3,\varphi \in so(3),{\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]\in {\text{R}}^{4\times 4}\right\}(3.1)$$

则扰动项的李代数为$\delta \xi =[\delta \rho ,\delta \varphi {]}^T$，进行手写推导:

根据公式4.2我们把最后的结果定义成一个算符${}^{⨀}$,它把一个齐次坐标的空间点变换成一个$4\times 6$的矩阵。

至于实现李代数有什么编程的库，这个有很多，比如说《视觉SLAM十四讲》中提到的Sophus以及ROS都可以做，就是几个变换，剩下的一般结合到非线性优化以及一些求导问题，还有一些对应的库。个人觉得李群和李代数掌握这么多也就够了。