视觉SLAM学习打卡【3】-李群和李代数

• 为了求得符合当前观测数据的最佳相机位姿（计算目标误差函数对变换矩阵/旋转矩阵的导数，即位姿估计问题），而旋转矩阵因为正交性产生的约束（加法不封闭，无法直接求导）使优化变得困难，特引出李群和李代数的概念。
• 书中本节公式推导较多，涉及到解微分方程、泰勒展开、叉积的反对称矩阵表示等基础数学知识，以及Baker-Campbell-Hausdorff公式（BCH公式）& Rodrigues’s Formula（罗德里格斯公式）。

一、基础概念

1.群

• 群=集合+一种运算，运算满足以下几条（注：此处运算不一定是乘法）：“凤姐咬你”
  $$\begin{array}{rl}& 1.\text{ 封闭性: }\forall {\mathrm{a}}_1,{\mathrm{a}}_2\in \mathrm{A},{\mathrm{a}}_1\cdot {\mathrm{a}}_2\in \mathrm{A}.\\ & \text{2. 结合律: }\forall {\mathrm{a}}_1,{\mathrm{a}}_2,{\mathrm{a}}_3\in \mathrm{A},\left({\mathrm{a}}_1\cdot {\mathrm{a}}_2\right)\cdot {\mathrm{a}}_3={\mathrm{a}}_1\cdot \left({\mathrm{a}}_2\cdot {\mathrm{a}}_3\right)\\ & \text{3. 幺元: }\exists {\mathrm{a}}_0\in \mathrm{A},s.t.\forall \mathrm{a}\in \mathrm{A},{\mathrm{a}}_0\cdot \mathrm{a}=\mathrm{a}\cdot {\mathrm{a}}_0=\mathrm{a}\\ & \text{4. 逆: }\forall \mathrm{a}\in \mathrm{A},\exists {\mathrm{a}}^{-1}\in \mathrm{A},\mathrm{a}\cdot {\mathrm{a}}^{-1}={\mathrm{a}}^{-1}\cdot \mathrm{a}={\mathrm{a}}_0\end{array}$$
• n*n的可逆矩阵+矩阵乘法=一般线性群GL(n)——是群但不是李群

2.李群

• 李群：具有连续（光滑）性质的群
• 旋转矩阵集合+矩阵乘法=特殊正交群SO(n)
  变换矩阵集合+矩阵乘法=特殊欧式群SE(n)

3.李代数

• 李代数=集合V+数域F+二元运算
• 每个李群对应一个李代数，反应李群局部性质。
• 李代数有种“曲线救国”的感觉：李群类似一个无法定义加法的曲面，李代数类似曲面的切面，在切线附近具有和原曲面相近的性质。我们想对李群求导（用到加法-导数定义中需要加一微小的量），可以通过李代数实现。
• 相信仅凭文字，读者可能对李代数的定位仍不清晰。无妨，直接上图：
  
• 李代数满足运算：“饱经风霜，独自优雅”
  $$\begin{array}{rl}& 1.\text{封闭性}\forall X,Y\in \mathbb{V},[X,Y]\in \mathbb{V}.\\ & \text{2.双线性}\forall X,Y,Z\in \mathbb{V},a,b\in \mathbb{F},\text{有:}[aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y]\\ & \text{3.自反性}\forall X\in \mathbb{V},[X,X]=0\\ & 4.\text{雅可比性}\forall X,Y,Z\in \mathbb{V},[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0.\end{array}$$
  其中，二元运算[ , ]称为李括号，表达了两个元素的差异（eg：自反性——自己和自己的差异为0）

二、两个重要的李代数

1.李群SO(3)对应的李代数so(3)

两个向量的${\varphi }_1,{\varphi }_2$的李括号为：
$$[{\varphi }_1,{\varphi }_2]=({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{)}^{\vee }$$
注：一般情况下，李代数so(3)既可以是3维向量$\varphi$，也可以是3*3反对称矩阵$\Phi$，不加区分。

2.李群SE(3)对应的李代数se(3)

两个向量的${\xi }_1,{\xi }_2$的李括号为：
$$[{\xi }_1,{\xi }_2]=({\xi }_1^{\wedge }{\xi }_2^{\wedge }-{\xi }_2^{\wedge }{\xi }_1^{\wedge }{)}^{\vee }$$
注：此处的${\xi }^{\wedge }$虽然不是反对称矩阵，但为和so(3)统一，仍作此表示。

三、指数与对数映射

• 从 李代数 到 李群 ：指数映射
• 从 李群 到 李代数 ：对数映射

相互映射的推导主要用到指数、正弦，余弦的泰勒展开，具体过程略去，直接给结论。

注：

• 看过上一节视觉SLAM学习打卡【二】，细心的友友们可能会发现：李代数se(3)到李群SO(3)的指数映射=旋转向量到旋转矩阵的转换公式=罗德里格斯公式。
• 李群SO(3)（旋转矩阵组成）对应的李代数so(3)就是旋转向量组成的空间。

四、李代数求导

• 引入李代数的一大动机就是方便求导优化
• 使用李代数解决求导问题的思路分为两种：
  a.用李代数表示姿态，然后根据李代数加法来对李代数求导。
  b.对李群左乘或者右乘微小扰动，然后对扰动求导，称为左扰动和右扰动模型。

1.Baker-Campbell-Hausdorff公式（BCH公式）

• 由于是矩阵的指数，而不是标量的指数，故李群乘积和李代数加法并不等价。$$即{\mathrm{R}}_1{\mathrm{R}}_2=\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge })\ne \exp (({\varphi }_1+{\varphi }_2{)}^{\wedge })$$
• 李群乘积与李代数运算的对应关系由BCH公式给出。$$即\ln (\exp (\mathrm{A})\exp (\mathrm{B}))=\mathrm{A}+\mathrm{B}+\frac{1}{2}\left[\mathrm{A},\mathrm{B}\right]+\frac{1}{12}\left[\mathrm{A},[\mathrm{A},\mathrm{B}]\right]-\frac{1}{12}\left[\mathrm{B},[\mathrm{A},\mathrm{B}]\right]+...$$
  注：上式中 [ , ] 表示李括号运算
• 当${\varphi }_1,{\varphi }_2$为小量时，可以对BCH公式进行线性近似，得到李群乘积对应的李代数的表达式。
  $$即\ln (\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{ll}{\mathrm{J}}_l{\left({\varphi }_2\right)}^{-1}{\varphi }_1+{\varphi }_2& \text{ 当}{\varphi }_1\text{为小量时}\\ {\mathrm{J}}_{\text{r}}{\left({\varphi }_1\right)}^{-1}{\varphi }_2+{\varphi }_1& \text{ 当}{\varphi }_2\text{为小量时}\end{array}$$
  其中，左乘雅可比${\mathrm{J}}_l$为：$${\mathrm{J}}_l=\frac{\sin \theta }{\theta }\mathrm{I}+(1-\frac{\sin \theta }{\theta })\alpha {\alpha }^{\mathrm{T}}+\frac{1-\cos \theta }{\theta }{\alpha }^{\wedge }$$
  ${\mathrm{J}}_l$逆为：
  $${\mathrm{J}}_l^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}\mathrm{I}+(1-\frac{\theta }{2}\cot \frac{\theta }{2})\alpha {\alpha }^{\mathrm{T}}+\frac{\theta }{2}{\alpha }^{\wedge }$$
  右乘雅可比矩阵只需对自变量取负号即可：
  $${\mathrm{J}}_{\mathrm{r}}\left(\varphi \right)={\mathrm{J}}_1\left(-\varphi \right)$$
• BCH公式应用：对旋转R(李代数为$\varphi$)左乘一个微小旋转ΔR (李代数为Δ$\varphi$)，得到的李群ΔR ⋅ R 对应的李代数为：
  $$\ln {\left(\exp (\Delta {\varphi }^{\wedge })\exp ({\varphi }^{\wedge })\right)}^{\vee }=\varphi +{\mathrm{J}}_l^{-1}\left(\varphi \right)\Delta \varphi$$

2.SO(3)对应的李代数求导

• 用李代数 ϕ 表示旋转R，通过李代数加法对李代数求导.
  $$\frac{\partial (\mathrm{Rp})}{\partial \varphi }=-(\mathrm{Rp}{)}^{\wedge }{\mathrm{J}}_l$$
• 对R左乘扰动ΔR（对应李代数为$\phi$）.
  $$\frac{\partial (\mathrm{Rp})}{\partial \phi }=-(\mathrm{Rp}{)}^{\wedge }$$
  注：扰动模型相比于直接对李代数求导，省去了雅可比${\mathrm{J}}_l$的计算。因此，扰动模型在实际中更为实用。

3.SE(3)对应的李代数求导

扰动模型（左乘扰动ΔT（李代数为$\delta \xi$））：
$$\frac{\partial (\mathrm{Rp})}{\partial \delta \xi }=[\begin{array}{cc}\mathrm{I}& -(\mathrm{Rp}+\mathrm{t}{)}^{\wedge }\\ 0^{\mathrm{T}}& 0^{\mathrm{T}}\end{array}]=(\mathrm{TP}{)}^{\odot }$$