0. 简介

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

1. 旋转点求导

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

f′(x)=lim⁡Δx→0f(x+Δx)Δxf'(x) = \lim_{\Delta x\rightarrow0}\frac{f(x+\Delta x)}{\Delta x}f′(x)=Δx→0lim​Δxf(x+Δx)​

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

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

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

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

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

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

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

2 JR0\boldsymbol{J}_{\boldsymbol{R}_0}JR0​​是什么

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

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

e(R,p)=e(R0,p)+JR0δϕ\boldsymbol{e}(\boldsymbol{R}, \boldsymbol{p}) = \boldsymbol{e}(\boldsymbol{R}_0, \boldsymbol{p}) + \boldsymbol{J}_{\boldsymbol{R}_0}\delta\boldsymbol{\phi}e(R,p)=e(R0​,p)+JR0​​δϕ

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

3 ⊕\oplus⊕、∧\wedge∧ 和 ×_{\times}×​ 的含义

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

a⊕ξ≜aexp⁡(ξ^)a\oplus \xi \triangleq a \exp{(\hat{\xi})} a⊕ξ≜aexp(ξ^​)

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

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

exp⁡ξ\exp{\xi}expξ 为李代数到李群的指数映射；

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

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

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

e(R,p)=Rp=exp⁡(ϕ∧)p=exp⁡((ϕ0+δϕ)∧)p=exp⁡(ϕ0∧+δϕ∧)p将Jl(ϕ0)简化为Jl(ϕ0)=(R0Rδ)pBCH近似≈exp⁡((Jlδϕ)∧)exp⁡(ϕ0∧)p泰勒展开并去除高阶项≈(I+(Jlδϕ)∧)exp⁡(ϕ0∧)p=exp⁡(ϕ0∧)p+(Jlδϕ)∧exp⁡(ϕ0∧)p=R0p+(Jlδϕ)∧R0p=e(R0,p)+(Jlδϕ)∧R0p利用a∧b=−b∧a性质=e(R0,p)−(R0p)∧Jlδϕ根据之前的的公式默认为=e(R0,p)+JR0δϕ⇒JR0=−(R0p)∧Jl\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} e(R,p)将Jl​(ϕ0​)简化为Jl​(ϕ0​)BCH近似泰勒展开并去除高阶项利用a∧b=−b∧a性质根据之前的的公式默认为⇒JR0​​​=Rp=exp(ϕ∧)p=exp((ϕ0​+δϕ)∧)p=exp(ϕ0∧​+δϕ∧)p=(R0​Rδ​)p≈exp((Jl​δϕ)∧)exp(ϕ0∧​)p≈(I+(Jl​δϕ)∧)exp(ϕ0∧​)p=exp(ϕ0∧​)p+(Jl​δϕ)∧exp(ϕ0∧​)p=R0​p+(Jl​δϕ)∧R0​p=e(R0​,p)+(Jl​δϕ)∧R0​p=e(R0​,p)−(R0​p)∧Jl​δϕ=e(R0​,p)+JR0​​δϕ=−(R0​p)∧Jl​​

最后我们得到以下结果：

JR0=−(R0p)∧Jl(ϕ0)\boldsymbol{J}_{\boldsymbol{R}_0} = - (\boldsymbol{R}_0\boldsymbol{p})^\wedge\boldsymbol{J}_l(\boldsymbol{\phi}_0) JR0​​=−(R0​p)∧Jl​(ϕ0​)

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

e(R,p)=Rp=exp⁡(ψ)∧R0p泰勒展开并去除高阶项≈(I+ψ∧)R0p=R0p+ψ∧R0p=e(R0,p)+ψ∧R0p利用a∧b=−b∧a性质=e(R0,p)−(R0p)∧ψ=e(R0,p)+JR0ψ⇒JR0=−(R0p)∧\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} e(R,p)泰勒展开并去除高阶项利用a∧b=−b∧a性质⇒JR0​​​=Rp=exp(ψ)∧R0​p≈(I+ψ∧)R0​p=R0​p+ψ∧R0​p=e(R0​,p)+ψ∧R0​p=e(R0​,p)−(R0​p)∧ψ=e(R0​,p)+JR0​​ψ=−(R0​p)∧​
不难看出来，利用左扰动模型计算的导数比使用李代数直接求导省去了一个 Jl(ϕ0)\boldsymbol{J}_l(\boldsymbol{\phi}_0)Jl​(ϕ0​) 的计算，因此更为实用，同时理论上精度也会更高（因为在计算该矩阵时需要近似）。

下面是右扰动公式：
e(R,p)=Rp=R0exp⁡(ψ)∧p泰勒展开并去除高阶项≈R0(I+ψ∧)p=R0p+R0ψ∧p=e(R0,p)+R0ψ∧p利用a∧b=−b∧a性质=e(R0,p)−R0p∧ψ=e(R0,p)+JR0ψ⇒JR0=−R0p∧\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} e(R,p)泰勒展开并去除高阶项利用a∧b=−b∧a性质⇒JR0​​​=Rp=R0​exp(ψ)∧p≈R0​(I+ψ∧)p=R0​p+R0​ψ∧p=e(R0​,p)+R0​ψ∧p=e(R0​,p)−R0​p∧ψ=e(R0​,p)+JR0​​ψ=−R0​p∧​

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

5 连乘李群的求导

…详情请参照古月居