视觉SLAM基础-李群和李代数

李群和李代数

目录

• 李群和李代数
  • 引言
  • 1.0 李群
    • 1.1 群
    • 1.2 李群基础定义
  • 2.0李代数
    • 2.1 引出
    • 2.2 李代数的定义
    • 2.3 李代数 $so(3)$
    • 2.4 李代数 $se(3)$
  • 3.0 指数和对数映射
    • 3.1 $SO(3)$ 上的指数映射
    • 3.2 $SE(3)$ 上的指数映射
  • 4.0 李代数求导与扰动模型
    • 4.1 李群和李代数在计算上的对应关系
    • 4.2 李代数求导模型
    • 4.3 扰动模型
    • 4.4 $SE(3)$ 上的扰动模型
    • 4.5 相似变换及其李群和李代数
  • 5.0 Sophus 代码解析

引言

​ 为什么会有李群和李代数的引出。在通常的 SLAM 中，我们估计的无非就是在极短的时间内物体的一个相对位姿运动，然后进行累加，即可得到物体的当前位置，即 SLAM 中的定位问题，但是往往该运动在较短的时间内其变化量是极小的。

​ 通常其运动变化我们可以使用旋转加平移进行表示，即 $(R,t)=T$ 。但是在实际中，我们时常想要知道这一微小的变化量，不能一直将每一次变化都记录下来，这时候就联想到了导数，表示的正是在某一点的一个变化率。可是对于平移 $t$ 来说，其是具有加法运算的，但是对于 $R$ 来说，两个旋转矩阵相加，是没有意义的，而且相加之后是破坏了旋转矩阵 $R{R}^T=I$ 的性质的。

​ 怎么解决呢，这就要引出李群和李代数来解决。

​ $R$ 和 $T$ 都是李群的一种，虽然其不具有加法运算，但是当把其映射到李代数空间上时，就具有了加法的性质，在李代数上进行加法计算后，或者说是导数运算后，再将其映射到对应的李群即可。

​ 前面我们介绍了若干种描述三维世界刚体运动的方式。除了用它们来描述相机的位姿之外，还要对它们进行估计和优化。这个问题可以描述为什么样的相机位姿最符合当前观测数据。而典型的做法就是将其构建成一个优化问题来求解。

1.0 李群

上一讲在介绍旋转矩阵和变换矩阵时，我们说三维旋转矩阵构成了特殊正交群 $SO(3)$，变换矩阵构成了特殊欧式群 $SE(3)$。我们知道，矩阵对于乘法是封闭的，即假设相机进行了两次连续的旋转，旋转矩阵分别为${\mathbf{R}}_1$ 和${\mathbf{R}}_2$, 这两个矩阵相乘后得到的也是个旋转矩阵，表示了总的旋转。也即：

$$\mathrm{\forall }{\mathbf{R}}_1,{\mathbf{R}}_2\in SO(3),{\mathbf{R}}_1{\mathbf{R}}_2\in SO(3)$$

对变换矩阵也是如此。但这二者对于加法是不封闭的。两个变换矩阵相加后得到的矩阵并不是一个变换矩阵。

1.1 群

​ 如此，我们就可以引出群的定义：群（group）是一种集合加上一种运算的代数结构。把集合记为$\mathbf{A}$，运算记为$\cdot$, 群就可以记为$\mathbf{G}=(\mathbf{A},\cdot )$。
​ 三维旋转矩阵构成了特殊正交群( $SpecialOrthogonalGroup$ )

$$SO(3)=\{\mathit{R}\in {\mathbb{R}}^{3\times 3}\mid \mathit{R}{\mathit{R}}^T=\mathit{I},\det (\mathit{R})=1\}$$

​ 三维变换矩阵构成了特殊欧氏群（$SpecialEuclideanGroup$）

$$SE(3)=\{T=\left[\begin{array}{cc}\mathit{R}& t\\ 0^T& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\mid \mathit{R}\in SO(3),t\in {\mathbb{R}}^3\}$$

​ 什么是群？群（Group）是一种集合加上一种运算的代数结构。

记集合为$A$，运算为 · ，那么当运算满足以下性质时，称 ($A$, · )成群：

• 封闭性：

$$\mathrm{\forall }{a}_1,a_2\in A,a_1\cdot a_2\in A.$$

• 结合率：

$$\mathrm{\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).$$

• 幺元：

$$\mathrm{\exists }{a}_0\in A,s.t.\mathrm{\forall }a\in A,a_0\cdot a=a\cdot a_0=a.$$

• 逆：

$$\mathrm{\forall }a\in A,\mathrm{\exists }{a}^{-1}\in A,s.t.a\cdot a^{-1}=a_0.$$

​ 可以验证，旋转矩阵集合和变换矩阵集合分别和矩阵乘法构成群。还有其他很多群，如整数的加法等。群结构保证了在群上的运算具有良好的性质。

1.2 李群基础定义

​ 定义：李群就是具有连续（光滑）性质的群。

​ 前面举的整数的加法的例子显然不是连续的，因而它不是李群。但 $SO(3)$ 和 $SE(3)$ 在实数空间上是连续的（机器人在三维空间中显然是连续地运动，而不会进行“瞬移”）。

​ 介绍完李群，在引入李代数之前，我们来回顾下开头我们提到的问题：为什么要用到李群和李代数？避免一直是数学上的推导。我们用一个比较实际的例子。假设某个时刻我们预测机器人的位姿为$\mathbf{T}$（待定值）, 它观测到了一个惯性坐标系下的点$\mathbf{p}$ 而产生了一个观测数据 $\mathbf{z}$，它是该点在相机坐标系下的坐标，则可得

$$\mathbf{z}=\mathbf{T}\mathbf{p}+\omega$$

​ 其中，$\omega$ 是观测噪声。由于观测噪声的存在，$\mathbf{z}$ 无法严格满足式$\mathbf{z}=\mathbf{T}\mathbf{p}$。因此而产生的误差$\mathbf{e}$ 为

$$\mathbf{e}=\mathbf{z}-\mathbf{Tp}$$

​ 若共有N 个观测值，那么就有N 个这样的式子。机器人的位姿估计就转变成寻找一个最优的$\mathbf{T}$ 使得整体的误差最小化：

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

通常，直接求解上式得出最优的$\mathbf{T}$ 是很困难的（或计算量很大）。我们常常先给定一个猜测值（初始值）${\mathbf{T}}_0$，然后不断地对它进行迭代更新。而这个过程需要用到导数（可以想想梯度下降法）。回顾导数的定义，$\dot{f(x)}=\underset{\mathrm{\Delta }x\to 0}{lim}\frac{f(x+\mathrm{\Delta }x)-f(x)}{\mathrm{\Delta }x}$。显然计算导数和进行更新时都要用到加法。但SO(3) 和SE(3) 上对矩阵加法的运算并不封闭。如果要继续采取这个迭代更新的策略势必要再想想办法，使得导数“可行”。而这就可以通过李群及其对应的李代数来实现。

2.0李代数

2.1 引出

现在我们考虑任意的旋转矩阵$\mathbf{R}$，根据上一讲的知识，有

$$\mathbf{R}{\mathbf{R}}^T=\mathbf{I}$$

因为机器人在不断运动，所以$\mathbf{R}$ 也在随时间不断变换，引入时间$t$ 就得到$\mathbf{R}(t)$，上式也变成

$$\mathbf{R}(t)\mathbf{R}(t{)}^T=\mathbf{I}$$

等式两边一起对时间求导可得：

$$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^T+\mathbf{R}(t)\dot{\mathbf{R}}(t{)}^T=0$$

亦即

$$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^T=-\mathbf{R}(t)\dot{\mathbf{R}}(t{)}^T=-(\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^T{)}^T$$

一个矩阵等于其自身转置后取负号，可知$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^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}$$

设这个对应的向量为$\varphi (t)\in R^3$，就有

$$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^T=\varphi (t{)}^{\wedge }$$

等式两边同时右乘$\mathbf{R}(t)$ 则有

$$\dot{\mathbf{R}}(t)=\varphi (t{)}^{\wedge }\mathbf{R}(t)$$

从式 $(11)$ 可以看到，每对旋转矩阵求一次导数，只需要左乘一个$\varphi (t{)}^{\wedge }$ 就可以了。假设 $t_0=0$ 并设此时$\mathbf{R}(0)=\mathbf{I}$，对式 $(10)$ 进行一阶泰勒展开就有：

$$\mathbf{R}(t)\approx \mathbf{R}(t_0)+\dot{\mathbf{R}}(t_0)(t-t_0)=\mathbf{I}+\varphi (t_0{)}^{\wedge }\cdot (t)$$

NOTE：$\varphi$ 反应了$\mathbf{R}$ 的导数的性质，故称它在 $SO(3)$ 原点附近的正切空间（$TangentSpace$）上。

同时，在 $t_0$ 附近，设$\varphi$ 也是一个常数，即$\varphi (t_0)={\varphi }_0$，根据式(11) 就有

$$\dot{\mathbf{R}}(t)=\varphi (t_0{)}^{\wedge }\mathbf{R}(t)={\varphi }_0^{\wedge }\mathbf{R}(t)$$

显然，上式是一个关于$\mathbf{R}$ 的微分方程，而且知道初始值$\mathbf{R}(0)=\mathbf{I}$，他的解为

$$\mathbf{R}(t)=\exp ({\varphi }_0^{\wedge }t)$$

由于我们之前做了一些假设，所以这个式子只在 $t$ 附近有效。

但通过式 $(14)$ 我们知道，当某个时刻的$\mathbf{R}$ 已知时，存在一个向量$\varphi$，二者满足这个矩阵指数关系 。而这个$\varphi$，就是对应到 $SO(3)$ 上的李代数 $so(3)$。下面我们就介绍一下李代数的定义和如何计算矩阵指数$\exp ({\varphi }^{\wedge })$.

2.2 李代数的定义

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

• 封闭性：$\mathrm{\forall }\mathbf{X},\mathbf{Y}\in V,[\mathbf{X},\mathbf{Y}]\in V$.
• 双线性：$\mathrm{\forall }\mathbf{X},\mathbf{Y},\mathbf{Z}\in V,a,b\in F$, 有$[a\mathbf{X}+b\mathbf{Y},\mathbf{Z}]=a[\mathbf{X},\mathbf{Z}]+[\mathbf{Y},\mathbf{Z}],[\mathbf{Z},a\mathbf{X}+b\mathbf{Y}]=a[\mathbf{Z},\mathbf{X}]+b[\mathbf{Z},\mathbf{Y}]$
• 自反性：$\mathrm{\forall }\mathbf{X}\in V,[\mathbf{X},\mathbf{X}]=\mathbf{0}$.
• 雅可比等价：$\mathrm{\forall }\mathbf{X},\mathbf{Y},\mathbf{Z}\in V,[\mathbf{X},[\mathbf{Y},\mathbf{Z}]]+[\mathbf{Z},[\mathbf{X},\mathbf{Y}]]+[\mathbf{Y},[\mathbf{Z},\mathbf{X}]]=\mathbf{0}$.

其中的二元运算$[,]$ 称为李括号，它表达了两个元素的差异。三维向量 $R^3$上定义的叉积就是一种李括号，$g=(R^3,R,\times )$ 就构成了李代数。

2.3 李代数 $so(3)$

之前我们提到的SO(3) 对应的向量$\varphi$ 就是一种李代数，是定义在R³ 上的向量。它的反对称矩阵记为$\mathbf{\Phi }={\varphi }^{\wedge }$. 在定义下面，两个向量${\varphi }_1,{\varphi }_2$ 的李括号为：

$$[{\varphi }_1,{\varphi }_2]=({\mathbf{\Phi }}_{\mathbf{1}}{\mathbf{\Phi }}_{\mathbf{2}}-{\mathbf{\Phi }}_{\mathbf{2}}{\mathbf{\Phi }}_{\mathbf{1}}{)}^{\vee }$$

高翔在书中没有明说，但经过计算可以得到结果为${\varphi }_1,{\varphi }_2$ 的叉积（又叫外积）。所以这里我猜测so(3) 对应的李括号为三维向量R³ 上的叉积。

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

至此，我们已经清楚了$so(3)$ 的内容：它们是一个由三维向量组成的集合；每个向量对应到一个反对称矩阵；（李括号为三维向量的外积运算）；可以用来表达旋转矩阵的导数，和 $SO(3)$ 的关系由指数映射给定：$\mathbf{R}=\exp ({\varphi }^{\wedge })$.

下面我们先看李代数 $se(3)$ 再来解释指数映射。

2.4 李代数 $se(3)$

对于$SE(3)$，它也有对应的李代数 $se(3)$。和 $so(3)$ 类似，$se(3)$ 在 $R^6$ 空间中：

$$se(3)=\left\{\begin{array}{c}\xi =\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]\in R^6,\rho \in R^3,\varphi \in so(3),{\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ {\mathbf{0}}^T& 0\end{array}\right]\in R^{4\times 4}\end{array}\right\}$$

把每个 $se(3)$ 元素记做$\xi$，它是一个六维向量：前三维为平移，记作$\rho$；后三维是旋转，记作$\varphi$，实质上是 $s0(3)$ 元素。此外，在 $se(3)$ 中，${}^{\wedge }$ 符号的含义被拓展了：这里它将一个六维向量转换为四维矩阵，但这里不再表示反对称矩阵。

同样，李代数 $se(3)$ 也有类似的李括号：

$$[{\xi }_1,{\xi }_2]=({\xi }_1^{\wedge }{\xi }_2^{\wedge }-{\xi }_2^{\wedge }{\xi }_1^{\wedge }{)}^{\vee }$$

3.0 指数和对数映射

3.1 $SO(3)$ 上的指数映射

如前所述，李代数到李群是一个指数映射。现在来看看指数映射是如何计算的。

任意矩阵的指数映射都可以写成一个泰勒展开。但要注意的是展开式只有在收敛的情况下才有解，其结果仍然是一个矩阵。所以，对 $so(3)$ 中的元素$\varphi$，其指数映射可以写成：

$$\exp ({\varphi }^{\wedge })=\sum _{n=0}^{\mathrm{\infty }}\frac{1}{n!}({\varphi }^{\wedge }{)}^n$$

将$\varphi$ 记为$\theta \mathbf{a}$，并利用其性质：(偶次项) ${\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 }$ 可得：

$$\begin{gathered} \exp {\varphi }^{\wedge }=\exp (\theta \mathbf{a})=\sum _{n=0}^{\mathrm{\infty }}\frac{1}{n!}(\theta {\mathbf{a}}^{\wedge }{)}^n \\ =\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{a}{\mathbf{a}}^T+\sin \theta {\mathbf{a}}^T \end{gathered}$$

回想前一讲，可以发现式$(20)$ 和罗德里格斯公式有着相同的形式。这表明 $so(3)$ 实际上就是由旋转向量组成的空间，而指数映射就是罗德里格斯公式。也可以定义一个对数映射，将 $SO(3)$ 映射到 $so(3)$ 上：

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

但上式太过复杂了。通常我们会用前一讲介绍的旋转向量到旋转矩阵的方式来计算旋转矩阵。

NOTE：指数映射是一个满射，即对每个SO(3) 中的元素都可以找到一个so(3) 的元素与之对应；但存在多个so(3) 元素对应到同一个SO(3) 上（一个旋转角为$\theta$ 的旋转向量多旋转一周得到的结果显然是一样的）。如果把旋转角固定在$\pm \pi$ 或者$[0,2\pi ]$ 之间，那么李群和李代数中的元素就是一一对应的。

3.2 $SE(3)$ 上的指数映射

SE(3) 上的指数映射推导原理与SO(3) 类似，但更复杂。这里直接给出结论：

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

$\exp ({\xi }^{\wedge })$ 的左上角是SO(3) 中的元素$\mathbf{R}$，是其旋转部分。而矩阵$\mathbf{J}$ 则为：

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

式(23) 与罗德里格斯公式相似。$\xi$ 中的平移部分$\rho$ 经过指数变换后，发生了一次以$\mathbf{J}$ 为系数的线性变换。

相反的，SE(3) 到se(3) 也有对应的对数映射。但一般先利用左上角的旋转矩阵计算出旋转向量；对右上角的平移向量$\mathbf{t}$ 则有：

$$\mathbf{t}=\mathbf{J}\rho$$

即可得到$\rho$。

4.0 李代数求导与扰动模型

4.1 李群和李代数在计算上的对应关系

至此，我们已经介绍完了李群和李代数还有它们之间的对应关系。但最后还有一个问题。我们知道，对于两次连续的旋转可以用矩阵乘法来描述，但用李代数如何表示呢？我们自然是希望李群中两个矩阵相乘对应李代数上对应元素的相加，这样我们才能够利用加法来计算导数和进行更新。但很遗憾，这是不成立的。

两个李代数指数映射的乘积，由BCH 公式给出：

$$\ln (\exp (\mathbf{A})\exp (\mathbf{B}))=\mathbf{A}+\mathbf{B}+\frac{1}{12}[\mathbf{A},\mathbf{B}]+\frac{1}{12}[\mathbf{A},[\mathbf{A},\mathbf{B}]]-\frac{1}{12}[\mathbf{B},[\mathbf{A},\mathbf{B}]]+\dots$$

其中，$[,]$ 为李括号。可以看到，两个矩阵指数的乘积是两个矩阵之和再加上一些由李括号组成的余项。

想象下机器人的运动，连续两帧间相机的位姿变化是一个小量；在优化过程中，通过求导而得到的更新量也是一个小量。更具体地，考虑SO(3) 上的李代数$\ln (\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge }){)}^{\vee }$，当${\varphi }_1$ 或者${\varphi }_2$ 为小量时，显然，小量二次以上的项都可以忽略。此时，BCH 的线性近似为：

$$\ln (\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{l}{\mathbf{J}}_l({\varphi }_2{)}^{-1}{\varphi }_1+\varphi 2,\mathrm{当}{\varphi }_1\mathrm{为}\mathrm{小}\mathrm{量},\\ {\mathbf{J}}_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1,\mathrm{当}{\varphi }_2\mathrm{为}\mathrm{小}\mathrm{量},\end{array}$$

解释：以第一个式子为例，当对一个旋转矩阵${\mathbf{R}}_{\mathbf{2}}$，对应李代数为${\varphi }_2$，左乘一个微小的旋转${\mathbf{R}}_1$ (李代数为${\varphi }_1$) 时，对应的李代数间的关系可以近似为原有的李代数${\varphi }_2$ 加上一项${\mathbf{J}}_l({\varphi }_2{)}^{-1}{\varphi }_1$。同理，第二个式子则描述了右乘的情况。所以，在使用 $BCH$ 近似公式时必须分清楚是左乘模型还是右乘模型。

对于左乘 $BCH$ 近似模型，雅可比矩阵 ${\mathbf{J}}_l$ 就是式 $(23)$ 中的内容：

$${\mathbf{J}}_l=\mathbf{J}=\frac{\sin \theta }{\theta }\mathbf{I}+(1-\frac{\sin \theta }{\theta })\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}+(1-\frac{\theta }{2}\cot \frac{\theta }{2})\mathbf{a}{\mathbf{a}}^T-\frac{\theta }{2}{\mathbf{a}}^{\wedge }$$

对于右乘模型，右乘雅可比矩阵就是将左乘雅可比的自变量取负号：

$${\mathbf{J}}_r(\varphi )={\mathbf{J}}_l(-\varphi )$$

小结一下，假设有某个旋转$\mathbf{R}$，对应的李代数为$\varphi$。对它左乘一个微小旋转，记作$\mathrm{\Delta }\mathbf{R}$，对应的李代数为$\mathrm{\Delta }\varphi$。那么，在李群上，得到的结果就是$\mathrm{\Delta }\mathbf{R}\cdot \mathbf{R}$。而在李代数上，为${\mathbf{J}}_l(\varphi {)}^{-1}\mathrm{\Delta }\varphi +\varphi$，即

$$\exp (\mathrm{\Delta }{\varphi }^{\wedge })\exp (\mathrm{\Delta }{\varphi }^{\wedge })=\exp ((\varphi +{\mathbf{J}}_l(\varphi {)}^{-1}\mathrm{\Delta }\varphi {)}^{\wedge })$$

反之，如果我们在李代数上进行加法，让$\varphi$ 加上$\mathrm{\Delta }\varphi$，那么可以近似为李群上带左/右雅可比矩阵的乘法：

$$\exp ((\varphi +\mathrm{\Delta }\varphi {)}^{\wedge })=\exp (({\mathbf{J}}_l\mathrm{\Delta }\varphi {)}^{\wedge })\exp ({\varphi }^{\wedge })=\exp ({\varphi }^{\wedge })\exp (({\mathbf{J}}_r\mathrm{\Delta }\varphi {)}^{\wedge })$$

同样地，对于$SE(3)$，也有类似的 $BCH$ 近似公式：

$$\begin{array}{}\text{(1)}& \exp (\mathrm{\Delta }{\xi }^{\wedge })\exp ({\xi }^{\wedge })\approx \exp (({\mathcal{J}}_{\mathcal{l}}^{\mathcal{-}\mathcal{1}}\mathcal{\Delta }\xi \mathcal{+}\xi {\mathcal{)}}^{\wedge }\mathcal{)}\text{(2)}& \exp ({\xi }^{\wedge })\exp (\mathrm{\Delta }{\xi }^{\wedge })\approx \exp (({\mathcal{J}}_{\mathcal{r}}^{\mathcal{-}\mathcal{1}}\mathcal{\Delta }\xi \mathcal{+}\xi {\mathcal{)}}^{\wedge }\mathcal{)}\end{array}$$

这里$\mathcal{J}$ 是一个$6\ast 6$ 的矩阵，形式较为复杂且在实际计算中并不用到矩该阵，所以略去其具体形式。

回顾介绍李群时举的那个例子。现在我们有了李代数，which 具有良好的加法运算。因此，我们可以用它来解决求导问题了。具体的思路有两种：

1. 用李代数来表示位姿，根据李代数加法来对李代数进行求导；
2. 利用李群来左乘或者右乘微小扰动，在对这个扰动的李代数进行求导。

4.2 李代数求导模型

假设空间点$\mathbf{p}$ 进行了一次矩阵表示为$\mathbf{R}$ 的旋转，得到了$\mathbf{R}\mathbf{p}$。要计算旋转后的点的坐标相对于旋转的导数，可以不严谨的写为：

$$\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \mathbf{R}}$$

前面提到，SO(3)没有加法，所以上式无法按照导数的定义进行计算。设$\mathbf{R}$ 对应的李代数为$\varphi$，就可以写成：

$$\frac{\partial (\exp ({\varphi }^{\wedge })\mathbf{p})}{\partial \varphi }$$

按照导数的定义展开，并在推导中利用 $BCH$ 公式和一阶泰勒展开，可以得到下式：

$$\frac{\partial (\exp ({\varphi }^{\wedge })\mathbf{p})}{\partial \varphi }=\underset{\delta \varphi \to 0}{lim}\frac{\exp ((\varphi +\delta \varphi {)}^{\wedge })\mathbf{p}-\exp ({\varphi }^{\wedge })\mathbf{p}}{\delta \varphi }=-(\mathbf{R}\mathbf{p}{)}^{\wedge }{\mathbf{J}}_l$$

这个公式是直接对旋转矩阵 $\mathbf{R}$ 进行求导，最后的结果中含有左雅可比矩阵 ${\mathbf{J}}_l$。

4.3 扰动模型

另一种方法就是先对旋转矩阵$\mathbf{R}$ 进行一次扰动$\mathrm{\Delta }\mathbf{R}$，然后再对个扰动进行求导。这里以左乘扰动模型为例。设左扰动的李代数为$\phi$，直接对$\phi$（而不是$\mathbf{R}$）进行求导：

$$\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \phi }=\underset{\phi \to 0}{lim}\frac{\exp ({\phi }^{\wedge })\exp ({\varphi }^{\wedge })\mathbf{p}-\exp ({\varphi }^{\wedge })\mathbf{p}}{\phi }=-(\mathbf{R}\mathbf{p}{)}^{\wedge }$$

和李代数求导模型进行对比，最明显的就是式(37) 少了左雅可比矩阵${\mathbf{J}}_l$。在扰动模型中，我们把小量$\mathrm{\Delta }\mathbf{R}$ 先直接乘在了李群上，然后再对这个小量的李代数进行求导。而在李代数求导模型中，我们是把这个小量$\delta \varphi$ 加在了李代数上，然后直接对对应的李代数进行求导。因此在所得的结果中就会有一个表示李群和李代数运算间关系（不是转换关系）的左雅可比矩阵${\mathbf{J}}_l$。

4.4 $SE(3)$ 上的扰动模型

假设某空间点$\mathbf{p}$ 经过了一次变换$\mathbf{T}$，对应的李代数为$\xi$，得到了$\mathbf{T}\mathbf{p}$。给$\mathbf{T}$ 左乘一个微小扰动$\mathrm{\Delta }\mathbf{T}$，扰动项的李代数为$\delta \xi =[\delta \rho ,\delta \varphi {]}^T$。则有：

$$\begin{array}{}\text{(3)}& \frac{\partial (\mathbf{T}\mathbf{p})}{\partial \delta \xi }& =\underset{\delta \xi \to 0}{lim}\frac{\exp (\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })\mathbf{p}-\exp ({\xi }^{\wedge })\mathbf{p}}{\delta \xi }\text{(4)}& & \approx \underset{\delta \xi \to 0}{lim}\frac{(\mathbf{I}+\delta {\xi }^{\wedge })\exp ({\xi }^{\wedge })\mathbf{p}-\exp ({\xi }^{\wedge })\mathbf{p}}{\delta \xi }\text{(5)}& & =\underset{\delta \xi \to 0}{lim}\frac{\delta {\xi }^{\wedge }\exp ({\xi }^{\wedge })\mathbf{p}}{\delta \xi }\text{(6)}& & =\underset{\delta \xi \to 0}{lim}\frac{\left[\begin{array}{cc}\delta \varphi & \delta \rho \\ {\mathbf{0}}^T& 0\end{array}\right]\left[\begin{array}{c}\mathbf{R}\mathbf{p}+\mathbf{t}\\ 1\end{array}\right]}{\delta \xi }\text{(7)}& & =\underset{\delta \xi \to 0}{lim}\frac{\left[\begin{array}{c}\delta {\varphi }^{\wedge }(\mathbf{R}\mathbf{p}+\mathbf{t})+\delta \rho \\ 0\end{array}\right]}{\delta \xi }\text{(8)}& & =\left[\begin{array}{cc}{\mathbf{I}}_{3\times 3}& -(\mathbf{R}\mathbf{p}+\mathbf{t}{)}_{3\times 3}^{\wedge }\\ {\mathbf{0}}_{1\times 3}^T& {\mathbf{0}}_{1\times 3}^T\end{array}\right]\text{(9)}& & =(\mathbf{T}\mathbf{p}{)}^{⨀}\end{array}$$

最后的结果被定义为符号${}^{⨀}$ 的一个运算：把一个齐次坐标下的空间点变换成一个4*6 的矩阵。

4.5 相似变换及其李群和李代数

最后介绍下单目SLAM中会用到的相似变换群Sim(3)。由于单目视觉中存在一个尺度不确定性（简单说，把单目相机两次拍照的两张相片想象成双目相机一次拍的两张照片，这两次拍照中单目相机的位移（对应于双目中的基线）是不知道的，导致了在像素深度估计上的尺度不确定），因此在单目的情况下，需要把尺度因子显式表达出来。就由欧式变换到了相似变换。

设空间点为$\mathbf{p}$，相似变换为：

$${\mathbf{p}}^{\prime }=\left[\begin{array}{cc}s\mathbf{R}& \mathbf{t}\\ {\mathbf{0}}^T& 1\end{array}\right]=s\mathbf{R}\mathbf{p}+\mathbf{t}$$

这里，s 就是尺度因子，它相当于对p 进行了一次缩放。

显然，相似变换也对矩阵乘法构成群，称为相似变换群：

$$Sim(3)=\left\{\begin{array}{c}\mathbf{S}=\left[\begin{array}{cc}s\mathbf{R}& \mathbf{t}\\ {\mathbf{0}}^T& 1\end{array}\right]\in R^{4\times 4}\end{array}\right\}$$

它对应的李代数为sim(3)，是一个7维向量：前六维就是se(3)，最后一项为尺度因子$\sigma$。

$$sim(3)=\left\{\begin{array}{c}\zeta |\zeta =\left[\begin{array}{c}\rho \\ \varphi \\ \sigma \end{array}\right]\in R^7,{\zeta }^{\wedge }=\left[\begin{array}{cc}\sigma \mathbf{I}+{\varphi }^{\wedge }& \rho \\ {\mathbf{0}}^T& 0\end{array}\right]\in R^{4\times 4}\end{array}\right\}$$

二者间仍然是通过指数映射和对数映射相关联。指数映射为：

$$\exp ({\zeta }^{\wedge })=\left[\begin{array}{cc}{e}^{\sigma }\exp ({\varphi }^{\wedge })& {\mathbf{J}}_s\mathbf{rho}\\ {\mathbf{0}}^T& 1\end{array}\right]$$

其中，

$${\mathbf{J}}_s=\frac{e^{\sigma }-1}{\sigma }\mathbf{I}+\frac{\sigma e^{\sigma }\sin \theta +(1-e^{\sigma }\cos \theta )\theta }{{\sigma }^2+{\theta }^2}{\mathbf{a}}^{\wedge }+(\frac{e^{\sigma }-1}{\sigma }-\frac{(e^{\sigma }\cos \theta -1)\sigma +(e^{\sigma }\sin \theta )\theta }{{\sigma }^2+{\theta }^2}){\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }$$

可以看到，相似变换的雅可比矩阵远比前面的复杂。而对数映射可以写为：

$$s=e^{\sigma },\mathbf{R}=\exp ({\varphi }^{\wedge }),\mathbf{t}={\mathbf{J}}_s\rho$$

同样地，$Sim(3)$ 也具有 $BCH$ 近似公式，也有李代数倒数模型和扰动模型。这里我们介绍扰动模型。给$\mathbf{S}\mathbf{p}$ 左乘一个微小扰动$\exp ({\zeta }^{\wedge })$ 并对该微小扰动求导，记$\mathbf{S}\mathbf{p}$ 的前三维坐标为q，有

$$\frac{\partial (\mathbf{S}\mathbf{p})}{\partial \zeta }=\left[\begin{array}{ccc}{\mathbf{I}}_{3\times 3}& -{\mathbf{q}}_{3\times 3}^{\wedge }& {\mathbf{q}}_{3\times 1}\\ {\mathbf{0}}_{1\times 3}^T& {\mathbf{0}}_{1\times 3}^T& 0\end{array}\right]$$

显然，$\mathbf{S}\mathbf{p}$ 是一个4维齐次坐标，$\zeta$ 是一个7维向量。该导数就是一个$4\ast 7$ 大小的雅可比矩阵。

5.0 Sophus 代码解析

• 这里使用的是带有模板的 Sophus 库。
• 依赖 fmt

#include 
#include 
#include 
#include 
#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;
}