SLAM本质剖析番外-李群李代数的微分和导数

0. 简介

这几个月，博主已经从SLAM算法的使用向着算法的数学推导进行了记录和分享，之前也分享了李群李代数关注核心一文，从现象中解释了李群和李代数表达的含义。但是这还不够，所以这次作者作为SLAM本质剖析的番外，来介绍李群李代数的微分和导数。

1. 旋转点求导

李群或者李代数上叠加微小量的情况呢？传统的求导过程中，我们常见的做法是对自变量添加一个微小值来进行：

$$f^{\prime }(x)=\underset{\Delta x\to 0}{\lim }\frac{f(x+\Delta x)}{\Delta x}$$

但是这种形式对于旋转矩阵 $SO(3)$ 我们不能这么做，因为李群对加法不封闭，因此两个旋转矩阵相加不一定是旋转矩阵，但是利用李代数，根据下面两个方向的 BCH 近似不难看出我们有两种思路进行求导，分别是：

• 用李代数（旋转向量）来表示姿态，然后利用李代数加法叠加微小量并对该微小量进行求导

  • 李代数求导：在李群对应的李代数的局部坐标上，即：（$\mathbf{\varphi },{\mathbf{\varphi }}^{\wedge }\in \mathfrak{so}(3)$） 上添加扰动，即：$\mathbf{\varphi }\leftarrow \mathbf{\varphi }+\delta \mathbf{\varphi }$，由于李代数本身对应旋转向量，因此对旋转向量添加扰动相当于同时改变旋转轴和旋转角度。

• 用李群（旋转矩阵）表示姿态，然后左/右乘上一个扰动，然后对该扰动求导，即左扰动模型和右扰动模型

  • 旋转矩阵右扰动求导：由于旋转矩阵没有加法，因此要对旋转矩阵本身添加扰动，需要先通过指数映射将李代数转化为李群，然后根据李群的运算来添加扰动，即：$\mathbf{R}\leftarrow \mathbf{R}\exp ({\mathbf{\varphi }}^{\wedge })$，由于是旋转矩阵右乘扰动，因此相当于是在局部坐标系下对旋转矩阵进行更新

  • 旋转矩阵左扰动求导：和右扰动同理，我们也可以将扰动添加在旋转矩阵左侧，即：$\mathbf{R}\leftarrow \exp ({\mathbf{\varphi }}^{\wedge })\mathbf{R}$，由于是旋转矩阵左乘扰动，因此相当于在全局坐标系下对旋转矩阵进行更新

李代数这样的形式我们可以理解，Ceres也是通过这样的形式进行来实现李代数的累加。但是李群就需要根据BCH来进行计算了。

2 ${\mathbf{J}}_{{\mathbf{R}}_0}$是什么

为了明白在李群李代数公式中各个成员的含义，我们给出例子来解释不同的变量。假设我们对空间一个点 $p$ 使用旋转矩阵 ${R_0} $ 进行旋转得到$R_{0}p$：
$${\mathbf{J}}_R=\frac{\partial \mathbf{e}(\mathbf{R},\mathbf{p})}{\partial \mathbf{R}}=\frac{\partial \mathbf{R}\mathbf{p}}{\partial \mathbf{R}}$$

该式子在实际计算时可以施加微小扰动 $\varphi$，通过最小化扰动来对误差进行线性化，并近似转换为，从而求出在$R_0$到$R$的情况下$p$的变化。此刻默认$\mathbf{e}({\mathbf{R}}_0,\mathbf{p})$是已知的。

$$\mathbf{e}(\mathbf{R},\mathbf{p})=\mathbf{e}({\mathbf{R}}_0,\mathbf{p})+{\mathbf{J}}_{{\mathbf{R}}_0}\delta \mathbf{\varphi }$$

得到基于 $R_0$的偏导 --------也就是${\mathbf{J}}_{{\mathbf{R}}_0}$ 是函数在${\mathbf{R}}_0$的雅可比矩阵

3 $\oplus$、$\wedge$ 和 ${}_{\times }$ 的含义

对于李群而言，由于其没有向量空间上的加法操作，因此为了引入导数的概念，这里用一个映射将局部坐标 $\xi$ 映射到李群元素 $a$ 在李群空间附近的邻域上，用来作为李群上的 “加法” 操作，所以常常会使用$\oplus$来表示李群的加法，如下所示：

$$a\oplus \xi \triangleq a\exp (\hat{\xi })$$

式中，$\xi \in {\mathbb{R}}^n$ 是 $a$ 系下的局部坐标，这个也是我们所说的极小值右乘的做法，以李群 SO(3) 为例，局部坐标可以表示为 $\xi =\omega t$，其几何意义为以 $a$ 作为参考系下的一个角度扰动；

$\hat{\xi }$ 为 $\xi$ 的对应李代数，当然也可以像上面表示为${\xi }^{\wedge }$；

$\exp \xi$ 为李代数到李群的指数映射；

$\hat{\xi }=[\omega t{]}_{\times }$ 为李代数，李代数可以转化为反对称矩阵。通过${}_{\times }$来表示角度扰动（旋转轴+旋转角度）的旋转向量。

4 由BCH得到的左扰动和右扰动基础公式

首先将旋转矩阵（李群 $SO(3)$）转换为旋转向量（李代数 $\mathfrak{so}(3)$），并对旋转向量求导：

$$\begin{array}{rl}\mathbf{e}(\mathbf{R},\mathbf{p})& =\mathbf{Rp}=\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}\\ & =\exp (({\mathbf{\varphi }}_0+\delta \mathbf{\varphi }{)}^{\wedge })\mathbf{p}\\ & =\exp ({\mathbf{\varphi }}_0^{\wedge }+\delta {\mathbf{\varphi }}^{\wedge })\mathbf{p}\\ 将J_l({\varphi }_0)简化为J_l({\varphi }_0)& =(R_0{R}_{\delta })\mathbf{p}\\ \mathrm{BCH}\mathrm{近似}& \approx \exp (({\mathbf{J}}_l\delta \mathbf{\varphi }{)}^{\wedge })\exp ({\mathbf{\varphi }}_0^{\wedge })\mathbf{p}\\ \mathrm{泰勒展开并去除高阶项}& \approx (\mathbf{I}+({\mathbf{J}}_l\delta \mathbf{\varphi }{)}^{\wedge })\exp ({\mathbf{\varphi }}_0^{\wedge })\mathbf{p}\\ & =\exp ({\mathbf{\varphi }}_0^{\wedge })\mathbf{p}+({\mathbf{J}}_l\delta \mathbf{\varphi }{)}^{\wedge }\exp ({\mathbf{\varphi }}_0^{\wedge })\mathbf{p}\\ & ={\mathbf{R}}_0\mathbf{p}+({\mathbf{J}}_l\delta \mathbf{\varphi }{)}^{\wedge }{\mathbf{R}}_0\mathbf{p}\\ & =\mathbf{e}({\mathbf{R}}_0,\mathbf{p})+({\mathbf{J}}_l\delta \mathbf{\varphi }{)}^{\wedge }{\mathbf{R}}_0\mathbf{p}\\ \mathrm{利用}{\mathrm{a}}^{\wedge }\mathrm{b}=-{\mathrm{b}}^{\wedge }\mathrm{a}\mathrm{性质}& =\mathbf{e}({\mathbf{R}}_0,\mathbf{p})-({\mathbf{R}}_0\mathbf{p}{)}^{\wedge }{\mathbf{J}}_l\delta \mathbf{\varphi }\\ 根据之前的的公式默认为& =\mathbf{e}({\mathbf{R}}_0,\mathbf{p})+{\mathbf{J}}_{{\mathbf{R}}_0}\delta \mathbf{\varphi }\\ \Rightarrow {\mathbf{J}}_{{\mathbf{R}}_0}& =-({\mathbf{R}}_0\mathbf{p}{)}^{\wedge }{\mathbf{J}}_l\end{array}$$

最后我们得到以下结果：

$${\mathbf{J}}_{{\mathbf{R}}_0}=-({\mathbf{R}}_0\mathbf{p}{)}^{\wedge }{\mathbf{J}}_l({\mathbf{\varphi }}_0)$$

因为${\mathbf{J}}_l$一般乘上的是极小值，所以在一般情况是可以省略的。这就可以根据类似的推导得到左扰动公式：

$$\begin{array}{rl}\mathbf{e}(\mathbf{R},\mathbf{p})& =\mathbf{Rp}\\ & =\exp (\mathbf{\psi }{)}^{\wedge }{\mathbf{R}}_0\mathbf{p}\\ \mathrm{泰勒展开并去除高阶项}& \approx (\mathbf{I}+{\mathbf{\psi }}^{\wedge }){\mathbf{R}}_0\mathbf{p}\\ & ={\mathbf{R}}_0\mathbf{p}+{\mathbf{\psi }}^{\wedge }{\mathbf{R}}_0\mathbf{p}\\ & =\mathbf{e}({\mathbf{R}}_0,\mathbf{p})+{\mathbf{\psi }}^{\wedge }{\mathbf{R}}_0\mathbf{p}\\ \mathrm{利用}{\mathrm{a}}^{\wedge }\mathrm{b}=-{\mathrm{b}}^{\wedge }\mathrm{a}\mathrm{性质}& =\mathbf{e}({\mathbf{R}}_0,\mathbf{p})-({\mathbf{R}}_0\mathbf{p}{)}^{\wedge }\mathbf{\psi }\\ & =\mathbf{e}({\mathbf{R}}_0,\mathbf{p})+{\mathbf{J}}_{{\mathbf{R}}_0}\mathbf{\psi }\\ \Rightarrow {\mathbf{J}}_{{\mathbf{R}}_0}& =-({\mathbf{R}}_0\mathbf{p}{)}^{\wedge }\end{array}$$
不难看出来，利用左扰动模型计算的导数比使用李代数直接求导省去了一个 ${\mathbf{J}}_l({\mathbf{\varphi }}_0)$ 的计算，因此更为实用，同时理论上精度也会更高（因为在计算该矩阵时需要近似）。

下面是右扰动公式：
$$\begin{array}{rl}\mathbf{e}(\mathbf{R},\mathbf{p})& =\mathbf{Rp}\\ & ={\mathbf{R}}_0\exp (\mathbf{\psi }{)}^{\wedge }\mathbf{p}\\ \mathrm{泰勒展开并去除高阶项}& \approx {\mathbf{R}}_0(\mathbf{I}+{\mathbf{\psi }}^{\wedge })\mathbf{p}\\ & ={\mathbf{R}}_0\mathbf{p}+{\mathbf{R}}_0{\mathbf{\psi }}^{\wedge }\mathbf{p}\\ & =\mathbf{e}({\mathbf{R}}_0,\mathbf{p})+{\mathbf{R}}_0{\mathbf{\psi }}^{\wedge }\mathbf{p}\\ \mathrm{利用}{\mathrm{a}}^{\wedge }\mathrm{b}=-{\mathrm{b}}^{\wedge }\mathrm{a}\mathrm{性质}& =\mathbf{e}({\mathbf{R}}_0,\mathbf{p})-{\mathbf{R}}_0{\mathbf{p}}^{\wedge }\mathbf{\psi }\\ & =\mathbf{e}({\mathbf{R}}_0,\mathbf{p})+{\mathbf{J}}_{{\mathbf{R}}_0}\mathbf{\psi }\\ \Rightarrow {\mathbf{J}}_{{\mathbf{R}}_0}& =-{\mathbf{R}}_0{\mathbf{p}}^{\wedge }\end{array}$$

相比于左扰动的模型中计算 $\mathbf{Rp}$ 的反对称矩阵，右扰动模型计算的是 $\mathbf{p}$ 的反对称矩阵，因此有细微的区别，使用时注意区分。

5 连乘李群的求导

…详情请参照古月居