Yihua Zhang
EN 中
Archive

Visual SLAM Notes: Lie Groups and Lie Algebras

Visual Slam笔记——李群和李代数

Published
9min read
3,673words
zhlanguages

Only in Chinese.

问题的引入

当我们估计出相机姿态[R,t][R, t]了以后，估计的结果和实际的相机姿态肯定会有一些不一致性，因而我们需要对估计出来的结果进行优化。优化方法一般都采用迭代优化的方法，每次迭代都更新一个位姿的增量Δ\Delta，使得目标函数最小。这个Δ\Delta就是通过误差函数对T\mathbf{T}或者R\mathbf{R}求微分得到的，也就是说我们需要对变换矩阵T\mathbf{T}以及旋转矩阵R\mathbf{R}求导。

但是，旋转矩阵并没有良好定义的加法，即R1R_1和R2R_2是都是旋转矩阵，但是R1+R2R_1 + R_2并不符合旋转矩阵的定义：旋转矩阵应该满足RRT=IRR^T = I，且det(R)=1det(R) = 1，即行列式的值为1的正交矩阵，因此上边的结论是显而易见的。

所以，在实际操作中，我们无法通过如下的微调满足我们对相机观测模型的优化：

[R,t]⟶ΔR,Δt[R′,t′]t′=t+ΔtR′=R+ΔR[R, t] \stackrel{\Delta R, \Delta t}{\longrightarrow} [R', t'] \\ t' = t + \Delta t \\ R' = R + \Delta R(1)

因而也会影响到观测模型（关于旋转矩阵的函数）对R\mathbf{R}的求导，因为通常来说我们的导数定义为：

U(R)=dUdR=lim⁡ΔR→0U(R+ΔR)−U(R)ΔRU(R) = \frac{dU}{dR} = \lim\limits_{\Delta R \to 0}\frac{U(R + \Delta R) - U(R)}{\Delta R}(2)

没有良好定义的加法——没有传统意义上的导数定义。 因此，李群和李代数的作用就是间接对R\mathbf{R}进行求导。

预备知识

矩阵指数

首先要引入矩阵指数的概念（Matrix Exponential）。矩阵指数可以由线性微分方程的解的形式导出，比如有一阶线性微分方程：

x⋅=ax(t)\stackrel{·}{x} = ax(t)(3)

其中，x(t)∈Rx(t) \in \mathbb{R}，a∈Ra \in \mathbb{R}，并且初始分布为x(0)=x0x(0) = x_0，那么解为：

x(t)=eatx0x(t) = e^{at}x_0(4)

其中指数函数可以展开为无穷级数：

eat=1+at+(at)22!+(at)33!+⋅⋅⋅e^{at} = 1 + at + \frac{(at)^2}{2!} + \frac{(at)^3}{3!} + ···(5)

同理，如果我们对三维向量由一阶线性微分方程：

x⋅(t)=Ax(t)\stackrel{·}{x}(t) = Ax(t)(6)

其中x(t)∈R3x(t) \in \mathbb{R}^3，A∈R3×3A \in \mathbb{R}^{3 \times 3}，且初始条件为x(0)=x0x(0) = x_0，那么解为：

x(t)=eAtx0\\ x(t) = e^{At}x_0(7)

其中矩阵指数函数可以展开为无穷级数的形式：

eAt=I+At+(At)22!+(At)33!+⋅⋅⋅\\ e^{At} = I + At + \frac{(At)^2}{2!} + \frac{(At)^3}{3!} + ···(8)

矩阵指数与矩阵旋转的关系

向量旋转

如上图，三维向量p(0)p(0)绕着单位旋转轴ω^\hat{\omega} （ω^∈R3\hat{\omega} \in \mathbb{R}^3，∣∣ω^∣∣=1\vert\vert \hat\omega \vert\vert = 1）旋转θ\theta角度，得到三维向量 p(θ)p(\theta)。该旋转运动亦可以视作是三维向量p(0)p(0)以1rad/s1rad/s的角速度绕着单位旋转轴ω^\hat{\omega}从时间t=0t = 0 运动到时间 t=θt = \theta得到三维向量p(θ)p(\theta)。

如果用p(t)p(t)来表示旋转路径上向量端点处的位置，用p⋅(t)\stackrel{·}{p}(t)表示该点的瞬时速度值，则有：

p⋅=ω^×p\stackrel{·}{p} = \hat{\omega} \times p(9)

而向量叉乘可以写为左端向量对应的反对称矩阵与右端向量的乘积形式：

p⋅=[ω^]p\stackrel{·}{p} = [\hat{\omega}]p(10)

其中，定义[⋅][·]为反对称矩阵算子，假如ω^={ω1ω2ω3}\hat{\omega} = \begin{Bmatrix} \omega_1 \\ \omega_2 \\ \omega_3 \end{Bmatrix}，那么有：

[ω]=[0−ω3ω2ω30−ω1−ω2ω10][\omega] = \begin{bmatrix} 0 & -\omega_{3} & \omega_{2} \\ \omega_{3} & 0 & -\omega_{1} \\ -\omega_{2} & \omega_{1} & 0 \end{bmatrix}(11)

那么上式就有如下解：

p(t)=e[ω^]tp(0)p(t) = e^{[\hat\omega]t}p(0)(12)

因为角速度为1rad/s1rad/s，所以这里角度θ\theta和时间tt是可以互换的：

p(t)=e[ω^]θp(0)p(t) = e^{[\hat\omega]\theta}p(0)(13)

由于反对称矩阵有满足[ω^]3=−[ω^]{[\hat\omega]}^3 = -[\hat\omega]，将矩阵指数函数展开有：

e[ω^]θ=I+[ω^]θ+[ω^]2θ22!+[ω^]3θ33!+⋅⋅⋅=I+(θ−θ33!+θ55!−⋅⋅⋅)[ω^]+（θ22!−θ44!+θ66!−⋅⋅⋅）[ω^]2=I+sin⁡θ[ω^]+(a−cos⁡θ)[ω^]2\begin{align} e^{[\hat\omega]\theta} &= I + [\hat\omega]\theta + [\hat\omega]^2\frac{\theta ^ 2}{2!} + [\hat\omega]^3\frac{\theta ^ 3}{3!} + ··· \\ &= I + (\theta - \frac{\theta ^ 3}{3!} + \frac{\theta ^ 5}{5!} - ···)[\hat\omega] + （\frac{\theta ^ 2}{2!} - \frac{\theta ^ 4}{4!} + \frac{\theta ^ 6}{6!}-···）[\hat\omega] ^ 2 \\ &= I + \sin{\theta}[\hat\omega] + (a-\cos{\theta})[\hat\omega] ^ 2 \end{align}(14)

因此，三维旋转矩阵RR就可以用矩阵指数e[ω^]θe^{[\hat{\omega}]\theta}的形式来表示。

李群和李代数

群

群是一种集合加上一种代数运算。这个集合和运算应该满足“封结幺逆”四个性质，把集合记为 A\mathbf{A} ，运算记为⋅\cdot, 群就可以记为 G=(A,⋅)\mathbf{G}=(\mathbf{A},\cdot)。

  • 封闭性：∀a1,a2∈A,a1⋅a2∈A\forall a_1,a_2 \in \mathbf{A},a_1 \cdot a_2 \in \mathbf{A}
  • 结合律：∀a1,a2,a3∈A,(a1⋅a2)⋅a3=a1⋅(a2⋅a3)\forall a_1,a_2,a_3 \in \mathbf{A}, (a_1 \cdot a_2) \cdot a_3 = a_1\cdot (a_2 \cdot a_3)
  • 幺元：∃a0∈A,s.t.∀a∈A,a0⋅a=a⋅a0\exists a_0 \in \mathbf{A}, s.t. \forall a \in \mathbf{A}, a_0 \cdot a = a \cdot a_0
  • 逆元：∀a∈A,∃a−1∈A,s.t. a⋅a−1=a0\forall a \in \mathbf{A},\exists a^{-1} \in A, \quad s.t. \space a \cdot a^{-1}=a_0

可以验证，三维旋转矩阵对矩阵乘法构成了特殊正交群SO(3)SO(3)——直观意义上接连两次旋转动作认为旋转。

SO(3)={R∈R3×3∣RRT=I,det(R)=1}SO(3) = \{R \in \mathbb{R}^{3 \times 3} | RR^T = I, det(R) = 1\}(15)

变换矩阵构成了特殊欧式群SE(3)SE(3)，它也对乘法封闭。

SE(3)={T=[Rt0T1]∈R4×4∣R∈SO(3),t∈R3×3}SE(3) = \{ T = \begin{bmatrix} R & t \\ {0^T} & 1 \\ \end{bmatrix} \in \mathbb{R}^{4\times 4} | R \in SO(3), t \in \mathbb{R}^{3\times 3}\}(16)

但是二者对加法都不封闭，即两个变换矩阵相加后并不是旋转矩阵。

李群

李群（Lie group）是具有群结构的光滑微分流形，其群作用与微分结构相容，从抽象上来说，李群就是具有光滑或连续性质的群。对应到我们的问题上，由于三维机器人在空间中移动必然是连续地运动，不可能发生“瞬移”的情形，因而SO(3)SO(3)和SE(3)SE(3)都是李群。

下面由旋转矩阵RR引出李代数。因为R是随时间连续变化的——相机可看成是连续运动，我们将相机姿态看作是时间tt的函数，那么自然就有R(t)R(t)T=IR(t)R(t)^T = I成立。上式的左右两侧同时对t求导，我们可得

R⋅(t)R(t)T+R(t)R⋅(t)T=0\stackrel{·}{R}(t)R(t)^T + R(t)\stackrel{·}{R}(t)^T = 0(17)

整理后可得

R⋅(t)R(t)T=−(R(t)R⋅(t)T)T\stackrel{·}{R}(t)R(t)^T = -{( R(t)\stackrel{·}{R}(t)^T )}^T(18)

如果我们把等式左边看成一个整体，那么我们会发现上式左边是一个反对陈矩阵。

对于任意一个反对称矩阵，我们都由一个向量做反对称变换得到，比如：

a∧=A=[0−a3a2a30−a1−a2a10],A∨=a={a1,a2,a3}.a^{\wedge} = A = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \\ \end{bmatrix}, \quad A^{\vee} = a = \{a_1, a_2, a_3\}.(19)

如果将上式左边记为ϕ(t)∧=R⋅(t)R(t)T∈R3\phi(t)^{\wedge} = \stackrel{·}{R}(t)R(t)^T \in \mathbb{R}^3，我们对这个式子两边右乘R(t)R(t)并注意到R(t)R(t)的正交矩阵性质，就可以得到：

R⋅(t)=ϕ∧(t)R(t)\stackrel{·}{R}(t) = \phi^{\wedge}(t)R(t)(20)

就好像对R(t)R(t)求导后其实是左乘了一个ϕ(t)\phi(t)，而上边这个等式就类似一个一阶微分方程，ϕ\phi反应了一节导数的性质，它位于正切空间上(tangent space)。我们认为初始值R(0)=IR(0) = I，那么就可以解得：

R(t)=eϕ∧tR(t) = e^{\phi ^ \wedge t}(21)

由于这个式子只在tt附近有效。由此可知，对任意时刻tt，旋转矩阵RR和一个向量ϕ\phi对应，二者满足矩阵指数关系，这个ϕ\phi就是SO（3）SO（3）上的李代数so（3）so（3）。因而，为了求RR，我们只需要求出exp(ϕt)exp(\phi t)即可，这个关系即为指数映射。现在的问题是so(3)so(3)是如何定义的，指数映射又如何求出呢？

李代数

每个李群都有与之对应的李代数，李代数描述了李群的局部性质。在这个局部正切空间里，一些我们可以找到一些比较好的性质，比如良好定义的加法，从而我们有了良好定义的导数——将李群的导数转化为李代数上的导数，求出来后再转化回去，这就是我们处理李群导数相关内容的基本思路，同时也是研究的重心。

李代数由一个集合VV, 一个数域FF 和一个二元运算[⋅][\cdot] 组成。如果它们满足以下几条性质，则称(V,F,[⋅])(V, F, [\cdot]) 为一个李代数，记作gg，其中二元运算[⋅][\cdot]成为离括号，表达了两个元素的差异。

  • 封闭性： ∀X,Y∈V, [X,Y]∈V\forall \mathbf{X}, \mathbf{Y} \in V,\ [\mathbf{X}, \mathbf{Y}] \in V
  • 双线性： ∀X,Y,Z∈V, a,b∈F\forall \mathbf{X}, \mathbf{Y}, \mathbf{Z} \in V,\ a, b \in F , 有 [aX+bY, Z]=a[X,Z]+[Y,Z], [Z,aX+bY]=a[Z,X]+b[Z,Y][a\mathbf{X} +b\mathbf{Y},\ \mathbf{Z}]=a[\mathbf{X}, \mathbf{Z}] + [\mathbf{Y}, \mathbf{Z}],\ [\mathbf{Z},a\mathbf{X} +b\mathbf{Y}]=a[\mathbf{Z},\mathbf{X}] +b[\mathbf{Z},\mathbf{Y}]
  • 自反性： ∀X∈V, [X,X]=0\forall \mathbf{X} \in V,\ [\mathbf{X}, \mathbf{X}]=\mathbf{0}
  • 雅可比等价：∀X,Y,Z∈V, [X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0\forall \mathbf{X},\mathbf{Y},\mathbf{Z} \in V,\ [\mathbf{X},[\mathbf{Y},\mathbf{Z}]] + [\mathbf{Z},[\mathbf{X},\mathbf{Y}]] +[\mathbf{Y},[\mathbf{Z},\mathbf{X}]]=\mathbf{0}

举例三位向量R3\mathbb{R}^3上定义的叉乘就是一个离括号，其中g=(R3,R,×)g = (\mathbb{R}^3, \mathbb{R}, \times)就构成了李代数。

李代数so(3)so(3)

之前提到的SO(3)SO(3)对应的向量ϕ\phi就是一种李代数，他定义在R3\mathbb{R}^3的向量上。它的反对称矩阵记作：Φ=ϕ∧\mathbf{\Phi} = \phi^\wedge，那么SO(3)中的向量对应的李括号为：

[ϕ1,ϕ2]=(Φ1Φ2−Φ2Φ1)∨[\phi_1, \phi_2] = (\mathbf{\Phi}_1\mathbf{\Phi}_2 - \mathbf{\Phi}_2\mathbf{\Phi}_1)^{\vee}(22)

经过验证可以得到上边的式子等价于两向量作外积。

[ϕ1,ϕ2]=ϕ1×ϕ2[\phi_1, \phi_2] = \phi_1 \times \phi_2(23)

李代数se(3)se(3)

与so(3)so(3)不同的是，se(3)se(3)定义在R6\mathbb{R}^6空间中。

se(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧ρ0T0]∈R4×4}se(3)=\begin{Bmatrix} \mathbf{\xi}=\begin{bmatrix} \mathbf{\rho} \\ \mathbf{\phi} \end{bmatrix} \in R^6,\mathbf{\rho} \in R^3, \mathbf{\phi} \in so(3), \mathbf{\xi}^\wedge=\begin{bmatrix}\phi^\wedge & \mathbf{\rho} \\ \mathbf{0}^T & 0 \end{bmatrix} \in R^{4 \times 4} \end{Bmatrix}(24)

其中se(3)se(3)每个元素都称为ξ\xi，它是一个六维向量：前三维为平移，记作ρ\rho，后三位即为三维旋转向量ϕ\phi，实质上就是so(3)so(3)中的元素。另外在se(3)se(3) 中， ∧\wedge 符号的含义被拓展了：这里它将一个六维向量转换为四维矩阵，但这里不再表示反对称矩阵，其实这里ξ\xi即为变换矩阵的转置。同样，李代数se(3)se(3)的李括号被定义为：

[ξ1,ξ2]=(ξ1∧ξ2∧−ξ2∧ξ1∧)∨[\xi_1, \xi_2]=(\xi_1^\wedge\xi_2^\wedge-\xi_2^\wedge\xi_1^\wedge)^\vee(25)

上式经验证后发现，李括号将两个变换矩阵变成了了一个无旋的变换矩阵：

[ξ1,ξ2]={Φ1∧ρ2−Φ2∧ρ103×3}[\xi_1, \xi_2] = \begin{Bmatrix} \Phi_1^{\wedge}\rho_2 - \Phi_2^{\wedge}\rho_1 \\ \mathbf{0}_{3\times 3} \end{Bmatrix}(26)

即旋转部分被抹去，只剩下平移部分，而平移部分是由原来两个元素中的旋转和平移部分组合得到的。

指数映射和对数映射

李群到李代数之间是一个指数映射（反之是对数映射），通过借助这个指数映射我们可以将大量在李群上不好解决的问题转嫁到李代数上解决，之后再映射回去间接得到李群上的答案。

SO(3)SO(3)上的指数映射

我们利用关系李群和李代数之间的关系：

R⋅(t)=ϕ∧(t)R(t)\stackrel{·}{R}(t) = \phi^{\wedge}(t)R(t)(27)

可知求出起初指数映射，就能得到李群上的导数。求指数映射我们需要用到泰勒展开式。任意矩阵的指数映射都可以写成一个泰勒展开式，这个展开式只有在收敛的情况下才有解，结果仍然是一个矩阵。所以对于so(3)so(3)中的元素ϕ\phi，其指数映射可以写成

exp(ϕ∧)=∑n=0∞1n!(ϕ∧)nexp(\phi^{\wedge}) = \sum\limits_{n=0}^\infty\frac{1}{n!}(\phi^\wedge)^n(28)

我们定义ϕ=θa\phi = \theta \mathbf{a}，其中θ\theta和aa分别是ϕ\phi的模长和单位向量。我们可以验证：

a∧a∧=aaT−Ia∧a∧a∧=−a∧\mathbf{a}^\wedge \mathbf{a}^\wedge=\mathbf{a}\mathbf{a}^T-\mathbf{I} \\ \mathbf{a}^\wedge \mathbf{a}^\wedge \mathbf{a}^\wedge=-\mathbf{a}^\wedge(29)

以上两个式子提供了我们对泰勒展开式中高次项化简的方法。 那么我们对上边泰勒展开式化简可以得到：

exp⁡ϕ∧=exp⁡(θa)=∑n=0∞1n!(θa∧)n=cos⁡θI+(1−cos⁡θ)aaT+sin⁡θaT\exp{\phi^\wedge}=\exp(\theta \mathbf{a})=\sum\limits_{n=0}^\infty \frac{1}{n!}(\theta \mathbf{a}^\wedge)^n \\ =\cos\theta\mathbf{I} + (1-\cos\theta)\mathbf{a}\mathbf{a}^T + \sin\theta\mathbf{a}^T(30)

这和之前的罗德里格斯公式有着相同的形式，标明so(3)so(3)实际上就是由旋转向量组成的空间，而指数映射其实就是罗德里格斯公式。那么与之对应的对数映射，给定旋转矩阵，我们就可以求李代数，将SO(3)SO(3)映射到so(3)so(3)上：

ϕ=ln⁡(R)∨=(∑n=0∞(−1)nn+1(R−I)n+1)∨\phi=\ln(R)^\vee=(\sum\limits_{n=0}^\infty\frac{(-1)^n}{n+1}(\mathbf{R}-\mathbf{I})^{n+1})^\vee(31)

但是实际上没必要这样求，因为旋转向量已经介绍了矩阵到向量的转换关系：

θ=arccos⁡tr(R)−12Rn=n\theta = \arccos{\frac{tr(\mathbf{R}) - 1}{2}} \\ \mathbf{R}n = n(32)

这里需要注意的是，指数映射是一个满射，即每个SO(3)SO(3)中的元素都可以找到一个so(3)so(3)里的元素与之对应；但是泛指不成立，因为多旋转一圈的旋转向量对应的李代数是相同的。但是如果把旋转角固定在[0,2π][0, 2\pi]之间，那么李群和李代数的元素就是一一对应的。

SE(3)SE(3)上的指数映射

下面是推导结果：

exp⁡(ξ∧)=[∑n=0∞1n!(ϕ∧)n∑n=0∞1(n+1)!(ϕ∧)nρ0T1]=[RJρ0T1]=T\exp(\xi^\wedge)=\begin{bmatrix} \sum\limits_{n=0}^\infty\frac{1}{n!}(\phi^\wedge)^n & \sum\limits_{n=0}^\infty\frac{1}{(n+1)!}(\phi^\wedge)^n\rho \\ \mathbf{0}^T & 1 \\ \end{bmatrix}= \begin{bmatrix} \mathbf{R} & \mathbf{J}\rho \\ \mathbf{0}^T & 1 \\ \end{bmatrix}=\mathbf{T}(33)

其中exp(ξ∧)exp(\xi^{\wedge})的左上角是SO(3)的元素，即旋转部分。矩阵J\mathbf{J}则为：

J=sin⁡θθI+(1−sin⁡θθ)aaT+1−cos⁡θθa∧\mathbf{J} = \frac{\sin\theta}{\theta}\mathbf{I}+(1-\frac{\sin\theta}{\theta})\mathbf{a}\mathbf{a}^T+\frac{1-\cos\theta}{\theta}\mathbf{a}^\wedge(34)

这个公式与落地里格斯公式类似，ξ\xi中的平一部分经过ρ\rho指数变换后，发生了一次以J\mathbf{J}为系数的线性变换。

相反的，SE(3)SE(3)到se(3)se(3)也有相应的对数映射，但是我们一般不用，我们一般利用左上角的旋转矩阵计算出旋转向量，再用右上角的平移向量得到ρ\rho。

李代数求导与扰动模型

李代数与李群的计算对应关系

我们由于李群没有加法讨论出来了李代数，我们想要利用李代数的加法定义李群的导数，再利用指数映射和对数映射完成变换关系。但是还有一个基本问题，就是在李代数上做加法，是否就等价于在李群上做乘法呢？

exp(ϕ1∧)exp(ϕ2∧)=exp((ϕ1+ϕ2)∧)exp(\phi_1^{\wedge})exp(\phi_2^{\wedge}) = exp({(\phi_1 + \phi_2)}^{\wedge})(35)

上式在标量的情况下显然是成立的，但是现在指数上式矩阵，结果很遗憾是不成立的。我们由BCH(Baker-Campbell-Hausdorff)公式给出：

ln⁡(exp⁡(A)exp⁡(B))=A+B+112[A,B]+112[A,[A,B]]−112[B,[A,B]]+…\ln(\exp(\mathbf{A})\exp(\mathbf{B}))=\mathbf{A}+\mathbf{B}+\frac{1}{12}[\mathbf{A},\mathbf{B}]+\frac{1}{12}[\mathbf{A},[\mathbf{A},\mathbf{B}]]-\frac{1}{12}[\mathbf{B},[\mathbf{A},\mathbf{B}]]+\ldots(36)

其中[⋅][\cdot]为李括号，可以看到，两个矩阵指数的乘积结果的指数项，是两个矩阵之和再加上一些由李括号组成的余项。

我们在求导数的时候，其中一项肯定是小量，可以看成相机连续两帧之间的位姿变化，利用这个小量，我们就可以对上边的式子进行优化：

ln⁡(exp⁡(ϕ1∧)exp⁡(ϕ2∧))∨≈{Jl(ϕ2)−1ϕ1+ϕ2, 当ϕ1为小量Jr(ϕ1)−1ϕ2+ϕ1, 当ϕ2为小量\ln(\exp(\phi^\wedge_1)\exp(\phi^\wedge_2))^\vee \approx \begin{cases} \mathbf{J}_l(\phi_2)^{-1}\phi_1 + \phi2,\ 当\phi_1 为小量\\ \mathbf{J}_r(\phi_1)^{-1}\phi_2 + \phi_1,\ 当\phi_2 为小量\\ \end{cases}(37)

上边的优化结果分别对应着ϕ1\phi_1和ϕ2\phi_2为小量时的近似状态，我们分别称为左乘模型和右乘模型。之所以要区分做成还是右乘模型，是因为其中的Jl\mathbf{J}_l和Jr\mathbf{J}_r是不一样的。对于左乘BCH近似模型，雅可比矩阵Jl\mathbf{J}_l就是：

Jl=J=sin⁡θθI+(1−sin⁡θθ)aaT+1−cos⁡θθa∧\mathbf{J}_l = \mathbf{J} = \frac{\sin\theta}{\theta}\mathbf{I}+(1-\frac{\sin\theta}{\theta})\mathbf{a}\mathbf{a}^T+\frac{1-\cos\theta}{\theta}\mathbf{a}^\wedge(38)

它的逆为：

Jl−1=θ2cot⁡θ2I+(1−θ2cot⁡θ2)aaT−θ2a∧\mathbf{J}_l^{-1}=\frac{\theta}{2}\cot\frac{\theta}{2}\mathbf{I}+(1-\frac{\theta}{2}\cot\frac{\theta}{2})\mathbf{a}\mathbf{a}^T-\frac{\theta}{2}\mathbf{a}^\wedge(39)

而对于右乘模型，右乘雅可比矩阵就是将左乘雅可比矩阵的自变量取负号。

Jr(ϕ)=Jl(ϕ)\mathbf{J}_r(\phi) = \mathbf{J}_l(\phi)(40)

现在来小结一下，假如有一个旋转矩阵R\mathbf{R}，对应的李代数为ϕ\phi，现在对它有一个微小的扰动，记作ΔR\mathbf{\Delta R}，对应的李代数为Δϕ\Delta \phi。那么在李群上，新的旋转矩阵即为δR⋅R\mathbf{\delta R \cdot R}，而在李代数上，新的李代数为Jl−1Δϕ+ϕ\mathbf{J}_l^{-1}\Delta \phi + \phi，即：

exp⁡(ϕ∧)exp⁡(Δϕ∧)=exp⁡((ϕ+Jl(ϕ)−1Δϕ)∧)\exp(\phi^\wedge)\exp(\Delta\phi^\wedge)=\exp((\phi+\mathbf{J}_l(\phi)^{-1}\Delta\phi)^\wedge)(41)

反之，如果我们在李代数上做加法，那么可以近似为李群上带左/右雅可比矩阵的乘法：

exp((ϕ+Δϕ)∧)=exp((JlΔϕ)∧)exp(ϕ∧)=exp(ϕ∧)exp((JrΔϕ)∧)exp((\phi + \Delta \phi)^\wedge) = exp((\mathbf{J_l}\Delta\phi)^\wedge)exp(\phi^\wedge) = exp(\phi^\wedge)exp((\mathbf{J_r}\Delta\phi)^\wedge)(42)

至此，我们可以讨论一下怎么求没有良好定义加法的李群上的导数：

  1. 利用李代数表示位子，然后根据李代数的加法对李代数进行求导。（导数模型）
  2. 对李群来左乘或右乘微小扰动，并对这个扰动的李代数求导。（扰动模型）

求导模型

假设空间点p\mathbf{p}进行了一次旋转R\mathbf{R}，得到了新的点Rp\mathbf{Rp}，要计算旋转后的点相对于旋转的导数，可以不严谨得写为：

∂(Rp)∂R\frac{\partial(\mathbf{R}\mathbf{p})}{\partial\mathbf{R}}(43)

设R\mathbf{R}对应的李代数为ϕ\phi，就可以写成：

∂(Rp)∂R=∂(exp⁡(ϕ∧)p)∂ϕ\frac{\partial(\mathbf{R}\mathbf{p})}{\partial\mathbf{R}} = \frac{\partial(\exp(\phi^\wedge)\mathbf{p})}{\partial\phi}(44)

按照导数的定义并利用一阶泰勒展开以及BCH公式，就可以得到：

∂(exp⁡(ϕ∧)p)∂ϕ=lim⁡δϕ→0exp⁡((ϕ+δϕ)∧)p−exp⁡(ϕ∧)pδϕ=−(Rp)∧Jl\frac{\partial(\exp(\phi^\wedge)\mathbf{p})}{\partial\phi}=\lim_{\delta\phi\to0}\frac{\exp((\phi+\delta\phi)^\wedge)\mathbf{p}-\exp(\phi^\wedge)\mathbf{p}}{\delta\phi}=-(\mathbf{R}\mathbf{p})^\wedge\mathbf{J}_l(45)

这个公式时直接对旋转矩阵R\mathbf{R}进行求导，最后结果含有左雅可比矩阵Jl\mathbf{J_l}。当然，我们自然希望避免计算雅可比矩阵，所以我们一般选用下边的扰动模型。

扰动模型

假设某空间点p\mathbf{p} 经过了一次变换T\mathbf{T}，对应的李代数为ξ\xi，得到Tp\mathbf{Tp}。给 T\mathbf{T}左乘一个微小扰动ΔT\mathbf{\Delta T}，扰动项的李代数为δξ=[δρ,δϕ]T\delta \xi = [\delta \rho , \delta \phi]^T。则有：

\frac{\partial(\mathbf{T}\mathbf{p})}{\partial\delta\xi}=\lim_{\delta\xi\to0}\frac{\exp(\delta\xi^\wedge)\exp(\xi^\wedge)\mathbf{p}-\exp(\xi^\wedge)\mathbf{p}}{\delta\xi}= \begin{bmatrix} \mathbf{I}_{3\times3} & -(\mathbf{R}\mathbf{p} + \mathbf{t})^\wedge_{3\times3} \\ \mathbf{0}^T_{1\times3} & \mathbf{0}^T_{1\times3}\\ \end{bmatrix}=(\mathbf{T}\mathbf{p})^\bigodot

最后结果被定义为了运算符号⨀\bigodot，把一个齐次坐标下的空间点变换成一个4*6 的矩阵。

Cite 
@misc{zhang2020liegroup,
  author = {Zhang, Yihua},
  title  = {Visual Slam笔记——李群和李代数},
  year   = {2020},
  url    = {https://normaluhr.github.io/en/p/lie-group}
}