【三】李群和李代数

群和李群

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

1. 封闭性：$\forall a,b\in A,a\cdot b\in A;$
2. 结合律：$\forall a,b,c\in A,(a\cdot b)\cdot c=a\cdot (b\cdot c);$
3. 幺元： $\exists e\in A$, s.t. $\forall a\in A,e\cdot a=a\cdot e=a$
4. 逆： $\forall a\in A,\exists a^{-1}\in A$, s.t. $a\cdot a^{-1}=e_i$

李群是指具有连续（光滑）性质的群。像整数Z那样离散的群没有连续性质，所以不是李群。

在之前介绍旋转矩阵和变换矩阵的定义时，说三维旋转矩阵构成了特殊正交群SO(3),同时变换矩阵构成了特殊欧式群SE(3),具体为：
$$\begin{array}{rl}& \mathrm{S}\mathrm{O}(3)=\left\{\mathbf{R}\in {\mathbb{R}}^{3\times 3}\mid \mathbf{R}{\mathbf{R}}^{\mathrm{T}}=\mathbf{I},\det (\mathbf{R})=1\right\}\\ & \mathrm{S}\mathrm{E}(3)=\left\{T=\left[\begin{array}{ll}\mathbf{R}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\mid \mathbf{R}\in \mathrm{S}\mathrm{O}(3),\mathbf{t}\in {\mathbb{R}}^3\right\}\end{array}$$
在这里SO(n)和SE(n)在实数空间上是连续的。我们能够直观地想象一个刚体能够连续地在空间中运动，所以他们都是李群。
关于这两个李群，我们知道他们对加法是不封闭的也就是说：
$${\mathbf{R}}_1+{\mathbf{R}}_2\notin \mathrm{S}\mathrm{O}(3),{\mathbf{T}}_1+{\mathbf{T}}_2\notin \mathrm{S}\mathrm{E}(3)$$

但是，他们对乘法是封闭的，即：
$${\mathbf{R}}_1{\mathbf{R}}_2\in \mathrm{S}\mathrm{O}(3),{\mathbf{T}}_1{\mathbf{T}}_2\in \mathrm{S}\mathrm{E}(3)$$

所以对这两个李群来说，他们的运算即为乘法。

李代数so(3)

我们知道$\dot{R}(t)R(t{)}^{\mathrm{T}}$是一个反对称矩阵，我们可以找到一个三维向量$\varphi (t)\in {\mathbb{R}}^3$与之对应：
$$\dot{R}(t)R(t{)}^{\mathrm{T}}=\varphi (t{)}^{\wedge }$$
又因为$R(t{)}^{\mathrm{T}}R(t)=I$，正交阵。因此得到：
$$\dot{R}(t)=\varphi (t{)}^{\wedge }R(t)=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]R(t)$$
这样对$R(t)$求导时，只用直接左乘一个$\varphi (t{)}^{\wedge }$即可，当$R(t)=I$时，那么它的导数就是$\varphi (t{)}^{\wedge }$，一个反对称矩阵。
只有反对称矩阵组成的空间，即 so(3)，我们称之为在单位矩阵处的正切空间tangent space。

$\varphi (t)$正是对应到SO(3)上的李代数so(3)。 给定某时刻的R，我们就能求得一个$\varphi$，它描述了 R在局部的导数关系。所以说它在SO(3)的正切空间上。
每个李群都有与之对应的李代数。李代数描述了李群的局部性质，准确地说，是单位元附近的正切空间。
为什么这么称呼它？二维曲线在某处的导数是一条切线，三维曲线在某处的导数是一个切面，所以正切空间，实际上是导数所组成的空间。

具体的李代数的四个性质1.封闭性 2.双线性 3.自反性 4.雅可比等价 。这个四个性质在这里就不过多的介绍了。

由于向量$\varphi (t)$与反对称矩阵是一一对应的，在不引起歧义的情况下，就说so(3)的元素是三维向量或者三维反对称矩阵，不加区别：

$$so(3)=\left\{\varphi \in {\mathbb{R}}^3,\Phi ={\varphi }^{\wedge }\in {\mathbb{R}}^{3\times 3}\right\}$$

至此，已经清楚了so(3)的内容。他们是一个由三维向量组成的集合，每个向量对应一个反对称矩阵，可以用于表达旋转矩阵的导数。它与SO(3)的关系又指数映射给定：

$$\mathbf{R}=\exp \left({\varphi }^{\wedge }\right)$$

任意矩阵的指数映射可以写成一个泰勒展开，但是只有在收敛的情况下才会有结果，其结果仍是一个矩阵（学过工程矩阵的应该都知道）：
$$\exp \left({\varphi }^{\wedge }\right)=\sum _{n=0}^{\infty }\frac{1}{n!}{\left({\varphi }^{\wedge }\right)}^n$$

在这里，由于$\varphi$是三维向量，我们可以定义它的模长和方向，分别记作$\theta$和$a$。于是$\varphi =\theta a$这里$a$是一个长度为1的方向向量。这样经过化简就会得到罗德里格斯公式（旋转轴的向量与旋转矩阵的关系）：
$$\exp \left(\theta {\mathbf{a}}^{\wedge }\right)=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{a}{\mathbf{a}}^{\mathrm{T}}+\sin \theta {\mathbf{a}}^{\wedge }$$

因此就表面了so(3)就是由所谓的旋转向量组成的空间，即指数映射就是罗德里格斯公式。反之，如果定义对数映射，也能把SO(3)中的元素对应到so(3)中：

$$\varphi =\ln (\mathbf{R}{)}^{\vee }={\left(\sum _{n=0}^{\infty }\frac{(-1{)}^n}{n+1}(\mathbf{R}-\mathbf{I}{)}^{n+1}\right)}^{\vee }$$

计算旋转向量不需要使用对数映射，因为对数映射过于麻烦。
对于转角$\theta$，有：

$$\begin{array}{rl}\mathrm{tr}(R)& =\cos \theta \mathrm{tr}(I)+(1-\cos \theta )\mathrm{tr}\left(n{n}^{\mathrm{T}}\right)+\sin \theta \mathrm{tr}\left(n^{\wedge }\right)\\ & =3\cos \theta +(1-\cos \theta )\\ & =1+2\cos \theta \end{array}$$
因此：

$$\theta =\arccos \frac{\mathrm{tr}(\mathbf{R})-1}{2}$$

关于转轴$n$，旋转轴上的向量在旋转后不发生改变，说明：

$$Rn=n$$
意思是让旋转轴按照旋转矩阵的方式进行旋转，得到的还是旋转轴，（旋转轴经过旋转之后不变）
因此，转轴$n$是矩阵$R$特征值1对应的特征向量。求解此方程，在归一化，就得到了旋转轴。

指数映射只是一个满射，并不是单射。这意味这每个SO(3)中的元素，都可以找到一个so(3)元素与之对应；但是可能存在多个so(3)中的元素，对应到同一个SO(3).至少对于旋转角$\theta$，我们知道多转360°和没有转是一样的———它具有周期性。但是，如果我们把旋转角度固定在$\pm \pi$之间，那么李群和李代数元素是一一对应的。
旋转矩阵的导数可以由旋转向量指定，指导着如何在旋转矩阵中进行微积分运算。

李代数se(3)

对于SE(3)，它的对应的李代数se(3)为：
$$\mathrm{se}(3)=\left\{\xi =\left[\begin{array}{l}\rho \\ \varphi \end{array}\right]\in {\mathbb{R}}^6,\rho \in {\mathbb{R}}^3,\varphi \in \sigma 0(3),{\xi }^{\wedge }=\left[\begin{array}{ll}{\varphi }^{\wedge }& \rho \\ 0^{\top }& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}$$
se(3)为于${\mathbb{R}}^6$空间中，它是一个六维向量，前三维为平移（但含义与变换矩阵中的平移不同，因为在进行指数映射之后，需要进行一次以$J$为系数矩阵的线性变换。即$t=J\rho$），记作$\rho$；后三维为旋转，记作$\varphi$，实质上是so(3)元素。

在这里的se(3)中的这个符号${}^{\wedge }$并不表示反对称了，而是为了与so(3)中的这个符号相对应，也使用了这个符号，在这里这个符号是将一个六维向量转换成四维矩阵.（这里。我们应该仔细地想一下，为什么这个符号表示的不是反对称了，因为这个是对旋转来说的，对平移并不是这样，变换分为旋转和平移，这是两种性质不同的变换。而为什么是变换成四维矩阵，因为在之前的变换矩阵那里，为了整合平移和旋转，设计了一种这样四维矩阵的格式，也就是变换矩阵$T$。也就只有这种格式才能每进行一次变换（平移＋旋转）直接左乘即可。虽然这个四维矩阵和那个变换矩阵$T$不一样，因为这正是李代数与李群，之间有一个指数映射关系）

se(3)的指数映射为：

$$\begin{array}{rl}\exp \left({\xi }^{\wedge }\right)& =\left[\sum _{n=0}^{\infty }\frac{1}{n!}{\left({\varphi }^{\wedge }\right)}^n\sum _{n=0}^{\infty }\frac{1}{(n+1)!}{\left({\varphi }^{\wedge }\right)}^n\rho \right]\\ & \triangleq \left[\begin{array}{cc}R& J\rho \\ 0^{\mathrm{T}}& 1\end{array}\right]=T.\end{array}$$

从上式可以看出：$\xi$的指数映射左上角的$R$是我们熟知的SO(3)中的元素，与se(3)中的旋转部分$\theta$对应。而右上角的$J$推导得出：

$$J=\frac{\sin \theta }{\theta }I+\left(1-\frac{\sin \theta }{\theta }\right)a{a}^{\mathrm{T}}+\frac{1-\cos \theta }{\theta }{a}^{\wedge }$$

该式与罗德里格斯公式有些相似，但不完全一样。
同样也可以使用对数映射求se(3)中的元素，但是没必要且复杂，可以从左上角的$R$计算旋转向量，而右上角的$t$满足：$t=J\rho$。由于$J$可以由$\varphi$得到，所以$\rho$也可以由此线性方程解得。

最终的李代数的定义与相互转换关系为：

在这里放几张图供大家好好理解理解李群和李代数之间的关系和李代数要发挥的作用。

李代数求导和扰动模型

在这里，就显示出李代数的作用了。在SLAM 中，我们要估计一个相机的位置和姿态，该位姿是由SO(3)上的旋转矩阵或SE(3)上的变换矩阵描述的。设相机的位姿为$T$。它观察到了一个世界坐标位于$p$的点，产生了一个数据z。那么，由坐标变换关系知：
$$z=Tp+w$$
其中$w$为随机噪声。由于它的存在，$z$往往不可能精确地满足$z=Tp$的关系。所以，我们通常会计算理想的观测与实际数据的误差：
$$e=z-Tp$$
有$N$个这样的路标点就有$N$个这样的式子：

$$\underset{T}{\min }J(T)=\sum _{i=1}^N{\Vert z_i-T{p}_i\Vert }_2^2$$

要得到最优的$T$才能使误差最小，这就涉及对位姿的求导问题。
我们经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值
因为使用SO(3)或SE(3)求导会比较麻烦，并且没有良好定义加法，因此只能使用李代数求导（这就是李代数的意义所在），李代数求导又分为两种方式：
1.用李代数表示姿态，然后根据李代数加法对李代数求导。
2.对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型（重要）

第一种求导方式为

$$\frac{\partial (Rp)}{\partial R}=\frac{\partial \left(\exp \left({\varphi }^{\wedge }\right)\mathbf{p}\right)}{\partial \varphi }$$

$$\frac{\partial \left(\exp \left({\varphi }^{\wedge }\right)p\right)}{\partial \varphi }=\underset{\delta \varphi \to 0}{\lim }\frac{\exp \left((\varphi +\delta \varphi {)}^{\wedge }\right)p-\exp \left({\varphi }^{\wedge }\right)p}{\delta \varphi }$$

然后使用BCH近似：
SO(3)
$$\exp \left(\Delta {\varphi }^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)=\exp \left({\left(\varphi +J_l^{-1}(\varphi )\Delta \varphi \right)}^{\wedge }\right)$$
$$\exp \left((\varphi +\Delta \varphi {)}^{\wedge }\right)=\exp \left({\left(J_1\Delta \varphi \right)}^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)=\exp \left({\varphi }^{\wedge }\right)\exp ({\left(J_r\Delta \varphi \right)}^{\wedge }$$
SE(3)

$$\begin{array}{rl}& \mathrm{cxp}\left(\Delta {\xi }^{\wedge }\right)\exp \left({\xi }^{\wedge }\right)\approx \exp \left({\left({\mathcal{J}}_i^{-1}\Delta \xi +\xi \right)}^{\wedge }\right)\\ & \exp \left({\xi }^{\wedge }\right)\exp \left(\Delta {\xi }^{\wedge }\right)\approx \exp \left({\left({\mathcal{J}}_r^{-1}\Delta \xi +\xi \right)}^{\wedge }\right)\end{array}$$

左乘BCH近似雅可比$J_l$为：
$$J_l=J=\frac{\sin \theta }{\theta }I+\left(1-\frac{\sin \theta }{\theta }\right)a{a}^{\mathrm{T}}+\frac{1-\cos \theta }{\theta }{a}^{\wedge }$$
逆为：
$$J_l^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}I+\left(1-\frac{\theta }{2}\cot \frac{\theta }{2}\right)a{a}^{\mathrm{T}}-\frac{\theta }{2}{a}^{\wedge }$$
右乘雅可比仅需要对自变量取负号：
$$J_r(\varphi )=J_l(-\varphi )$$

并使用泰勒展开舍去高阶项后得到：
$$\frac{\partial (\mathbf{R}p)}{\partial \varphi }=(-\mathbf{R}p{)}^{\wedge }{J}_l$$
因为有需要计算复杂的雅可比$J_l$因此，这种方式比较复杂，使用较少。

第二种方式：（扰动模型(左乘)）
另一种求导方式使对$\mathbf{R}$进行一次扰动$\Delta \mathbf{R}$，看结果相对于扰动的变化率。（这一部分可以这样理解：对自变量进行小量扰动，然后求出对小量的偏导，令其为0，看看小量在什么时候能够求得极值，求出的这个小量再加回到自变量，这时自变量加上小量后，会使整个带有自变量的误差函数的值达到最小）

具体的公式为：
$$\begin{array}{rl}\frac{\partial (Rp)}{\partial \phi }& =\underset{\phi \to 0}{\lim }\frac{\exp \left({\phi }^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)\mathbf{p}-\exp \left({\varphi }^{\wedge }\right)\mathbf{p}}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{\left(\mathbf{I}+{\phi }^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)\mathbf{p}-\exp \left({\varphi }^{\wedge }\right)\mathbf{p}}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }\mathbf{R}p}{\phi }=\underset{\phi \to 0}{\lim }\frac{-(\mathbf{R}p{)}^{\wedge }\phi }{\phi }=-(\mathbf{R}p{)}^{\wedge }\end{array}$$

可见，相比于第一种方式，直接对李代数求导，省去了一个雅可比$J_l$的计算。这样的方式更简单，这个扰动模型更为实用。

对SE(3)上的李代数求导，同样使用扰动模型：
$$\frac{\partial (\mathbf{T}\mathbf{p})}{\partial \delta \mathbf{\xi }}=\left[\begin{array}{cc}I& -(Rp+t{)}^{\wedge }\\ 0^{\mathrm{T}}& 0^{\mathrm{T}}\end{array}\right]\stackrel{\text{ def }}{=}(Tp{)}^{\odot }$$

这里把最后的结果定义成了一个算符${}^{\odot }$,它把一个齐次坐标的空间点变换成一个4X6的矩阵。

Sophus的使用

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/se3.hpp"

using namespace std;
using namespace Eigen;

/// 本程序演示sophus的基本用法

int main(int argc, char **argv) {

  // 沿Z轴转90度的旋转矩阵
  Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
  // 或者四元数
  Quaterniond q(R);
  Sophus::SO3d SO3_R(R);              // Sophus::SO3d可以直接从旋转矩阵构造
  Sophus::SO3d SO3_q(q);              // 也可以通过四元数构造
  // 二者是等价的
  cout << "SO(3) from matrix:\n" << SO3_R.matrix() << endl;
  cout << "SO(3) from quaternion:\n" << SO3_q.matrix() << endl;
  cout << "they are equal" << endl;

  // 使用对数映射获得它的李代数
  Vector3d so3 = SO3_R.log();
  cout << "so3 = " << so3.transpose() << endl;
  // hat 为向量到反对称矩阵
  cout << "so3 hat=\n" << Sophus::SO3d::hat(so3) << endl;
  // 相对的，vee为反对称到向量
  cout << "so3 hat vee= " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;

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

  cout << "*******************************" << endl;
  // 对SE(3)操作大同小异
  Vector3d t(1, 0, 0);           // 沿X轴平移1
  Sophus::SE3d SE3_Rt(R, t);           // 从R,t构造SE(3)
  Sophus::SE3d SE3_qt(q, t);            // 从q,t构造SE(3)
  cout << "SE3 from R,t= \n" << SE3_Rt.matrix() << endl;
  cout << "SE3 from q,t= \n" << SE3_qt.matrix() << 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 = \n" << Sophus::SE3d::hat(se3) << endl;
  cout << "se3 hat vee = " << Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose() << endl;

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

  return 0;
}

代码在这里。

参考

1.从零开始一起学习SLAM | 为啥需要李群与李代数？
2.SLAM笔记（九）再谈李代数
3.SLAM14讲学习笔记（一) 李群李代数基础
4.【视觉SLAM十四讲】李群与李代数
5.旋转矩阵、变换矩阵，李群（Lie Group）、李代数（Lie Algebra）及扰动模型
6.视觉SLAM十四讲