4 李群和李代数

在SLAM中，除了表示外，还需要对其进行估计和优化。因为在SLAM中位姿是未知的，需要解决什么样的相机位姿最符合当前观测数据这样的问题。一种典型的方式是构建成一个优化问题，求解最优的$\mathbf{R},\mathbf{T}$，使得误差最小化。

旋转矩阵自身带有约束（正交且行列式为1），作为优化变量时会引入额外的约束，使优化变得困难。通过李群-李代数间的转换关系，希望把位姿估计变成无约束的优化问题，简化求解方式。

4.1 李群和李代数基础

特殊正交群：

$$\mathrm{SO}(n)=\{\mathbf{R}\in {\mathbb{R}}^{n\times n}|\mathbf{R}{\mathbf{R}}^{\mathrm{\top }}=\mathbf{I},\det (\mathbf{R})=1\}$$

特殊欧氏群：

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

旋转矩阵和变换矩阵对于加法不封闭，但是对于乘法封闭。乘法对应着旋转或变换的复合。

4.1.1 群

群是一种集合加上一种运算的代数结构，满足：

• 封闭性
• 结合律
• 幺元
• 逆

旋转矩阵集合和矩阵乘法构成群，变换矩阵和矩阵乘法也构成群。

矩阵中常见的群：

• 一般线性群 $\mathrm{GL}(\mathrm{n})$ $n\times n$的可逆矩阵
• 特殊正交群
• 特殊欧氏群

李群是指具有连续（光滑）性质的群，像整数群那样离散的群没有连续性质，不是李群。特殊正交群和特殊欧氏群在实数空间上是连续的，都是李群。能够直观想象一个刚体连续在空间中运动。

4.1.2 李代数的引出

考虑任意旋转矩阵 $\mathbf{R}$ 满足：

$$\mathbf{R}{\mathbf{R}}^{\mathrm{\top }}=\mathbf{I}$$

$\mathbf{R}$ 是某个相机的旋转，随时间连续变化，即为时间的函数 $\mathbf{R}(t)$：

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

两边对时间求导，得到：

$$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^{\mathrm{\top }}+\mathbf{R}(t)\dot{\mathbf{R}}(t{)}^{\mathrm{\top }}=0$$$$\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^{\mathrm{\top }}=-{[\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^{\mathrm{\top }}]}^{\mathrm{\top }}$$

可以看出， $\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^{\mathrm{\top }}$ 是一个反对称矩阵。同理，对于任意反对称矩阵，也可以找到与之对应的向量，用 ${}^{\vee }$ 表示：

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

设 $t_0=0$ ，并设此时旋转矩阵为 $\mathbf{R}(0)=\mathbf{I}$ 。按照导数定义，可以把 $\mathbf{R}(t)$ 在0附近进行一阶泰勒展开：

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

$\varphi$ 反映了 $\mathbf{R}$ 的导数性质，称它在 $\mathrm{SO}(3)$ 原点附近的正切空间。同时在 $t_0$ 附近，设 $\varphi$ 保持为常数 $\varphi (t_0)={\varphi }_0$ ，有：

$$\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=0$ 附近有效。旋转矩阵通过指数关系与另一个反对称矩阵发生了联系。

4.1.3 李代数的定义

每个李群都有与之对应的李代数。李代数描述了李群的局部性质。通用的李代数的定义如下：

李代数由一个集合 $\mathbb{V}$ ，一个数域 $\mathbb{F}$ 和一个二元运算（李括号） $[,]$ 。若满足：

• 封闭性：$\mathrm{\forall }\mathit{X}\mathbf{,}\mathit{Y}\in \mathbb{V},[\mathit{X}\mathbf{,}\mathit{Y}]\in \mathbb{V}$

• 双线性：$\mathrm{\forall }\mathit{X}\mathbf{,}\mathit{Y}\mathbf{,}\mathit{Z}\in \mathbb{V},a,b\in \mathbb{F},$

  $$[a\mathit{X}+b\mathit{Y},\mathit{Z}]=a[\mathit{X},\mathit{Z}]+b[\mathit{Y},\mathit{Z}],[\mathit{Z},a\mathit{X}+b\mathit{Y}]=a[\mathit{Z},\mathit{X}]+b[\mathit{Z},\mathit{Y}]$$

• 自反性：$\mathrm{\forall }\mathit{X}\in \mathbb{V},[\mathit{X},\mathit{X}]=\mathbf{0}$

• Jacobi等价：$\mathrm{\forall }\mathit{X}\mathbf{,}\mathit{Y}\mathbf{,}\mathit{Z}\in \mathbb{V},$

  $$[\mathit{X},[\mathit{Y},\mathit{Z}]]+[\mathit{Z},[\mathit{X},\mathit{Y}]]+[\mathit{Y},[\mathit{Z},\mathit{X}]]=\mathbf{0}$$

李括号表达了两个元素的差异。它不要求结合律，而要求元素和自己做李括号之后为零的性质。

三维空间中定义的叉积是一种李括号， $\mathfrak{g}=({\mathbb{R}}^3,\mathbb{R},\times )$ 构成一个李代数。

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

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

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

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

$\mathfrak{so}(3)$的元素是3维向量或者3维反对称矩阵：

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

$\mathfrak{so}(3)$是一个由三维向量组成的集合，每个向量对应到一个反对称矩阵，可以表达旋转矩阵的导数。它与 $\mathrm{SO}(3)$的关系由指数映射给定：

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

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

对于 $\mathrm{SE}(3)$ ，也有相应的李代数 $\mathfrak{se}(3)$ ，是一个6维向量：前3维为平移，后3维为旋转。

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

此处拓展了^的含义。

李代数 $\mathfrak{se}(3)$ 也有类似的李括号：

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

4.2 指数与对数映射

4.2.1 $\mathrm{SO}(3)$上的指数映射

计算$\exp ({\varphi }^{\wedge })$

通过泰勒展开写出，即：

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

令$\varphi =\theta \mathit{a}$，这里$\mathit{a}$为长度为1的方向向量。有以下两条性质：

• $${\mathit{a}}^{\wedge }{\mathit{a}}^{\wedge }=\mathit{a}{\mathit{a}}^{\mathrm{\top }}-\mathbf{I}$$
• $${\mathit{a}}^{\wedge }{\mathit{a}}^{\wedge }{\mathit{a}}^{\wedge }=-{\mathit{a}}^{\wedge }$$

推导得到：

$$\exp ({\varphi }^{\wedge })=\cos \theta \mathbf{I}+(1-\cos \theta )\mathit{a}{\mathit{a}}^{\mathrm{\top }}+\sin \theta {\mathit{a}}^{\wedge }$$

此公式和罗德里格斯公式如出一辙。这表明，$\mathfrak{so}(3)$ 实际上就是由所谓的旋转向量组成的空间

定义对数映射，也可以把 $\mathrm{SO}(3)$ 中的元素对应到 $\mathfrak{so}(3)$ 中

指数映射只是一个满射，可能存在多个 $\mathfrak{so}(3)$ 中的元素对应到同一个 $\mathrm{SO}(3)$。至少对于旋转角 $\theta$，多转一圈和没有多转对于旋转矩阵是没有区别的。如果把旋转角度固定在 $\pm \pi$，李群和李代数是一一对应的。

4.2.2 $\mathrm{SE}(3)$上的指数映射

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

其中，

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

附：$\mathit{J}\to \mathbf{R}$

$$\begin{array}{rl}\mathbf{R}& =\sum _{n=0}^{\mathrm{\infty }}\frac{1}{n!}({\mathit{\varphi }}^{\wedge }{)}^n=\sum _{n=1}^{\mathrm{\infty }}\frac{1}{n!}({\mathit{\varphi }}^{\wedge }{)}^n+\mathbf{I}\\ & =\sum _{n=0}^{\mathrm{\infty }}\frac{1}{(n+1)!}({\mathit{\varphi }}^{\wedge }{)}^{n+1}+\mathbf{I}\\ & ={\mathit{\varphi }}^{\wedge }\sum _{n=0}^{\mathrm{\infty }}\frac{1}{(n+1)!}({\mathit{\varphi }}^{\wedge }{)}^n+\mathbf{I}\\ & ={\mathit{\varphi }}^{\wedge }\mathit{J}+\mathbf{I}\\ & =\sin \theta {\mathit{a}}^{\wedge }+(1-\cos \theta ){\mathit{a}}^{\wedge }{\mathit{a}}^{\wedge }\end{array}$$

由于 ${\mathit{\varphi }}^{\wedge }$ 不是满秩矩阵，所以反过来不行

Figure 1: 李群和李代数的关系.

4.3 BCH公式与扰动模型

4.3.1 BCH公式与近似形式

BCH公式：

$$\ln (\exp (\mathit{A})\exp (\mathit{B}))=\mathit{A}+\mathit{B}+\frac{1}{2}[\mathit{A},\mathit{B}]+\frac{1}{12}[\mathit{A},[\mathit{A},\mathit{B}]]-\frac{1}{12}[\mathit{B},[\mathit{A},\mathit{B}]]+...$$

当处理2个矩阵指数之积时，产生一些由李括号组成的余项。当${\varphi }_1$或${\varphi }_2$为小量时，二次以上的项都可以被忽略掉。BCH有线性近似表达：

$$\ln (\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{cc}{\mathit{J}}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2& {\varphi }_1\approx 0\\ {\mathit{J}}_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1& {\varphi }_2\approx 0\end{array}$$

对一个旋转矩阵左乘一个微小旋转矩阵，可以近似看作在原有的李代数加上一项。注意左乘近似和右乘近似。

$$\begin{array}{rl}& {\mathit{J}}_l=\mathit{J}=\frac{\sin \theta }{\theta }\mathbf{I}+(1-\frac{\sin \theta }{\theta })\mathit{a}{\mathit{a}}^{\mathrm{\top }}+\frac{1-\cos \theta }{\theta }{\mathit{a}}^{\wedge }\\ & {\mathit{J}}_l^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}\mathbf{I}+(1-\frac{\theta }{2}\cot \frac{\theta }{2})\mathit{a}{\mathit{a}}^{\mathrm{\top }}-\frac{\theta }{2}{\mathit{a}}^{\wedge }\\ & {\mathit{J}}_r(\varphi )={\mathit{J}}_l(-\varphi )\end{array}$$

BCH近似的意义：

$$\begin{array}{rl}& \exp (\mathrm{\Delta }{\varphi }^{\wedge })\exp ({\varphi }^{\wedge })=\exp (({\mathit{J}}_l^{-1}(\varphi )\mathrm{\Delta }\varphi +\varphi {)}^{\wedge })\\ & \exp ((\varphi +\mathrm{\Delta }\varphi {)}^{\wedge })=\exp (({\mathit{J}}_l\mathrm{\Delta }\varphi {)}^{\wedge })\exp ({\varphi }^{\wedge })=\exp ({\varphi }^{\wedge })\exp (({\mathit{J}}_r\mathrm{\Delta }\varphi {)}^{\wedge })\end{array}$$

对于 $\mathrm{SE}(3)$，有类似的公式，其中近似雅可比不同。

$\mathcal{J}$

4.3.2 $\mathrm{SO}(3)$李代数上的求导

对于李代数求导有很强的实际背景。不妨设小robot在某个时刻的位姿为 $\mathbf{T}$，它观察到了一个世界坐标位于 $\mathit{p}$ 的点，产生了一个观测数据 $\mathit{z}$。由坐标变换关系：

$$\mathit{z}=\mathbf{T}\mathit{p}+\mathit{w}$$

由于观测噪声 $\mathit{w}$ 的存在，$\mathit{z}$ 不可能精确满足 $\mathit{z}=\mathbf{T}\mathit{p}$ 的关系。通常会计算立项的观测与实际数据的误差：

$$\mathit{e}=\mathit{z}-\mathbf{T}\mathit{p}$$

假设一共有 $N$ 个这样的路标点和观测，就有 $N$ 个上式。对小robot的位姿估计，相当于寻找一个最优的 $\mathbf{T}$，使得整体误差最小化：

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

求解此问题需要计算目标函数 $J$ 关于变换矩阵 $\mathbf{T}$ 的导数。我们经常会构建并讨论与位姿有关的函数，以调整当前的估计值。

使用李代数解决求导问题的思路恩分为2种：

1. 用李代数表示姿态，根据李代数加法对李代数求导
2. 对李群左乘或右乘微小扰动，对该扰动求导

4.3.3 李代数求导

考虑一个空间点 $\mathit{p}$，对其进行旋转

$$\begin{array}{rl}\frac{\partial (\exp ({\varphi }^{\wedge })\mathit{p})}{\partial \varphi }& =\underset{\delta \varphi \to 0}{lim}\frac{\exp ((\varphi +\delta \varphi {)}^{\wedge })\mathit{p}-\exp ({\varphi }^{\wedge })\mathit{p}}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{lim}\frac{\exp (({\mathit{J}}_l\delta \varphi {)}^{\wedge })\exp ({\varphi }^{\wedge })\mathit{p}-\exp ({\varphi }^{\wedge })\mathit{p}}{\delta \varphi }\\ & \approx \underset{\delta \varphi \to 0}{lim}\frac{(\mathbf{I}+({\mathit{J}}_l\delta \varphi {)}^{\wedge })\exp ({\varphi }^{\wedge })\mathit{p}-\exp ({\varphi }^{\wedge })\mathit{p}}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{lim}\frac{({\mathit{J}}_l\delta \varphi {)}^{\wedge }\exp ({\varphi }^{\wedge })\mathit{p}}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{lim}\frac{-(\exp ({\varphi }^{\wedge })\mathit{p}{)}^{\wedge }{\mathit{J}}_l\delta {\varphi }^{\wedge }}{\delta \varphi }=-(\mathbf{R}\mathit{p}{)}^{\wedge }{\mathit{J}}_l\end{array}$$

其中第2行 BCH 线性近似，第3行泰勒展开舍去高阶项后的近似，第4-第5行将反对称符号看作叉积，交换之后变号。

4.3.4 扰动模型

另一种求导方式是对 $\mathbf{R}$ 进行一次扰动 $\mathrm{\Delta }\mathbf{R}$，可左乘也可以右乘。以左扰动为例，设左扰动 $\mathrm{\Delta }\mathbf{R}$ 对应的李代数为 $\phi$。对 $\phi$ 求导：

$$\begin{array}{rl}\frac{\partial \mathbf{R}\mathit{p}}{\partial \phi }& =\underset{\phi \to 0}{lim}\frac{\exp ({\phi }^{\wedge })\exp ({\varphi }^{\wedge })\mathit{p}-\exp ({\varphi }^{\wedge })\mathit{p}}{\phi }\\ & \approx \underset{\phi \to 0}{lim}\frac{(\mathbf{I}+{\phi }^{\wedge })\exp ({\varphi }^{\wedge })\mathit{p}-\exp ({\varphi }^{\wedge })\mathit{p}}{\phi }\\ & =\underset{\phi \to 0}{lim}\frac{{\phi }^{\wedge }\mathbf{R}\mathit{p}}{\phi }=\underset{\phi \to 0}{lim}\frac{-(\mathbf{R}\mathit{p}{)}^{\wedge }\phi }{\phi }=-(\mathbf{R}\mathit{p}{)}^{\wedge }\end{array}$$

设右扰动 $\mathrm{\Delta }\mathbf{R}$ 对应的李代数为 $\phi$：

$$\begin{array}{rl}\frac{\partial \mathit{R}\mathit{p}}{\partial \phi }& =\underset{\phi \to 0}{lim}\frac{\exp ({\varphi }^{\wedge })\exp ({\phi }^{\wedge })\mathit{p}-\exp ({\varphi }^{\wedge })\mathit{p}}{\phi }\\ & \approx \underset{\phi \to 0}{lim}\frac{\exp ({\varphi }^{\wedge })(\mathbf{I}+{\phi }^{\wedge })\mathit{p}-\exp ({\varphi }^{\wedge })\mathit{p}}{\phi }\\ & =\underset{\phi \to 0}{lim}\frac{\mathbf{R}{\phi }^{\wedge }\mathit{p}}{\phi }=\underset{\phi \to 0}{lim}\frac{-\mathit{R}{\mathit{p}}^{\wedge }\phi }{\phi }=-\mathit{R}{\mathit{p}}^{\wedge }\end{array}$$

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

假设某空间点 $\mathit{p}$ 经过一次变换 $\mathbf{T}$ （对应李代数为 $\mathit{\xi }$），得到 $\mathbf{T}\mathit{p}$ （空间点是齐次坐标的形式）。现在给 $\mathbf{T}$ 一个左乘扰动，$\mathrm{\Delta }\mathbf{T}=\exp (\delta {\mathit{\xi }}^{\wedge })$ ，设扰动项的李代数为 $\delta \mathit{\xi }=[\delta \mathit{\rho },\delta \mathit{\varphi }{]}^{\mathrm{\top }}$ ，则：

$$\begin{array}{rl}\frac{\partial \mathit{T}\mathit{p}}{\partial \delta \mathit{\xi }}& =\underset{\delta \mathit{\xi }\to \mathbf{0}}{lim}\frac{\exp \left(\delta {\mathit{\xi }}^{\wedge }\right)\exp \left({\mathit{\xi }}^{\wedge }\right)\mathit{p}-\exp \left({\mathit{\xi }}^{\wedge }\right)\mathit{p}}{\delta \mathit{\xi }}\\ & =\underset{\delta \mathit{\xi }\to \mathbf{0}}{lim}\frac{(\mathbf{I}+\delta {\mathit{\xi }}^{\wedge })\exp \left({\mathit{\xi }}^{\wedge }\right)\mathit{p}-\exp \left({\mathit{\xi }}^{\wedge }\right)\mathit{p}}{\delta \mathit{\xi }}\\ & =\underset{\delta \mathit{\xi }\to \mathbf{0}}{lim}\frac{\delta {\mathit{\xi }}^{\wedge }\exp \left({\mathit{\xi }}^{\wedge }\right)\mathit{p}}{\delta \mathit{\xi }}\\ & =\underset{\delta \mathit{\xi }\to \mathbf{0}}{lim}\frac{\left[\begin{array}{cc}\delta {\mathit{\varphi }}^{\wedge }& \delta \mathit{\rho }\\ \\ {\mathbf{0}}^{\mathrm{T}}& 0\end{array}\right]\left[\begin{array}{c}\mathbf{R}\mathit{p}+\mathbf{T}\\ \\ 1\end{array}\right]}{\delta \mathit{\xi }}\\ & =\underset{\delta \mathit{\xi }\to \mathbf{0}}{lim}\frac{\left[\begin{array}{c}\delta {\mathit{\varphi }}^{\wedge }(\mathbf{R}\mathit{p}+\mathbf{T})+\delta \mathit{\rho }\\ \\ {\mathbf{0}}^{\mathrm{T}}\end{array}\right]}{[\delta \mathit{\rho },\delta \mathit{\varphi }{]}^{\mathrm{T}}}=\left[\begin{array}{cc}\mathbf{I}& -{(\mathbf{R}\mathit{p}+\mathbf{T})}^{\wedge }\\ \\ {\mathbf{0}}^{\mathrm{T}}& {\mathbf{0}}^{\mathrm{T}}\end{array}\right]\stackrel{\mathrm{def}}{=}{\left(\mathbf{T}\mathit{p}\right)}^{\odot }.\end{array}$$

将最后的结果定义成算符 $\odot$：将一个齐次的空间坐标点变换成一个 4x6 的矩阵，需要解释下矩阵求导的规则：

$$\frac{\mathrm{d}\left[\begin{array}{c}\mathit{a}\\ \mathit{b}\end{array}\right]}{\mathrm{d}\left[\begin{array}{c}\mathit{x}\\ \mathit{y}\end{array}\right]}={\left(\frac{\mathrm{d}[\mathit{a},\mathit{b}{]}^{\mathrm{T}}}{\mathrm{d}\left[\begin{array}{c}\mathit{x}\\ \mathit{y}\end{array}\right]}\right)}^{\mathrm{T}}={\left[\begin{array}{cc}\frac{\mathrm{d}\mathit{a}}{\mathrm{d}\mathit{x}}& \frac{\mathrm{d}\mathit{b}}{\mathrm{d}\mathit{x}}\\ \frac{\mathrm{d}\mathit{a}}{\mathrm{d}\mathit{y}}& \frac{\mathrm{d}\mathit{b}}{\mathrm{d}\mathit{y}}\end{array}\right]}^{\mathrm{T}}$$

右扰动如下：

$$\begin{array}{rl}\frac{\partial \mathit{T}\mathit{p}}{\partial \delta \mathit{\xi }}& =\underset{\delta \mathit{\xi }\to \mathbf{0}}{lim}\frac{\exp \left({\mathit{\xi }}^{\wedge }\right)\exp \left(\delta {\mathit{\xi }}^{\wedge }\right)\mathit{p}-\exp \left({\mathit{\xi }}^{\wedge }\right)\mathit{p}}{\delta \mathit{\xi }}\\ & =\underset{\delta \mathit{\xi }\to \mathbf{0}}{lim}\frac{\exp \left({\mathit{\xi }}^{\wedge }\right)(\mathbf{I}+\delta {\mathit{\xi }}^{\wedge })\mathit{p}-\exp \left({\mathit{\xi }}^{\wedge }\right)\mathit{p}}{\delta \mathit{\xi }}\\ & =\underset{\delta \mathit{\xi }\to \mathbf{0}}{lim}\frac{\exp \left({\mathit{\xi }}^{\wedge }\right)\delta {\mathit{\xi }}^{\wedge }\mathit{p}}{\delta \mathit{\xi }}\\ & =\underset{\delta \mathit{\xi }\to \mathbf{0}}{lim}\frac{\left[\begin{array}{cc}\mathbf{R}& \mathbf{T}\\ \\ {\mathbf{0}}^{\mathrm{T}}& 1\end{array}\right]\left[\begin{array}{cc}\delta {\mathit{\varphi }}^{\wedge }& \delta \mathit{\rho }\\ \\ {\mathbf{0}}^{\mathrm{T}}& 0\end{array}\right]\mathit{p}}{\delta \mathit{\xi }}\\ & =\underset{\delta \mathit{\xi }\to \mathbf{0}}{lim}\frac{\left[\begin{array}{c}\mathbf{R}\delta {\mathit{\varphi }}^{\wedge }{\mathit{p}}_{(0:3)}+\mathbf{R}\delta \mathit{\rho }\\ {\mathbf{0}}^{\mathrm{T}}\end{array}\right]}{[\delta \mathit{\rho },\delta \mathit{\varphi }{]}^{\mathrm{T}}}=\left[\begin{array}{cc}\mathbf{R}& -\mathbf{R}{\mathit{p}}_{(0:3)}^{\wedge }\\ \\ {\mathbf{0}}^{\mathrm{T}}& {\mathbf{0}}^{\mathrm{T}}\end{array}\right].\end{array}$$

4.4.2 评估轨迹的误差

实际工程中经常需要评估一个算法的估计轨迹与真实轨迹的差异来评价算法的精度。真实轨迹往往通过某些更高精度的系统获得，而估计的轨迹则是由待评价的算法计算得到的。

误差指标可以有很多种，常见的有绝对轨迹误差 $\mathrm{ATE}$：

$${\mathrm{ATE}}_{all}=\sqrt{\frac{1}{N}\sum _{i=1}^N||\log ({\mathbf{T}}_{gt,i}^{-1}{\mathbf{T}}_{esti,i}{)}^{\vee }|{|}_2^2}$$

这实际上是每个位姿李代数的均方根误差 RMSE 。这种误差可以刻画两条轨迹的旋转和平移误差。

也有文献仅考虑平移误差，因为从整条轨迹上看，旋转出现误差后，随后的轨迹在平移上也会出现误差。定义绝对平移误差：

$${\mathrm{ATE}}_{trans}=\sqrt{\frac{1}{N}\sum _{i=1}^N||trans({\mathbf{T}}_{gt,i}^{-1}{\mathbf{T}}_{esti,i})|{|}_2^2}$$

除此，也可以定义相对的误差。例如，考虑 $i$ 时刻到 $i+\mathrm{\Delta }t$ 时刻的运动，那相对位姿误差 $\mathrm{RPE}$ 可定义为：

$${\mathrm{RPE}}_{all}=\sqrt{\frac{1}{N-\mathrm{\Delta }t}\sum _{i=1}^{N-\mathrm{\Delta }t}||\log (({\mathbf{T}}_{gt,i}^{-1}{\mathbf{T}}_{gt,i+\mathrm{\Delta }t}{)}^{-1}({\mathbf{T}}_{esti,i}^{-1}{\mathbf{T}}_{esti,i+\mathrm{\Delta }t}){)}^{\vee }|{|}_2^2}$$

同样的，也可以只取平移部分。

$${\mathrm{RPE}}_{trans}=\sqrt{\frac{1}{N-\mathrm{\Delta }t}\sum _{i=1}^{N-\mathrm{\Delta }t}||trans(({\mathbf{T}}_{gt,i}^{-1}{\mathbf{T}}_{gt,i+\mathrm{\Delta }t}{)}^{-1}({\mathbf{T}}_{esti,i}^{-1}{\mathbf{T}}_{esti,i+\mathrm{\Delta }t}))|{|}_2^2}$$

4.5* 相似变换群与李代数

在单目视觉中使用相似变换群 $\mathrm{Sim}(3)$，以及对应的李代数 $\mathfrak{sim}(3)$ 。如果你只对双目 SLAM 或 RGBD-SLAM 感兴趣,可以跳过本节。

单目具有尺度不确定性，如果在单目 SLAM 中使用 $SE(3)$ 表示位姿,那 么由于尺度不确定性与尺度漂移,整个 SLAM 过程中的尺度会发生变化,这在 $SE(3)$ 中未能体现出来。因此,在单目情况下我们一般会显式地把尺度因子表达出来。用数学语言来说,对于位于空间的点 $\mathit{p}$ ,在相机坐标系下要经过一个相似变换,而非欧氏变换:

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

在相似变换中,我们把尺度 $s$ 表达了出来。它同时作用在 $\mathit{p}$ 的三个坐标之上,对 $\mathit{p}$ 进行了一次缩放。与 $\mathrm{SO}(3)$ 、$\mathrm{SE}(3)$ 相似,相似变换亦对矩阵乘法构成群,称为相似变换群 $\mathrm{Sim}(3)$ ：

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

同样地, $\mathrm{Sim}(3)$ 也有对应的李代数、指数映射、对数映射等等。李代数 $\mathfrak{sim}(3)$ 元素是一个七维向量 $\mathit{\zeta }$ 。它的前六维与 $\mathfrak{se}(3)$ 相同,最后多了一项 $\sigma$。

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

它比 $\mathfrak{se}(3)$ 多了一项 $\sigma$ 。关联 $Sim(3)$ 和 $\mathfrak{sim}(3)$ 的仍是指数映射和对数映射。指数映射为:

$$\exp ({\mathit{\zeta }}^{\wedge })=\left[\begin{array}{cc}{e}^{\sigma }\exp ({\mathit{\varphi }}^{\wedge })& {\mathit{J}}_s\mathit{\rho }\\ {\mathbf{0}}^{\mathrm{\top }}& 1\end{array}\right]$$$${\mathit{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}{\mathit{a}}^{\wedge }+(\frac{(e^{\sigma }\cos \theta -1)\sigma +(e^{\sigma }\sin \theta )\theta }{{\sigma }^2+{\theta }^2}){\mathit{a}}^{\wedge }{\mathit{a}}^{\wedge }$$

通过指数映射,我们能够找到李代数与李群的关系。对于李代数元素 $\mathit{\zeta }$ ,它与李群的对应关系为:

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

旋转部分和 $\mathrm{SO}(3)$ 是一致的。平移部分，在 $\mathfrak{se}(3)$ 中需要乘一个雅可比，而相似变换的雅可比更复杂一些。对于尺度因子，可以看到李群中的 $s$ 即为李代数中 $\sigma$ 的指数函数。

$\mathrm{Sim}(3)$ 的 BCH 近似与 $\mathrm{SE}(3)$ 是类似的。我们可以讨论一个点 $\mathit{p}$ 经过相似变换 $\mathbf{S}\mathit{p}$ 后,相对于 $\mathbf{S}$ 的导数。同样的,存在微分模型和扰动模型两种方式,而扰动模型较为简单。我们省略推导过程,直接给出扰动模型的结果。设给予 $\mathbf{S}\mathit{p}$ 左侧一个小扰动 $\exp ({\mathit{\zeta }}^{\wedge })$ ,并求 $\mathbf{S}\mathit{p}$ 对于扰动的导数。因为 $\mathbf{S}\mathit{p}$ 四维的齐次坐标， $\mathit{\zeta }$ 是七维向量,该导数应该是 $4\times 7$ 的雅可比。为了方便起见,记 $\mathbf{S}\mathit{p}$ 的前三维组成向量 $\mathit{q}$ ，那么:

$$\frac{\partial \mathbf{S}\mathit{p}}{\partial \mathit{\zeta }}=\left[\begin{array}{ccc}\mathbf{I}& -{\mathit{q}}^{\wedge }& \mathit{q}\\ {\mathbf{0}}^{\mathrm{\top }}& {\mathbf{0}}^{\mathrm{\top }}& 0\end{array}\right]$$

4.6 习题

5. 证明

$$\mathbf{R}{\mathit{p}}^{\wedge }{\mathbf{R}}^{\mathrm{\top }}=(\mathbf{R}\mathit{p}{)}^{\wedge }$$

pf:

$$\begin{array}{rl}& \mathbf{R}{\mathit{p}}^{\wedge }{\mathbf{R}}^{\mathrm{\top }}=(\mathit{R}\mathit{p}{)}^{\wedge }\\ ⟺& \mathbf{R}{\mathit{p}}^{\wedge }=(\mathit{R}\mathit{p}{)}^{\wedge }\mathbf{R}\\ ⟺& \mathrm{\forall }\mathit{u}\in {\mathbb{R}}^3,\mathbf{R}{\mathit{p}}^{\wedge }\mathit{u}=(\mathit{R}\mathit{p}{)}^{\wedge }\mathbf{R}\mathit{u}\\ ⟺& \mathrm{\forall }\mathit{u}\in {\mathbb{R}}^3,\mathbf{R}(\mathit{p}\times \mathit{u})=(\mathit{R}\mathit{p})\times (\mathbf{R}\mathit{u})\end{array}$$

根据向量叉乘的旋转变换不变性，可知上式成立。

⼏何角度来理解：两个向量 $\mathit{v},\mathit{u}$，其叉乘得到⼀个与 $\mathit{v},\mathit{u}$ 两者垂直的三维向量 $(\mathit{v}\times \mathit{u})$，将这三个向量都经过同⼀个旋转，它们的相对位姿和模长都不会改变。

6. 证明

$$\mathbf{R}\exp ({\mathit{p}}^{\wedge }){\mathbf{R}}^{\mathrm{\top }}=\exp [(\mathbf{R}\mathit{p}{)}^{\wedge }]$$

该式称为 $\mathrm{SO}(3)$ 上的伴随性质。

$$pf:\begin{array}{rl}& \mathbf{R}\exp ({\mathit{p}}^{\wedge }){\mathbf{R}}^{\mathrm{\top }}\\ =& \mathbf{R}(\mathbf{I}+\delta {\mathit{p}}^{\wedge }+\frac{1}{2!}(\delta {\mathit{p}}^{\wedge }{)}^2+...){\mathbf{R}}^{\mathrm{\top }}\\ =& \mathbf{I}+\mathbf{R}\delta {\mathit{p}}^{\wedge }{\mathbf{R}}^{\mathrm{\top }}+\frac{1}{2!}\mathbf{R}(\delta {\mathit{p}}^{\wedge }{)}^2{\mathbf{R}}^{\mathrm{\top }}+...\\ =& \mathbf{I}+(\mathbf{R}\delta \mathit{p}{)}^{\wedge }+\frac{1}{2!}[\mathbf{R}(\delta {\mathit{p}}^{\wedge }){\mathbf{R}}^{\mathrm{\top }}](\mathbf{R}(\delta {\mathit{p}}^{\wedge }){\mathbf{R}}^{\mathrm{\top }})+...\\ =& \exp [(\mathit{R}\mathit{p}{)}^{\wedge }]\end{array}$$

因此，有如下公式，在预积分中大放异彩（流形的交换律）：

$$\exp (\overrightarrow{\varphi })\mathbf{R}=\mathbf{R}\exp ({\mathbf{R}}^{\mathrm{\top }}\overrightarrow{\varphi })$$