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

0. 简介

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

1. 旋转点求导

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

$f$

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

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

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

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

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

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

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

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

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

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

3

对于李群而言，由于其没有向量空间上的加法操作，因此为了引入导数的概念，这里用一个映射将局部坐标 $\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)$）转换为旋转向量（李代数 $\boldsymbol{\mathfrak{so}}(3)$），并对旋转向量求导：

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

最后我们得到以下结果：

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

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

$\begin{aligned} \boldsymbol{e}(\boldsymbol{R}, \boldsymbol{p}) &= \boldsymbol{Rp}\\ &= \exp{(\boldsymbol{\psi})^\wedge}\boldsymbol{R}_0\boldsymbol{p}\\ \mathrm{泰勒展开并去除高阶项}&\approx (\boldsymbol{I} + \boldsymbol{\psi}^\wedge)\boldsymbol{R}_0\boldsymbol{p}\\ &= \boldsymbol{R}_0\boldsymbol{p} + \boldsymbol{\psi}^\wedge\boldsymbol{R}_0\boldsymbol{p}\\ &= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) + \boldsymbol{\psi}^\wedge\boldsymbol{R}_0\boldsymbol{p}\\ \mathrm{利用a^\wedge b = -b^\wedge a 性质}&= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) - (\boldsymbol{R}_0\boldsymbol{p})^\wedge\boldsymbol{\psi} \\ &= \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) + \boldsymbol{J}_{\boldsymbol{R}_0}\boldsymbol{\psi} \\ \Rightarrow \boldsymbol{J}_{\boldsymbol{R}_0} &= - (\boldsymbol{R}_0\boldsymbol{p})^\wedge \end{aligned}$
不难看出来，利用左扰动模型计算的导数比使用李代数直接求导省去了一个 $\boldsymbol{J}_l(\boldsymbol{\phi}_0)$

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

相比于左扰动的模型中计算 $\boldsymbol{Rp}$ 的反对称矩阵，右扰动模型计算的是 $\boldsymbol{p}$

5 连乘李群的求导

…详情请参照古月居