slam入门——十四讲笔记（三）

第4讲 李群与李代数

本节目标

1. 理解李群与李代数的概念，掌握$SO(3)$, $SE(3)$与对应李代数的表示方式。
2. 理解BCH近似的意义
3. 学会在李代数上的扰动模型
4. 使用Sophus对李代数进行运算

上一讲重点介绍了旋转的表示，但是在SLAM中，除了表示之外，我们还要对它们进行估计和优化。因为在SLAM中位姿是未知的，而我们需要解决什么样的相机位姿最符合当前观测数据这样的问题。一种典型的方式是把它构建成一个优化问题，求解最优的$R,\mathbf{t}$，使得误差最小化。

如前所言，旋转矩阵自身是带有约束的（正交且行列式为$1$）。它们作为优化变量时，会引入额外的约束，使优化变得困难。通过李群-李代数间的转换关系，我们希望把位姿估计变成无约束的优化问题，简化求解方式。由于我没有李群李代数的基本知识，从最基础的开始看起。

4.1 李群与李代数基础

上一讲，我们介绍了旋转矩阵和变换矩阵的定义。当时，我们说三维旋转矩阵构成了特殊正交群$SO(3)$，而变换矩阵构成了特殊欧氏群$SE(3)$：
S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } (4.1) SO(n)=\{R\in\mathbb{R}^{n\times n}|RR^T=I,det(R)=1\} \quad \text{(4.1)} SO(n)={R∈Rn×n∣RRT=I,det(R)=1}(4.1)
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } . (4.2) SE(3) = \{T=\begin{bmatrix}R&t\\ \mathbf{0}^T & 1 \end{bmatrix} \in \mathbb{R}^{4\times 4} | R \in SO(3), \mathbf{t} \in \mathbb{R}^3 \}. \quad \text{(4.2)} SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}.(4.2)

不过，当时我们并没有详细解释群的含义。我们会发现他们只有一种较好的运算：乘法。$SO(3)$和$SE(3)$关于乘法是封闭的。对于这种只有一个运算的集合。我们把它叫做群。

4.1.1 群

群（Group）是一种集合加上一种运算的代数结构。我们把集合记作$A$，运算记作$\cdot$，那么群可以记作$G=(A,\cdot )$。群要求这个运算满足一下几个条件：

1. 封闭性：$\forall a_1,a_2\in A,a_1\cdot a_2\in A$
2. 结合律：$\forall a_1,a_2,a_3\in A,(a_1\cdot a_2)\cdot a_3=a_1\cdot (a_2\cdot a_3).$
3. 幺元：$\exists a_0\in A,s.t.\forall a\in A,a_0\cdot a=a\cdot a_0=a$
4. 逆：$\forall a\in A,\exists a^{-1}\in A,s.t.a\cdot a^{-1}=a_0$

李群是指具有连续（光滑）性质的群。像整数群$\mathbb{Z}$那样离散的群没有连续性质，所以不是李群。而$SO(n)$和$SE(n)$，它们在实数空间上是连续的。我们能够直观地想象一个刚体能够连续地在空间中运动，所以它们都是李群。由于$SO(3)$和$SE(3)$对于相机姿态估计尤为重要，我们主要讨论这两个李群。

下面，我们先从较简单的$SO(3)$开始讨论，我们将会发现每个李群都有对应的李代数。我们首先引出$SO(3)$上面的李代数$\mathfrak{s}\mathfrak{o}(3)$。

4.1.2 李代数的引出

考虑任意旋转矩阵$R$，我们知道它满足：
$$R{R}^T=I(4.5)$$

现在，我们说，$R$是某个相机的旋转，它会随时间连续地变化，即为时间的函数：$R(t)$。由于它仍是旋转矩阵，有
$$R(t)R(t{)}^T=I$$

在等式两边对时间求导，得到：
$$\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.(4.6)$$

（额……由于不会打那个向上和向下尖的符号，直接把书上的搬过来了。。）

我们看到，旋转矩阵$R$与另一个反对称矩阵${\varphi }_0$通过指数关系发生了联系。也就是说，当我们知道某个时刻的$R$时，存在一个向量$\varphi$，我们满足这个矩阵指数关系。但是矩阵的指数是什么呢？这里有两个问题需要澄清：

1. 如果上式成立，那么给定某时刻的$R$，我们就能求得一个$\varphi$，它描述了$R$在局部的导数关系。与$R$对应的$\varphi$有什么含义呢？后面会看到，$\varphi$正是对应到$SO(3)$上的李代数$\mathfrak{s}\mathfrak{o}(3)$；
2. 其次，矩阵指数$exp(\hat{\varphi })$如何计算？——事实上，这正是李群与李代数间的指数/对数映射。

下面我们一一加以介绍。

4.1.3 李代数的定义

每个李群都有与之对应的李代数。李代数描述了李群的局部性质。通用的李代数定义如下：
李代数由一个集合$\mathbb{V}$，一个数域$\mathbb{F}$和一个二元运算$[,]$组成。如果它们满足以下几条性质，称$(\mathbb{V},\mathbb{F},[,])$为一个李代数，记作$\mathfrak{g}$。

其中二元运算被称为李括号。从表面上来看，李代数所需要的性质还是挺多的。相比于群中的较为简单的二元运算，李括号表达了两个元素的差异。它不要求结合律，而要求元素和自己做李括号之后为零的性质。

作为例子，三维向量${\mathbb{R}}^3$上定义的叉积$\times$是一种李括号，因此$\mathfrak{g}=({\mathbb{R}}^3,\mathbb{R},\times )$构成了一个李代数。我们可以尝试将叉积的性质代数到上面四条性质中。

$\mathbf{a}=(a_x,a_y,a_z)\mathbf{b}=(b_x,b_y,b_z)$
$\mathbf{a}\in {\mathbb{R}}^3,\mathbf{b}\in {\mathbb{R}}^3$
$\mathbf{a}\times \mathbf{b}=\left|\begin{array}{ccc}i& j& k\\ a_x& a_y& a_z\\ b_x& b_y& b_z\end{array}\right|=(a_y{b}_z-a_z{b}_y,a_z{b}_x-a_x{b}_z,a_x{b}_y-a_y{b}_x)$
显然，$\mathbf{a}\times \mathbf{b}\in {\mathbb{R}}^3$ ，满足性质1：封闭性


额… 这些符号打起来 emmm… 其他的 就不验证了，我信。。在纸上自行验证

4.1.4 李代数$\mathfrak{s}\mathfrak{o}(3)$

下面我们说，之前提到的$\varphi$，事实上是一种李代数。 $SO(3)$对应的李代数是定义在${\mathbb{R}}^3$上的向量，我们记作$\varphi$。根据前面的推导，每个$\varphi$都可以生成一个反对称矩阵：

在此定义下，两个向量${\varphi }_1,{\varphi }_2$的李括号为：

可以验证，该定义下的李括号满足上面的几条性质。由于$\varphi$与反对称矩阵关系很紧密，在不引起歧义的情况下，就说$\mathfrak{s}\mathfrak{o}(3)$的元素是3维向量或者3维反对称矩阵，不加区别：

至此，我们清楚了$\mathfrak{s}\mathfrak{o}(3)$的内容。它们是一个三维向量组成的集合，每个向量对应到一个反对称矩阵，可以表达旋转矩阵的导数。它与$SO(3)$的关系由指数映射给定：

指数映射会在稍微介绍。由于已经介绍了$\mathfrak{s}\mathfrak{o}(3)$，我们顺带先来看看$SE(3)$上对应的李代数。

4.1.5 李代数$\mathfrak{s}\mathfrak{e}(3)$

与$\mathfrak{s}\mathfrak{o}(3)$相似，$\mathfrak{s}\mathfrak{e}(3)$位于${\mathbb{R}}^6$空间中：

我们把每个$\mathfrak{s}\mathfrak{e}(3)$元素记作$\xi$，它是一个六维向量。前三维为平移，记作$\rho$；后三维为旋转，记作$\varphi$，实质上是$\mathfrak{s}\mathfrak{o}(3)$元素。同时，我们扩展了${}^{\wedge }$符号的含义。在$\mathfrak{s}\mathfrak{e}(3)$中，同样使用^符号，将一个六维向量转换成四维矩阵，但这里不再表示反对称：

---

立个flag：下一次刷这本书的时候，一定要手动推导一下，把图贴上来。 --2020.12.14

4.2 指数与对数映射

4.2.1 $\mathfrak{s}\mathfrak{o}(3)$上的指数映射

现在来考虑第二个问题：$exp(\varphi$^$)$是如何计算的？它是一个矩阵的指数，在李群和李代数中，称为指数映射。

任意矩阵的指数映射可以写成一个泰勒展开，但是只有在收敛的情况下才会有结果，其结果仍是一个矩阵。
$$exp(A)=\sum _{n=0}^{\infty }\frac{1}{n!}{A}^n\text{(4.18)}$$
同样地，对$\mathfrak{s}\mathfrak{o}(3)$中任一元素$\varphi$，我们亦可按此方式定义它的指数映射：
$$exp(\hat{\varphi })=\sum _{n=0}^{\infty }\frac{1}{n!}{\hat{\varphi }}^n\text{(4.19)}$$

回忆前一讲内容，它和罗德里格斯公式，即式$(3.14)$如出一辙。这表明，$\mathfrak{s}\mathfrak{o}(3)$实际上就是所谓的旋转向量组成的空间，而指数映射即罗德里格斯公式。通过它们，我们把$\mathfrak{s}\mathfrak{o}(3)$中任意一个向量对应到了一个位于$SO(3)$中的旋转矩阵。反之，如果定义对数映射，我们也能把$SO(3)$中的元素对应到$\mathfrak{s}\mathfrak{o}(3)$中：

不过我们通常不按照泰勒展开式去计算对数映射。在第3讲中，我们已经介绍过如何根据旋转矩阵计算对应的李代数，即使用$(3.16)$，利用迹的性质分别求解转角和转轴，采用那种方式更加省事一些。

现在，我们介绍了指数映射的计算方法。读者可能会问，指数映射性质如何呢？是否对于任意的$R$都能找到一个唯一的$\varphi$？很遗憾，指数映射只是一个满射。这意味着每个$SO(3)$中的元素，都可以找到一个$\mathfrak{s}\mathfrak{o}(3)$元素与之对应；但是可能存在多个$\mathfrak{s}\mathfrak{o}(3)$中的元素，对应到同一个$SO(3)$。至少对于旋转角$\theta$，我们知道多转360度和没有转是一样的——它具有周期性。但是，如果我们把旋转角固定在$\pm \pi$之间，那么李群和李代数元素是一一对应的。
$SO(3)$与$\mathfrak{s}\mathfrak{o}(3)$的结论似乎在我们意料之中。它和我们前面讲的旋转向量与旋转矩阵很相似，而指数映射即是罗德里格斯公式。旋转矩阵的导数可以由旋转向量指定，指导着如何在旋转矩阵中进行微积分运算。

4.2.2 $\mathfrak{s}\mathfrak{e}(3)$上的指数映射

下面我们来介绍$\mathfrak{s}\mathfrak{e}(3)$上的指数映射。为了节省篇幅，我们不再像$\mathfrak{s}\mathfrak{o}(3)$那样详细推导指数映射。$\mathfrak{s}\mathfrak{e}(3)$上的指数映射形式如下：

该式与罗德里格斯有些相似，但不完全一样。我们看到，平移部分经过指数映射之后，发生了一次以$J$为系数矩阵的线性变换。请读者重视这里的$J$，因为我们后面还要用到它。

同样的，虽然我们也可以类比推得对数映射，不过根据变换矩阵$T$求$\mathfrak{s}\mathfrak{o}(3)$上的对应向量也有更省事的方式：从左上的$R$计算旋转向量，而右上的$\mathbf{t}$满足：
$$\mathbf{t}=J\rho (4.27)$$
由于$J$可以由$\varphi$得到，所以这里的$\rho$亦可由此线性方程解得。现在，我们已经弄清了李群、李代数的定义与相互的转换关系，总结如下图所示。

4.3 李代数求导与扰动模型

4.3.1 BCH公式与近似公式

使用李代数的一大动机是为了进行优化，而在优化过程中导数是非常必要的信息（会在第六讲详细介绍）。下面我们来考虑一个问题。虽然我们已经清楚了$SO(3)\text{和}SE(3)$上的李群与李代数关系，但是，当我们在$SO(3)$中完成两个矩阵乘法时，李代数中$\mathfrak{s}\mathfrak{o}(3)$上发生了什么改变呢？反过来说，当$\mathfrak{s}\mathfrak{o}(3)$上做两个李代数的加法时，$SO(3)$上是否对应着两个矩阵的乘积？如果成立的话，相当于：

如果${\varphi }_1,{\varphi }_2$是标量，那么显然该式成立；但此处我们计算的是矩阵的指数函数，而非标量的指数。换言之，我们在研究下式是否成立：
$$ln(exp(A)exp(B))=A+B?$$
很遗憾，该式在矩阵时并不成立。

两个李代数指数映射乘积的完整形式，由Baker-Campbell-Hausdorff公式（BCH公式）给出。由于它完整的形式比较复杂，我们给出它展开式的前几项：

以第一个近似为例。该式告诉我们，当对一个旋转矩阵$R_2$左乘一个微小旋转矩阵$R_1$时，可以近似看作，在原有李代数${\varphi }_2$上，加上了一项$J_l({\varphi }_2{)}^{-1}{\varphi }_1$。同理，第二个近似描述了右乘一个微小位移的情况。于是，李代数在BCH近似下，分成了左乘近似和右乘近似两种，在使用时我们须加注意，使用的是左乘模型还是右乘模型。

本书以左乘为例。左乘BCH近似雅可比$J_l$事实上就是式(4.26)的内容：

这样，我们就可以谈论李群乘法与李代数加法的关系了。为了方便读者理解，我们重新叙述一下BCH近似的意义。

4.3.2 $SO(3)$上的李代数求导

下面我们来讨论一个带有李代数的函数，如何关于该李代数求导的问题。该问题有很强的实际背景。在SLAM中，我们要估计一个相机的位置和姿态，该位姿是由$SO(3)$上的旋转矩阵或$SE(3)$上的变换矩阵描述的。不妨设某个时刻小萝卜的位姿为$T$。它观察到了一个世界坐标系位于$p$的点，产生了一个观测数据$z$。那么，由坐标变换关系知：
$$z=Tp+w(4.37)$$
然而，由于观测噪声$w$的存在，$z$往往不可能精确地满足$z=Tp$的关系。所以，我们通常会计算理想的观测与实际数据的误差：
$$e=z-Tp(4.38)$$
假设一共有N个这样的路标点和观测，于是就有N个上式。那么，对小萝卜的位姿估计，相当于是寻找一个最优的$T$，使得整体误差最小化：
$$\underset{T}{\min }J(T)=\sum _{i=1}^N||z_i-T{p}_i|{|}_2^2.(4.39)$$
求解此问题，需要计算目标函数$J$关于变换矩阵$T$的导数。我们把具体的算法留到后面再讲。这里重点要说的是，我们经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。然而，$SO(3),SE(3)$上并没有良好定义的加法，他们只是群。如果我们把$T$当成一个普通矩阵来处理优化，那就必须对它加以约束。而从李代数角度来说，由于李代数由向量组成，具有良好的加法运算。因此，使用李代数解决求导问题的思路分为两种：

1. 用李代数表示姿态，然后根据李代数加法来对李代数求导。
2. 对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型。

第一种方式对应到李代数的求导模型，而第二种则对应到扰动模型。下面我们来讨论这两种思路的异同。

4.3.3 李代数求导

第二行的近似为BCH线性近似，第三行为泰勒展开舍去高阶项后近似，第四行至第五行将反对称符号看作叉积，交换之后变号。于是，我们推导了旋转后的点相对于李代数的导数：

不过，由于这里仍然含有形式比较复杂的$J_l$，我们不太希望计算它。而下面要讲的扰动模型则提供了更简单的导数计算方式。

4.3.4 扰动模型（左乘）

另一种求导方式，是对$R$进行一次扰动$\Delta R$。这个扰动可以乘在左边也可以乘在右边，最后结果会有一点儿微小的差异，我们以左扰动为例。设左扰动$\Delta R$对应的李代数为$\phi$。然后，对$\phi$求导，即：

该式的求导比上面更为简单：

可见，扰动模型相比于直接对李代数求导，省去了一个雅可比$J_l$的计算。这使得扰动模型更为实用。请读者务必理解这里的求导运算，这在位姿估计当中具有重要的意义。

4.3.5 $SE(3)$上的李代数求导

最后，我们给出$SE(3)$上的扰动模型，而直接李代数上的求导就不再介绍了。假设某空间点$p$经过一次变换$T$（对应李代数为$\xi$），得到$Tp$。现在，给$T$左乘一个扰动$\Delta T=exp(\delta \hat{\xi })$，我们设扰动项的李代数为$\delta \xi =[\delta \rho ,\delta \varphi {]}^T$，那么：

我们把最后的结果定义成一个算符$\odot$，它把一个齐次坐标的空间点变换成一个$4\times 6$的矩阵。

至此，我们已经介绍了李群李代数上的微分运算。之后的章节中，我们将应用这些知识去解决实际问题。关于李群李代数的某些重要数学性质，我们作为习题，有缘再学。

4.4 实践：Sophus

我们已经介绍了李代数的入门知识，现在是通过实践演练巩固一下所学知识的机会了。我们来讨论如何在程序中操作李代数。在第三讲中，我们看到Eigen提供了几何模块，但没有提供李代数的支持。一个较好的李代数库是Strasdat维护的Sophus库。Sophus库支持本章主要讨论的$SO(3)和SE(3)$，此外还含有二维运动$SO(2),SE(2)$以及相似变换$Sim(3)$的内容。它是直接在Eigen基础上开发的，我们不需要安装额外的依赖库。读者可以直接从github上获取Sophus，或者，在本书（《视觉slam十四讲》）的代码目录slambook/3rdparty下也提供了Sophus源代码。由于历史原因，Sophus早期版本只提供了双精度的李群/李代数类。后续版本改写成了模板类。模板类的Sophus中可以使用不同精度的李群/李代数，但同时增加了使用难度。本书使用非模板的Sophus库。如果读者准备使用github上的Sophus，请确保使用的是非模板的版本。你可以输入一下命令获得非模板类的Sophus：

git clone https://github.com/strasdat/Sophus.git
cd Sophus
git checkout a621ff

Sophus库只需编译即可，无须安装。
下面我们来演示一下Sophus库中的$SO(3)和SE(3)$运算：

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <sophus/so3.h>
#include <sophus/se3.h>

int main() {

    // 沿Z轴转90度的旋转矩阵
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();

    Sophus::SO3 SO3_R(R); // Sophus::SO(3)可以直接从旋转矩阵构造
    Sophus::SO3 SO3_v(0, 0, M_PI/2); // 亦可从旋转向量构造
    Eigen::Quaterniond q(R);  // 或者四元数
    Sophus::SO3 SO3_q(q);

    // 上述表达方式都是等价的
    // 输出SO(3)时，以so（3）形式输出
    cout << "SO(3) from matrix: " << SO3_R << endl;
    cout << "SO(3) from vector: " << SO3_v << endl;
    cout << "SO(3) from quaternion: " << SO3_q << endl;

    // 使用对数映射获得它的李代数
    Eigen::Vector3d so3 = SO3_R.log();
    cout << "so3 = " << so3.transpose() << endl;
    // hat 为向量到反对称矩阵
    cout<<"so3 hat=\n"<<Sophus::SO3::hat(so3)<<endl;
    // 相对的，vee为反对称到向量
    cout << "so3 hat vee = " << Sophus::SO3::vee(Sophus::SO3::hat(so3)).transpose() << endl; // transpose纯粹是为了输出美观一些

    // 增量扰动模型的更新
    Eigen::Vector3d update_so3(1e-4, 0, 0); // 假设更新量为这么多
    Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3)*SO3_R;
    cout << "SO3 updated = " << SO3_updated << endl;

    /********************萌萌的分割线*****************************/
    cout<<"************我是分割线*************"<<endl;
    // 对SE(3)操作大同小异
    Eigen::Vector3d t(1,0,0);           // 沿X轴平移1
    Sophus::SE3 SE3_Rt(R, t);           // 从R,t构造SE(3)
    Sophus::SE3 SE3_qt(q,t);            // 从q,t构造SE(3)
    cout<<"SE3 from R,t= "<<endl<<SE3_Rt<<endl;
    cout<<"SE3 from q,t= "<<endl<<SE3_qt<<endl;
    // 李代数se(3) 是一个六维向量，方便起见先typedef一下
    typedef Eigen::Matrix<double,6,1> Vector6d;
    Vector6d se3 = SE3_Rt.log();
    cout<<"se3 = "<<se3.transpose()<<endl;
    // 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.
    // 同样的，有hat和vee两个算符
    cout<<"se3 hat = "<<endl<<Sophus::SE3::hat(se3)<<endl;
    cout<<"se3 hat vee = "<<Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose()<<endl;

    // 最后，演示一下更新
    Vector6d update_se3; //更新量
    update_se3.setZero();
    update_se3(0,0) = 1e-4d;
    Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3)*SE3_Rt;
    cout<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<endl;

    return 0;
}

cmake_minimum_required(VERSION 3.17)
project(useSophus)

set(CMAKE_CXX_STANDARD 14)

find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

add_executable(useSophus useSophus.cpp)
target_link_libraries(useSophus ${Sophus_LIBRARIES})

4.5 *相似变换群与李代数

最后，我们要提一下在单目视觉中使用的相似变换群$Sim(3)$，以及对应的李代数$\mathfrak{s}\mathfrak{i}\mathfrak{m}(3)$。如果你只对双目SLAM或RGBD SLAM感兴趣，可以跳过本节。
我们已经介绍过单目的尺度不确定性。如果在单目 SLAM 中使用 $SE(3)$ 表示位姿，那么由于尺度不确定性与尺度漂移，整个 SLAM 过程中的尺度会发生变化，这在 $SE(3)$中 未能体现出来。因此，在单目情况下我们一般会显式地把尺度因子表达出来。用数学语言 来说，对于位于空间的点 $p$，在相机坐标系下要经过一个相似变换，而非欧氏变换：
$$p\prime =\left[\begin{array}{cc}sR& t\\ 0^T& 1\end{array}\right]p=sRp+t.(4.42)$$
在相似变换中，我们把尺度$s$表达了出来。它同时作用在$p$的三个坐标之上，对$p$进行了一次缩放。与$SO(3)、SE(3)$相似，相似变换亦对矩阵乘法构成群，称为相似变换群$Sim(3)$：

4.6 小结

本讲引入了李群SO(3)和SE(3)，以及它们对应的李代数$\mathfrak{s}\mathfrak{o}(3)和\mathfrak{s}\mathfrak{e}(3)$。我们介绍了位姿在它们上面的表达和转换，然后通过BCH的线性近似，我们可以对位姿进行求导和扰动了。这给之后讲解位姿的优化打下了理论基础，因为我们需要经常的对某一个位姿的估计值进行调整，使它对应的误差减小。只有在弄清楚如何对位姿进行调整和更新之后，我们才能继续下一步的内容。

之所以这一章写的比较粗糙，是因为 一方面，时间有限；另一方面，我认为学习slam需要反复多次，整体把握，逐步深入。而不适合在一个点上琢磨太久。所以后面的东西更精彩，继续往后吧，改天再回来！