视觉SLAM14讲笔记01：ch2-4李群与李代数

三维空间刚体运动

SLAM数学表达

$${\mathbf{x}}_k=f\left({\mathbf{x}}_{k-1},{\mathbf{u}}_k,{\mathbf{w}}_k\right)$$
这里${\mathbf{u}}_k$是运动传感器的读数（有时也叫输入），${\mathbf{w}}_k$为噪声。我们把它称为
运动方程。

与运动方程相对应，还有一个观测方程。当小萝卜在${\mathbf{x}}_k$位置上
看到某个路标点${\mathbf{y}}_j$，产生了一个观测数据${\mathbf{z}}_{k,j}$。
$${\mathbf{z}}_{k,j}=h\left({\mathbf{y}}_j,{\mathbf{x}}_k,{\mathbf{v}}_{k,j}\right)$$
这里${\mathbf{v}}_{k,j}$是这次观测里的噪声。

旋转矩阵

点和向量，坐标系

三维空间中的某个向量的坐标可以用${\mathbb{R}}^3$当中的三个数来描述。如果我们确定一个坐标系，那就可以谈论向量a 在这组基下的坐标了：
$$\mathbf{a}=\left[{\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3\right]\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]=a_1{e}_1+a_2{e}_2+a_3{e}_3$$

内积可以描述向量间的投影关系。而外积呢是这个样子:
$$\mathbf{a}\times \mathbf{b}=\left[\begin{array}{ccc}\mathbf{i}& \mathbf{j}& \mathbf{k}\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\right]=\left[\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ a_3{b}_1-a_1{b}_3\\ a_1{b}_2-a_2{b}_1\end{array}\right]=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]\mathbf{b}\triangleq {\mathbf{a}}^{\wedge }\mathbf{b}$$
我们引入了人符^号，把$\mathbf{a}$写成一个矩阵。事实上是一个反对称矩阵 (Skew-symmetric),你可以将^记成一个反对称符号。这样就把外积$a\times b$, 写成了矩阵与向量的乘法$a^{\wedge }b$,把它变成了线性运算。

坐标系间的欧氏变换

相机运动是一个刚体运动，它保证了同一个向量在各个坐标系下的长度和夹角都不会 发生变化。这种变换称为欧氏变换。

首先来考虑旋转。我们设某个单位
正交基$\left(e_1,e_2,e_3\right)$经过一次旋转，变成了$\left(e_1^{\prime },e_2^{\prime },e_3^{\prime }\right)$。那么，对于同一个向量a （注意该 向量并没有随着坐标系的旋转而发生运动），它在两个坐标系下的坐标为${\left[a_1,a_2,a_3\right]}^T$和
${\left[a_1^{\prime },a_2^{\prime },a_3^{\prime }\right]}^T$。根据坐标的定义，有：
$$\left[e_1,e_2,e_3\right]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[e_1^{\prime },e_2^{\prime },e_3^{\prime }\right]\left[\begin{array}{c}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]$$
所以：
$$\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]=\left[\begin{array}{ccc}{\mathbf{e}}_1^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_1^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_1^T{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_2^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_2^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_2^T{\mathbf{e}}_3^{\prime }\\ {\mathbf{e}}_3^T{\mathbf{e}}_1^{\prime }& {\mathbf{e}}_3^T{\mathbf{e}}_2^{\prime }& {\mathbf{e}}_3^T{\mathbf{e}}_3^{\prime }\end{array}\right]\left[\begin{array}{l}{a}_1^{\prime }\\ a_2^{\prime }\\ a_3^{\prime }\end{array}\right]\triangleq \mathbf{R}{\mathbf{a}}^{\prime }$$
可以说，矩阵$R$ 描述了旋转本身。因此它又称为旋转矩阵。

变换矩阵与齐次坐标

$$\left[\begin{array}{l}{\mathbf{a}}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{ll}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\left[\begin{array}{l}\mathbf{a}\\ 1\end{array}\right]\triangleq \mathbf{T}\left[\begin{array}{l}\mathbf{a}\\ 1\end{array}\right]$$
我们把一个三维向量的末尾添加1，变成了四维向量，称为齐次
坐标。对于这个四维向量，我们可以把旋转和平移写在一个矩阵里面，使得整个关系变成
了线性关系。该式中，矩阵©称为变换矩阵(Transform Matrix)。我们暂时用N 表示a 的齐次坐标。

旋转向量和欧拉角

任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是，我们可以使
用一个向量，其方向与旋转轴一致，而长度等于旋转角。这种向量，称为旋转向量(或轴角，Axis-Angle)。
而欧拉角则提供了一种非常直观的 方式来描述旋转—— 它使用了三个分离的转角，把一个旋转分解成三次绕不同轴的旋转。

1. 绕物体的Z 轴旋转，得到偏航角yaw；
2. 绕旋转之后的Y 轴旋转，得到俯仰角pitch；
3. 绕旋转之后的X 轴旋转，得到滚转角roll。

四元数

旋转矩阵用九个量描述三自由度的旋转，具有冗余性；欧拉角和旋转向量是紧凑的, 但具有奇异性。事实上，我们找不到不带奇异性的三维向量描述方式。一个四元数q 拥有一个实部和三个虚部。
$$\mathbf{q}=q_0+q_{1}i+q_{2}j+q_{3}k$$
其中i,j,k为四元数的三个虚部。这三个虚部满足关系式:
$$\{\begin{array}{l}{i}^2=j^2=k^2=-1\\ ij=k,ji=-k\\ jk=i,kj=-i\\ ki=j,ik=-j\end{array}$$
我们知道单位四元数能够表达三维空间的旋转。这种表达方式和旋转矩
阵、旋转向量有什么关系呢？
$$\mathbf{q}={\left[\cos \frac{\theta }{2},n_x\sin \frac{\theta }{2},n_y\sin \frac{\theta }{2},n_z\sin \frac{\theta }{2}\right]}^T$$
反之，我们亦可从单位四元数中计算出对应旋转轴与夹角：
$$\{\begin{array}{l}\theta =2\arccos q_0\\ {\left[n_x,n_y,n_z\right]}^T={\left[q_1,q_2,q_3\right]}^T/\sin \frac{\theta }{2}\end{array}$$

李群与李代数

李群与李代数的定义

上一讲，我们介绍了三维世界中刚体运动的描述方式，包括旋转矩阵、旋转向量、欧 拉角、四元数等若干种方式。我们重点介绍了旋转的表示，但是在SLAM中，除了表示之外，我们还要对它们进行估计和优化。因为在SLAM中位姿是未知的，而我们需要解决什么样的相机位姿最符合当前观测数据这样的问题。一种典型的方式是把它构建成一个优化问题。

群 (Group)是一种集合加上一种运算的代数结构。我们把集合记作$A$ , 运算记作・, 那么群可以记作$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,\left(a_1\cdot a_2\right)\cdot a_3=a_1\cdot \left(a_2\cdot a_3\right)$

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{V}$ , —个数域$\mathbb{F}$ 和一个二元运算[,]组成。如果它们满足以下几条
性质，称$(\mathbb{V},\mathbb{F},[,])$为一个李代数，记作$\mathfrak{g}$。

1. 封闭性 $\forall \mathbf{X},\mathbf{Y}\in \mathbb{V},[\mathbf{X},\mathbf{Y}]\in \mathbb{V}$
2. 双线性 $\forall \mathbf{X},\mathbf{Y},\mathbf{Z}\in \mathbb{V},a,b\in \mathbb{F},$ 有 :
   $$[a\mathbf{X}+b\mathbf{Y},\mathbf{Z}]=a[\mathbf{X},\mathbf{Z}]+b[\mathbf{Y},\mathbf{Z}],[\mathbf{Z},a\mathbf{X}+b\mathbf{Y}]=a[\mathbf{Z},\mathbf{X}]+b[\mathbf{Z},\mathbf{Y}]$$
3. 自反性 $\forall \mathbf{X}\in \mathbb{V},[\mathbf{X},\mathbf{X}]=0$
4. 雅可比等价 $\forall \mathbf{X},\mathbf{Y},\mathbf{Z}\in \mathbb{V},[\mathbf{X},[\mathbf{Y},\mathbf{Z}]]+[\mathbf{Z},[\mathbf{Y},\mathbf{X}]]+[\mathbf{Y},[\mathbf{Z},\mathbf{X}]]=0$

其中二元运算被称为李括号。从表面上来看，李代数所需要的性质还是挺多的。相比于群中的较为简单的二元运算，李括号表达了两个元素的差异。它不要求结合律，而要求元素和自己做李括号之后为零的性质。
作为例子，三维向量上定义的叉积是一种李括号，$\mathfrak{g}=\left({\mathbb{R}}^3,\mathbb{R},\times \right)$构成了一个李代数。

指数与对数映射

SO(3) 上 的 指 数 映 射

根据前面的推导，每个 $\varphi$ 都可以生成一个反对称矩阵：
$$\mathbf{\Phi }={\mathbf{\varphi }}^{\wedge }=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]\in {\mathbb{R}}^{3\times 3}$$
在此定义下，两个向量 ${\varphi }_1,{\varphi }_2$ 的李括号为:
$$\left[{\varphi }_1,{\varphi }_2\right]={\left({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1\right)}^{\vee }$$
李群为：
$$\mathfrak{s}\mathfrak{o}(3)=\left\{\mathbf{\varphi }\in {\mathbb{R}}^3,\mathbf{\Phi }={\mathbf{\varphi }}^{\wedge }\in {\mathbb{R}}^{3\times 3}\right\}$$

现在，我们说，R 是某个相机的旋转，它会随时间连续地变化，即为时间的函数：R(t)由于它仍是旋转矩阵，有
$$\mathbf{R}(t)\mathbf{R}(t{)}^T=\mathbf{I}$$
在等式两边对时间求导：
$$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^T=-{\left(\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^T\right)}^T$$
引入了^符号，将一个向量变成了反对称矩阵。同理，对于任意反对称矩阵，我们亦能找 到一个与之对应的向量。把这个运算用符号${}^{\vee }$表示：
$${\mathbf{a}}^{\wedge }=\mathbf{A}=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right],{\mathbf{A}}^{\vee }=\mathbf{a}$$
于是，由于 $\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^T$ 是一个反对称矩阵，我们可以找到一个三维向量 $\mathbf{\varphi }(t)\in {\mathbb{R}}^3$ 与
之对应。于是有：
$$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^T=\mathbf{\varphi }(t{)}^{\wedge }$$
等式两边右乘 $\mathbf{R}(t),$ 由于 $\mathbf{R}$ 为正交阵，有
$$\dot{\mathbf{R}}(t)=\varphi (t{)}^{\wedge }\mathbf{R}(t)=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]\mathbf{R}(t)$$
上式是一个关于 R 的微分方程，而且我们知道初始值 $\mathbf{R}(0)=\mathbf{I},$ 解之，得:
$$\mathbf{R}(t)=\exp \left({\varphi }_0^{\wedge }t\right)$$
$\exp \left({\varphi }^{\wedge }\right)$ 是如何计算的？它是一个矩阵的指数，在李群和李 代数中，称为指数映射(ExponentialMap)。
任意矩阵的指数映射可以写成一个泰勒展开，但是只有在收敛的情况下才会有结果, 其结果仍是一个矩阵。
$$\exp \left({\varphi }^{\wedge }\right)=\sum _{n=0}^{\infty }\frac{1}{n!}{\left({\varphi }^{\wedge }\right)}^n$$
我们来仔细推导一下这个定义。由于 $\varphi$ 是三维向量，我们可以定义它的模长和它的
方向，分别记作 $\theta$ 和 $\mathbf{a},$ 于是有 $\mathbf{\varphi }=\theta \mathbf{a}\circ$ 这里 $\mathbf{a}$ 是一个长度为 1 的方向向量。首先，对
于 ${\mathbf{a}}^{\wedge },$ 有以下两条性质:
$$\begin{array}{rl}& {\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 }\end{array}$$
它们提供了处理${\mathbf{a}}^{\wedge }$高阶项的方法。利用这两个性质, 我们可以把指数映射写成：
$$\begin{array}{rl}\exp \left({\varphi }^{\wedge }\right)& =\exp \left(\theta {\mathbf{a}}^{\wedge }\right)=\sum _{n=0}^{\infty }\frac{1}{n!}{\left(\theta {\mathbf{a}}^{\wedge }\right)}^n\\ & =\mathbf{I}+\theta {\mathbf{a}}^{\wedge }+\frac{1}{2!}{\theta }^2{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\frac{1}{3!}{\theta }^3{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\frac{1}{4!}{\theta }^4{\left({\mathbf{a}}^{\wedge }\right)}^4+\dots \\ & =\mathbf{a}{\mathbf{a}}^T-{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\theta {\mathbf{a}}^{\wedge }+\frac{1}{2!}{\theta }^2{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }-\frac{1}{3!}{\theta }^3{\mathbf{a}}^{\wedge }-\frac{1}{4!}{\theta }^4{\left({\mathbf{a}}^{\wedge }\right)}^2+\dots \\ & =\mathbf{a}{\mathbf{a}}^T+\left(\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-\dots \right){\mathbf{a}}^{\wedge }-\left(1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-\dots \right){\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }\\ & ={\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\mathbf{I}+\sin \theta {\mathbf{a}}^{\wedge }-\cos \theta {\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }\\ & =(1-\cos \theta ){\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\mathbf{I}+\sin \theta {\mathbf{a}}^{\wedge }\\ & =\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{a}{\mathbf{a}}^T+\sin \theta {\mathbf{a}}^{\wedge }\end{array}$$
最后我们得到罗德里格斯公式:
$$\exp \left(\theta {\mathbf{a}}^{\wedge }\right)=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{a}{\mathbf{a}}^T+\sin \theta {\mathbf{a}}^{\wedge }$$

SE (3)上的指数映射

对于 $SE(3),$ 它也有对应的李代数 $\mathfrak{s}\mathfrak{e}(3)\circ$ 为省略篇幅，我们就不描述如何引出 $\mathfrak{s}\mathfrak{e}(3)$
了。与 $\mathfrak{s}\mathfrak{o}(3)$ 相似，$\mathfrak{s}\mathfrak{e}(3)$ 位于 ${\mathbb{R}}^6$ 空间中:
$\mathfrak{s}\mathfrak{e}(3)=\left\{\mathbf{\xi }=\left[\begin{array}{c}\mathbf{\rho }\\ \mathbf{\varphi }\end{array}\right]\in {\mathbb{R}}^6,\mathbf{\rho }\in {\mathbb{R}}^3,\mathbf{\varphi }\in \mathfrak{s}\mathfrak{o}(3),{\mathbf{\xi }}^{\wedge }=\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}$
我们把每个 $\mathfrak{s}\mathfrak{e}(3)$ 元素记作 $\mathbf{\xi },$ 它是一个六维向量。前三维为平移，记作 $\mathbf{\rho };$ 后三维为旋转，记作 $\varphi ,$ 实质上是 $\mathfrak{s}\mathfrak{o}(3)$ 元素。同时，我们拓展了${}^{\wedge }$符号的含义。在 $\mathfrak{s}\mathfrak{e}(3)$ 中, 同样使用 ^ 符号，将一个六维向量转换成四维矩阵，但这里不再表示反对称:
$${\mathbf{\xi }}^{\wedge }=\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}$$
下面我们来介绍 $\mathfrak{s}\mathfrak{e}(3)$ 上的指数映射。为了节省篇幅，我们不再像 $\mathfrak{s}\mathfrak{o}(3)$ 那样详细推 导指数映射。$\mathfrak{s}\mathfrak{e}(3)$ 上的指数映射形式如下:
$$\begin{array}{rl}\exp \left({\mathbf{\xi }}^{\wedge }\right)& =\left[\begin{array}{cc}{\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\mathbf{\rho }\\ 0^T& 1\end{array}\right]\\ & \triangleq \left[\begin{array}{cc}\mathbf{R}& \mathbf{J}\mathbf{\rho }\\ 0^T& 1\end{array}\right]=\mathbf{T}\end{array}$$
如果你有耐心，可以照着 $\mathfrak{s}\mathfrak{o}(3)$ 上的做法推导，把 exp 进行泰勒展开推导此式。从结 果上看，$\xi$的指数映射左上角的 R 是我们熟知的 $\mathfrak{s}\mathfrak{o}(3)$ 中的元素，与 $\mathfrak{s}\mathfrak{e}(3)$ 当中的旋转部
分 $\varphi$ 对应。而右上角的 J 则可整理为（设 $\varphi =\theta a$):
$$J=\frac{\sin \theta }{\theta }I+\left(1-\frac{\sin \theta }{\theta }\right)a{a}^T+\frac{1-\cos \theta }{\theta }{a}^{\wedge }$$

李代数求导与扰动模型

BCH公式与近似形式

BCH公式告诉我们，当处理两个矩阵指数之积时，它们会产生一些由
李括号组成的余项。
In the particular case of $SO(3)$, we can show that
$$\begin{array}{l}\ln {\left({\mathbf{C}}_1{\mathbf{C}}_2\right)}^{\vee }=\ln {\left(\exp \left({\varphi }_1^{\wedge }\right)\exp \left({\varphi }_2^{\wedge }\right)\right)}^{\vee }\\ ={\varphi }_1+{\varphi }_2+\frac{1}{2}{\varphi }_1^{\wedge }{\varphi }_2+\frac{1}{12}{\varphi }_1^{\wedge }{\varphi }_1^{\wedge }{\varphi }_2+\frac{1}{12}{\varphi }_2^{\wedge }{\varphi }_2^{\wedge }{\varphi }_1+\cdots \end{array}$$
where ${\mathbf{C}}_1=\exp \left({\varphi }_1^{\wedge }\right),{\mathbf{C}}_2=\exp \left({\varphi }_2^{\wedge }\right)\in SO(3).$ Alternatively, if we as-
sume that ${\varphi }_1$ or ${\varphi }_2$ is small, then using the approximate BCH formulas we can show that
$$\begin{array}{r}\ln {\left({\mathbf{C}}_1{\mathbf{C}}_2\right)}^{\vee }=\ln {\left(\exp \left({\varphi }_1^{\wedge }\right)\exp \left({\varphi }_2^{\wedge }\right)\right)}^{\vee }\approx \{\begin{array}{rl}{\mathbf{J}}_{\ell }{\left({\varphi }_2\right)}^{-1}{\varphi }_1+{\varphi }_2& \text{ if }{\varphi }_1\text{ small }\\ {\varphi }_1+{\mathbf{J}}_r{\left({\varphi }_1\right)}^{-1}{\varphi }_2& \text{ if }{\varphi }_2\text{ small }\end{array}\end{array}$$
本书以左乘为例。左乘 BCH 近似雅可比 $J_l$实际上就是$SE(3)$上的指数映射中的$J$
$${\mathbf{J}}_l=\mathbf{J}=\frac{\sin \theta }{\theta }\mathbf{I}+\left(1-\frac{\sin \theta }{\theta }\right)\mathbf{a}{\mathbf{a}}^T+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }$$
它的逆为:
$${\mathbf{J}}_l^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}\mathbf{I}+\left(1-\frac{\theta }{2}\cot \frac{\theta }{2}\right)\mathbf{a}{\mathbf{a}}^T-\frac{\theta }{2}{\mathbf{a}}^{\wedge }$$
而右乘雅可比仅需要对自变量取负号即可：
$${\mathbf{J}}_r(\mathbf{\varphi })={\mathbf{J}}_l(-\mathbf{\varphi })$$

假定对某个旋转 R，对应的李代数为 $\varphi$ 。我们给它左乘一个微小旋转，记作 $\Delta \mathbf{R}$ 对应的李代数为 $\Delta \varphi 。$ 那么，在李群上，得到的结果就是 $\Delta \mathbf{R}\cdot \mathbf{R},$ 而在李代数上，根据 BCH 近似，为: $J_l^{-1}(\varphi )\Delta \varphi +\varphi$ 。合并起来，可以简单地写成:
$$\exp \left(\Delta {\varphi }^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)=\exp \left({\left(\varphi +J_l^{-1}(\varphi )\Delta \varphi \right)}^{\wedge }\right)$$
反之，如果我们在李代数上进行加法，让一个 $\varphi$ 加上 $\Delta \varphi ,$ 那么可以近似为李群上带 左右雅可比的乘法:
$$\exp \left((\varphi +\Delta \varphi {)}^{\wedge }\right)=\exp \left({\left(J_l\Delta \varphi \right)}^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)=\exp \left({\varphi }^{\wedge }\right)\exp \left({\left(J_r\Delta \varphi \right)}^{\wedge }\right)$$
这将为之后李代数上的做微积分提供了理论基础。同样的，对于 SE(3)，亦有类似的 BCH 近似公式:
$$\exp \left(\Delta {\xi }^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\approx \exp \left({\left({\mathcal{J}}_l^{-1}\Delta \mathbf{\xi }+\mathbf{\xi }\right)}^{\wedge }\right)$$
$$\exp \left({\mathbf{\xi }}^{\wedge }\right)\exp \left(\Delta {\mathbf{\xi }}^{\wedge }\right)\approx \exp \left({\left({\mathcal{J}}_r^{-1}\Delta \mathbf{\xi }+\mathbf{\xi }\right)}^{\wedge }\right)$$

SO(3)李代数上的求导

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

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

首先，考虑$SO(3)$上的情况。假设我们对一个空间点$p$进行了旋转，得到了$Rp$。现在，要计算旋转之后点的坐标相对于旋转的导数，由于 SO(3) 没有加法，所以该导数无法按?导数的定义进行计算。设 R 对应的李代数为 $\mathbf{\varphi },$ 我们转而计算:
$$\frac{\partial \left(\exp \left({\varphi }^{\wedge }\right)p\right)}{\partial \varphi }$$
按照导数的定义，有：
$$\begin{array}{rl}\frac{\partial \left(\exp \left({\varphi }^{\wedge }\right)\mathbf{p}\right)}{\partial \varphi }& =\underset{\delta \varphi \to 0}{\lim }\frac{\exp \left((\varphi +\delta \varphi {)}^{\wedge }\right)\mathbf{p}-\exp \left({\varphi }^{\wedge }\right)\mathbf{p}}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{\exp \left({\left({\mathbf{J}}_l\delta \varphi \right)}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\varphi }^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{\left(\mathbf{I}+{\left({\mathbf{J}}_l\delta \mathbf{\varphi }\right)}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\varphi }^{\wedge }\right)\mathbf{p}}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{{\left({\mathbf{J}}_l\delta \mathbf{\varphi }\right)}^{\wedge }\exp \left({\varphi }^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{-{\left(\exp \left({\varphi }^{\wedge }\right)\mathbf{p}\right)}^{\wedge }{\mathbf{J}}_l\delta \mathbf{\varphi }}{\delta \mathbf{\varphi }}=-(\mathbf{R}\mathbf{p}{)}^{\wedge }{\mathbf{J}}_l\end{array}$$
第二行的近似为BCH线性近似，第三行为泰勒展开舍去高阶项后近似，第四行至第五行将反对称符号看作叉积，交换之后变号。于是，我们推导了旋转后的点相对于李代数的导数：
$$\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \mathbf{\varphi }}=(-\mathbf{R}\mathbf{p}{)}^{\wedge }{\mathbf{J}}_l$$

扰动模型（左乘）

另一种求导方式，是对$ R $进行一次扰动 $\Delta \mathbf{R}$。 这个扰动可以乘在左边也可以乘在右边，最后结果会有一点儿微小的差异，我们以左扰动为例。设左扰动 $\Delta \mathbf{R}$ 对应的李代数 为$\phi$。然后，对 $\phi$ 求导，即:
$$\begin{array}{rl}\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \mathbf{\phi }}& =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\exp \left({\mathbf{\phi }}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\phi }\\ & \approx \underset{\mathbf{\phi }\to 0}{\lim }\frac{\left(1+{\mathbf{\phi }}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\phi }\\ & =\underset{\mathbf{\phi }\to 0}{\lim }\frac{{\mathbf{\phi }}^{\wedge }\mathbf{R}\mathbf{p}}{\mathbf{\phi }}=\underset{\phi \to 0}{\lim }\frac{-(\mathbf{R}\mathbf{p}{)}^{\wedge }\mathbf{\phi }}{\mathbf{\phi }}=-(\mathbf{R}\mathbf{p}{)}^{\wedge }\end{array}$$
可见，扰动模型相比于直接对李代数求导，省去了一个雅可比$J_l$的计算。这使得扰动模型更为实用。请读者务必理解这里的求导运算，这在位姿估计当中具有重要的意义。

SE(3)上的李代数求导

最后，我们给出 SE(3) 上的扰动模型，而直接李代数上的求导就不再介绍了。假设 恭空间点 $\mathbf{p}$ 经过一次变换 $\mathbf{T}$ (对应李代数为 $\mathbf{\xi }$), 得到 $\mathbf{T}\mathbf{p}$ 。现在，给 $\mathbf{T}$ 左乘一个扰动 $\Delta \mathbf{T}=\exp \left(\delta {\mathbf{\xi }}^{\wedge }\right),$ 我们设扰动项的李代数为 $\delta \mathbf{\xi }=[\delta \mathbf{\rho },\delta \mathbf{\varphi }{]}^T,$ 那么：
$$\begin{array}{rl}\frac{\partial (\mathbf{T}\mathbf{p})}{\partial \delta \mathbf{\xi }}& =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ & \approx \underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left(\mathbf{I}+\delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\delta {\mathbf{\xi }}^{\wedge }\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{cc}\delta {\mathbf{\varphi }}^{\wedge }& \delta \mathbf{\rho }\\ 0^T& 0\end{array}\right]\left[\begin{array}{c}\mathbf{R}\mathbf{p}+\mathbf{t}\\ 1\end{array}\right]}{\delta \mathbf{\xi }}\end{array}$$

$$=\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{c}\delta {\varphi }^{\wedge }(\mathbf{R}\mathbf{p}+\mathbf{t})+\delta \mathbf{\rho }\\ 0\end{array}\right]}{\delta \mathbf{\xi }}=\left[\begin{array}{cc}\mathbf{I}& -(\mathbf{R}\mathbf{p}+\mathbf{t}{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]\triangleq (\mathbf{T}\mathbf{p}{)}^{\odot }$$