＜视觉SLAM十四讲＞李群与李代数

三维世界中描述刚体运动可以使用旋转矩阵,旋转向量,欧拉角,四元数等.

但是在SLAM中,除了表示之外,我们还要对它们进行估计和优化.因为在SLAM中位姿是未知的,而我们需要解决形如"在什么样的相机位姿最符合当前观测数据"这样的问题.一种典型的方式是将它构建成一个优化问题,求解最优的R,t,使得误差最小化.

比如说相机旋转,我们估计它旋转了15度,但是观测值是16度,那么如何将我们的估计值调整到16度呢,这就牵涉到了怎么求目标函数关于运动的导数(目标函数是关于旋转或关于运动的函数f®),也就是说,我们需要弄清楚当运动发生变化的时候,目标函数是怎么变化的.

旋转矩阵自身是带有约束的(正交且行列式为1),它们作为优化变量时,会引入额外的约束,使优化变得复杂困难.通过李群–李代数之间的转换关系,可以把位姿估计变成无约束的优化问题,简化求解方式.

个人理解：为什么要引入李群与李代数？

在slam问题中，进行位姿估计以及优化等操作的时候，就不可避免地需要对目标地运动进行求导，而导数的本质就是一个函数值减去另外一个函数值然后再除以一个无穷小量。比如说我们需要对相机的运动进行估计，相机的位姿我们可以由旋转矩阵以及平移向量来表述，所以很自然地，在进行运动求导地时候就一定会进行类似的相机旋转矩阵加减的运算。这就出现了一个很致命的问题：旋转矩阵之间进行乘法运算的时候具有封闭性，即旋转矩阵乘以一个旋转矩阵还是一个旋转矩阵，但是它对加法不具有封闭性(一个旋转矩阵加上一个旋转矩阵得到的矩阵就不一定是一个旋转矩阵了)。这也就意味着，我们无法通过对旋转矩阵求导完成相机的运动估计以及优化等问题。

而李群与李代数的引入就解决了这个问题。在SLAM中，我们可以直接将旋转矩阵当成李群，由旋转矩阵推导出来的旋转向量我们可以直接当成李代数。我们通过李群与李代数，将原本旋转矩阵(李群)的乘法运算转换成旋转向量(李代数)的加法运算，然后再利用李群与李代数之间的指数映射与对数映射，将通过加法运算得出来的结果映射回李群，这样就相当于间接地完成了旋转矩阵的加法运算，然后就可以对运动进行求导。

1.李群

三维旋转矩阵构成了特殊正交群SO(3):
SO(3)={R∈R3×3∣RRT=I,det(R)=1}SO(3)=\left\{ R \in \mathbb{R}^{3 \times 3} \ | \ RR^T=I,det(R)=1 \right\} SO(3)={R∈R3×3 ∣ RRT=I,det(R)=1}
三维变换矩阵构成了特殊欧式群SE(3):
SE(3)={T=[Rt0T1]∈R4×4∣R∈SO(3),t∈R3}SE(3)=\left\{ T=\left[ \begin{matrix} R & t\\ 0^T & 1 \end{matrix} \right] \in \mathbb{R}^{4 \times 4}\ | \ R \in SO(3),t \in \mathbb{R}^3 \right\} SE(3)={T=[R0Tt1]∈R4×4 ∣ R∈SO(3),t∈R3}

什么是群?
群是一种集合加上一种运算的代数结构,我们把集合记作A,运算记作"⋅\cdot⋅",那么群可以记作G=(A,⋅)G=(A,\cdot)G=(A,⋅).群要求运算要满足一下几个条件: **封结幺逆 **
1.封闭率
2.结合律
3.幺元
4.逆

其中,封闭率和结合率不必解释.幺元的意思就是说,在集合A中,一定要存在一个元素(a0a_0a0),使得这个元素和任意其他集合中的元素(aaa)进行运算的时候都等于那个元素,比如说,如果这个集合是整数集,运算是加法,那么这个幺元就是0,因为任何数与0相加都不变;如果运算是乘法,那么这个幺元就是1,因为任何数与1相乘都等于它本身.

至于第四个条件逆的意思就是说,在集合A中,对于任何一个元素a,都有另外一个元素b,它们两个的运算结果为1

李群是指具有连续(光滑)性质的群.–整数群是离散的,所以不是李群

直观上看,一个刚体能够连续地在空间中运动,所以SO(3)和SE(3)都是李群.但是SO(3)和SE(3)只有定义良好地乘法,没有加法(关于加法不封闭),所以很难进行取极限,求导等操作.但是在后面进行位姿估计与优化的时候需要对相近的运动进行求导,否则我们是无法确定什么样的位姿更适合当前的观测的.所以有了下面的李代数.

2.李代数,指数映射

李代数:与李群相对应的一种结构,位于向量空间.
- 通常以小写的so(3)和se(3)表示,书中以哥特体突出显示.
- 事实上是李群单位元处的正切空间.
- 每个李群都有与之对应的李代数(如SO(3)SO(3)SO(3)–so(3)so(3)so(3), SE(3)SE(3)SE(3)–se(3)se(3)se(3))
李代数的引出:
- 任意旋转矩阵RRR,它满足:RRT=IRR^T=IRRT=I.如果它表示某个相机的旋转,随时间连续变化,那么它可以这么表示:
  R(t)R(t)T=IR(t)R(t)^T=I R(t)R(t)T=I
  等式两边对时间求导,得:
  R˙(t)R(t)T+R(T)R˙(t)T=0=>R˙(t)R(t)T=−(R˙(t)R(t)T)T\dot{R}(t)R(t)^T+R(T)\dot{R}(t)^T=0 \\ =>\ \dot{R}(t)R(t)^T=-(\dot{R}(t)R(t)^T)^T R˙(t)R(t)T+R(T)R˙(t)T=0=> R˙(t)R(t)T=−(R˙(t)R(t)T)T
  可以看出,R˙(t)R(t)T\dot{R}(t)R(t)^TR˙(t)R(t)T是一个反对称矩阵.那么我们就可以找到一个三维向量ϕ(t)∈R3\phi(t) \in \mathbb{R}^3ϕ(t)∈R3与它对应:
  R˙(t)R(t)T=ϕ(t)∧\dot{R}(t)R(t)^T=\phi(t)^{\wedge} R˙(t)R(t)T=ϕ(t)∧
  等式两边右乘RRR,由于RRR为正交阵,有:
  R˙(t)=ϕ(t)R(t)=[0−ϕ3ϕ2ϕ30−ϕ1−ϕ2ϕ10]R(t)\dot{R}(t)=\phi(t)R(t)=\left[ \begin{matrix} 0 & -\phi_3 & \phi_2\\ \phi_3 & 0 & -\phi_1\\ -\phi_2 & \phi_1 & 0 \end{matrix} \right]R(t) R˙(t)=ϕ(t)R(t)=⎣⎡0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎦⎤R(t)
  可以看到每对旋转矩阵求一次导数,就相当于左乘一个ϕ(t)T\phi(t)^Tϕ(t)T矩阵即可.可以看到,上式是一个关于R(t)R(t)R(t)的微分方程.考虑t0=0t_0=0t0=0时,设此时旋转矩阵为R(0)=IR(0)=IR(0)=I,同时在t0t_0t0附近,设ϕ\phiϕ保持为常数ϕ(t0)=ϕ0\phi(t_0)=\phi_0ϕ(t0)=ϕ0.那么就有:
  R˙(t)=ϕ(t0)∧R(t)=ϕ0∧R(t)\dot{R}(t)=\phi(t_0)^{\wedge}R(t)=\phi_0^{\wedge}R(t)\\ R˙(t)=ϕ(t0)∧R(t)=ϕ0∧R(t)
  解这个微分方程方程得:
  R(t)=exp(ϕ0∧t)\color{fuchsia} R(t)=exp(\phi_0^{\wedge}t) R(t)=exp(ϕ0∧t)
  这说明:在t=0t=0t=0附近,旋转矩阵可以由 exp(ϕ0∧t)exp(\phi_0^{\wedge}t)exp(ϕ0∧t) 计算出来.
上面的ϕ\phiϕ就是一种李代数.SO(3)SO(3)SO(3)对应的李代数是定义在R3\mathbb{R}^3R3上的向量,记作ϕ\phiϕ.每一个ϕ\phiϕ都有一个反对称矩阵:
Φ=ϕ∧=[0−ϕ3ϕ2ϕ30−ϕ1−ϕ2ϕ10]∈R3×3\Phi=\phi^{\wedge}=\left[ \begin{matrix} 0 & -\phi_3 & \phi_2\\ \phi_3 & 0 & -\phi_1\\ -\phi_2 & \phi_1 & 0 \end{matrix} \right]\in \mathbb{R}^{3 \times 3} Φ=ϕ∧=⎣⎡0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10⎦⎤∈R3×3

那么又如何计算exp(ϕ0∧)exp(\phi_0^{\wedge})exp(ϕ0∧)?显然它是一个矩阵的指数,在李群和李代数中,称它为指数映射.

任意矩阵的指数映射,我们可以将它泰勒展开:
exp(A)=∑n=0∞1n!Anexp(A)=\sum_{n=0}^{\infty}\frac{1}{n!}A^n exp(A)=n=0∑∞n!1An
同样的,对so(3)中的任意元素ϕ\phiϕ,我们也可以按此方式定义它的指数映射:
exp(ϕ∧)=∑n=0∞1n!(ϕ∧)nexp(\phi^{\wedge})=\sum_{n=0}^{\infty}\frac{1}{n!}{(\phi^{\wedge})}^n exp(ϕ∧)=n=0∑∞n!1(ϕ∧)n
但是这个式子中包含了矩阵的无穷次幂,是不好直接计算的. 因为ϕ\phiϕ是三维向量,我们可以定义ϕ\phiϕ的模长和方向:

ϕ\phiϕ的模长记作: θ\thetaθ ϕ\phiϕ的方向记作: a\boldsymbol{a}a

这里a\boldsymbol{a}a是一个长度为1的方向向量,即∣∣a∣∣=1\left| \right | a \left| \right |=1∣∣a∣∣=1.对于a\boldsymbol{a}a,有一下两条性质:
a∧a∧=[−a22−a32a1a2a1a3a1a2−a12−a32a2a3a1a3a2a3−a12−a22]∈R3×3\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}= \left[ \begin{matrix} -a_2^2-a_3^2 & a_1a_2 & a_1a_3 \\ a_1a_2 & -a_1^2-a_3^2 & a_2a_3 \\ a_1a_3 & a_2a_3 & -a_1^2-a_2^2 \end{matrix} \right]\in \mathbb{R}^{3 \times 3} \\ a∧a∧=⎣⎡−a22−a32a1a2a1a3a1a2−a12−a32a2a3a1a3a2a3−a12−a22⎦⎤∈R3×3
a∧a∧a∧=a∧(aaT−I=−a∧)\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}=\boldsymbol{a}^{\wedge}(\boldsymbol{a}\boldsymbol{a}^T-I=-\boldsymbol{a}^{\wedge}) a∧a∧a∧=a∧(aaT−I=−a∧)
这两个式子提供了处理a∧\boldsymbol{a}^{\wedge}a∧高阶项的做法.我们就可以将指数映射写成:

exp(ϕ∧)=exp(θa∧)=∑n=0∞1n!(a∧)n=I+θa∧+12!θ2a∧a∧+13!θ3a∧a∧a∧+14!θ4(a∧)4+...=...=cosθI+(1−cosθ)aaT+sinθa∧\begin{aligned} exp(\phi^{\wedge})&=exp(\theta \boldsymbol{a}^{\wedge} ) = \sum_{n=0}^{\infty}\frac{1}{n!}{(\boldsymbol{a}^{\wedge})}^n\\ &=I+\theta \boldsymbol{a}^{\wedge} +\frac{1}{2!}\theta ^2\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge} + \frac{1}{3!}\theta ^3\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}+ \frac{1}{4!}\theta ^4(\boldsymbol{a}^{\wedge})^4+\ ...\\ &=\ ... \\ &=cos\theta I + (1-cos\theta)\boldsymbol{a}\boldsymbol{a}^T+sin\theta \boldsymbol{a}^{\wedge} \end{aligned} exp(ϕ∧)=exp(θa∧)=n=0∑∞n!1(a∧)n=I+θa∧+2!1θ2a∧a∧+3!1θ3a∧a∧a∧+4!1θ4(a∧)4+ ...= ...=cosθI+(1−cosθ)aaT+sinθa∧

可以看到,最后的结果,将exp(ϕ0∧)exp(\phi_0^{\wedge})exp(ϕ0∧)进行泰勒展开之后得到了这样一个式子:
exp(θa∧)=cosθI+(1−cosθ)aaT+sinθa∧exp(\theta \boldsymbol{a}^{\wedge} )=cos\theta I + (1-cos\theta)\boldsymbol{a}\boldsymbol{a}^T+sin\theta \boldsymbol{a}^{\wedge} exp(θa∧)=cosθI+(1−cosθ)aaT+sinθa∧

即罗德里格斯公式.这表明so(3)实际上就是又所谓的旋转向量组成的空间,而所谓的指数映射就是上面的这个罗德里格斯公式 .通过它们,我们将so(3)中的任意一个向量对应到了一个位于SO(3)中的旋转矩阵.

反之,如果定义对数映射,也能把SO(3)中的元素对应到so(3)中:
ϕ=ln(R)∨=(∑n=0∞(−1)nn+1(R−I)n+1)∨=>θ=arccostr(R)−12,Ra=a\phi=ln(R)^{\vee}=\left (\sum_{n=0}^{\infty}\frac{(-1)^n}{n+1}(R-I)^{n+1}\right )^{\vee}\\ =>\ \ \theta =arccos\frac{tr(R)-1}{2}\ ,\ \ Ra=a ϕ=ln(R)∨=(n=0∑∞n+1(−1)n(R−I)n+1)∨=> θ=arccos2tr(R)−1 , Ra=a

上面是关于李代数so(3)及其指数映射与对数映射

下面是关于李代数se(3)及其指数映射与对数映射

对于SE(3),它也有对应的李代数se(3).与so(3)类似,se(3)位于R6\mathbb{R}^6R6空间中:
se(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧ρ0T0]∈R4×4}se(3)=\left\{ \xi= \left[\begin{matrix} \rho \\ \phi \end{matrix} \right] \in \mathbb{R}^6, \rho\in\mathbb{R}^3, \phi\in so(3), \xi^{\wedge}=\left[\begin{matrix} \phi^{\wedge} & \rho \\ \bold{0}^T & 0 \end{matrix} \right] \in \mathbb{R}^{4 \times 4} \right\} se(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0Tρ0]∈R4×4}
我们把们个se(3)元素记作ξ\xiξ,它是一个六维向量,前三维为平移(ρ\rhoρ),后三维为旋转(ϕ\phiϕ).

se(3)的指数映射如下:
exp(ξ∧)=[∑n=0∞1n!(ϕ∧)n∑n=0∞1(n+1)!(ϕ∧)nρ01]≜[RJρ01]=T\begin{aligned} exp(\xi^{\wedge})&=\left [ \begin{matrix} \sum_{n=0}^{\infty}\frac{1}{n!}{(\phi^{\wedge})}^n & \sum_{n=0}^{\infty}\frac{1}{(n+1)!}{(\phi^{\wedge})}^n\rho \\ \boldsymbol{0} & 1 \end{matrix} \right ] \\ &\triangleq \left [ \begin{matrix} R & J\rho \\ \boldsymbol{0} & 1 \end{matrix} \right ]=\boldsymbol{T} \end{aligned} exp(ξ∧)=[∑n=0∞n!1(ϕ∧)n0∑n=0∞(n+1)!1(ϕ∧)nρ1]≜[R0Jρ1]=T

其中JJJ:
J=sinθθI+(1−sinθθaaT+1−cosθθa∧)J=\frac{sin\theta }{\theta} I + \left( 1-\frac{sin\theta}{\theta}\boldsymbol{a}\boldsymbol{a}^T+ \frac{1-cos\theta}{\theta}\boldsymbol{a}^{\wedge} \right) J=θsinθI+(1−θsinθaaT+θ1−cosθa∧)
该式与罗德里格斯公式类似,但不完全一样. 可以看到,平移部分经过指数映射之后,发生了一次以JJJ为系数矩阵的线性变换.

反过来,对数映射如下:
θ=arccostr(R)−12,Ra=a,t=Jρ\theta =arccos\frac{tr(R)-1}{2}\ ,\ \ Ra=a\ ,\ \ t=J\rho θ=arccos2tr(R)−1 , Ra=a , t=Jρ

3.李代数求导与扰动模型

待补充…