PDF 视觉SLAM十四讲从理论到实践 第2版

7-8讲（视觉里程计）知识笔记见 视觉SLAM十四讲从理论到实践第2版学习笔记(实践应用 P1)

9-10讲（后端）知识笔记见 视觉SLAM十四讲从理论到实践第2版学习笔记(实践应用 P2)

11-12讲（回环检测 & 建图）知识笔记见 视觉SLAM十四讲从理论到实践第2版学习笔记(实践应用 P3)

第1讲 预备知识

SLAM: Simultaneous Localization and Mapping（同时定位与地图构建）

指搭载特定传感器的主体，在没有环境先验信息的情况下，于运动过程中建立环境的模型，同时估计自己的运动。

如果传感器主要为相机，那就称为”视觉SLAM“。

第一部分 数学基础

第2讲 初识SLAM

传感器：

1. 携带于机器人本体上
   • 轮式编码器——轮子转动的角度
   • 相机——某种观测数据
   • 激光传感器——某种观测数据
   • 惯性测量单元（Inertial Measurement Unit，IMU）——运动的角速度和加速度
2. 安装于环境中
   • 导轨
   • 二维码标志

相机：

1. 单目（Monocular）

   只有一个摄像头

   单目SLAM估计的轨迹和地图将与真实的轨迹和地图相差一个因子，也就是尺度，由于单目SLAM无法仅凭图像确定这个真实尺度，所以称为尺度不确定性。

2. 双目（stereo）

   有两个摄像头

   两个相机的距离（称为基线）是已知的，通过基线估记每个像素的空间距离。

   计算量是双目的主要问题

3. 深度（RGB-D）

   多个摄像头，除了能采集到彩色图片，还能读出每个像素与相机之间的距离。

   可以通过红外结构光或Time of Flight（TOF）原理，像激光传感器那样，通过主动向物体发射光并接受返回的光，测出物体与相机之间的距离。

   优点：不像双目相机通过软件计算解决而是通过物理测量手段，相比于双目相机可节省大量计算资源。

   缺点：存在测量范围窄、噪声大、视野小、易受日光干扰、无法测量透视材质等诸多问题，在SLAM方面主要用于室内。室外则较难用。

经典视觉SLAM框架

视觉SLAM流程包含以下步骤：

1. 传感器信息读取

   相机图像信息的读取和预处理

2. 前端视觉里程计（Visual Odometry，VO）

   估算相邻图像间相机的运动以及局部地图的样子。VO又被称为前端。

3. 后端（非线性）优化

   后端接受不同时刻视觉里程计测量的相机位姿以及回环检测的信息，对他们进行优化，得到全局一致的轨迹和地图。在VO之后，又被称为后端。

4. 回环检测

   判断机器人是否到达过先前的位置，如果检测到回环，把信息提供给后端进行处理。

5. 建图

   根据估计的轨迹，建立与任务要求对应的地图。

视觉里程计

只计算相邻时刻的运动，而和过去的信息没有关联。

一方面，只要把相邻时刻的运动”串“起来，构成了机器人的运动轨迹，从而解决定位问题；

另一方面，跟据每个时刻的相机位置，计算出各像素对应的空间点的位置，就得到了地图。

但仅通过视觉里程计来估计轨迹，将不可避免出现累积漂移（Accumulating Drift）

这是由于视觉里程计在最简单的情况下只估计两个图像间的运动造成的。

每次估计都带有一定的误差，而由于里程计的工作方式，先前时刻的误差将会传递到下一时刻，导致经过一段时间之后，估计的轨迹将不再准确。

例如，机器人先向左转90°,再向右转90°。由于误差，把第一个90°估计成了89°。那我们就会尴尬地发现，向右转之后机器人的估计位置并没有回到原点。更糟糕的是，即使之后的估计完全准确，与真实值相比，都会带上这-1°的误差。

这也就是所谓的漂移（Drift )，导致无法建立一致的地图

为了解决漂移问题，还需要两种技术：后端优化和回环检测。

回环检测负责把“机器人回到原始位置”的事情检测出来，而后端优化则根据该信息,校正整个轨迹的形状。

后端优化

主要指处理SLAM过程中的噪声问题

后端优化要考虑的问题：如何从这些带有噪声的数据中估计整个系统的状态，以及这个状态估计的不确定性有多大——这称为最大后验概率估计（Maximum-a-Posteriori，MAP)。

这里的状态既包括机器人自身的轨迹，也包含地图。

在视觉SLAM中，前端和计算机视觉研究领域更为相关，比如图像的特征提取与匹配等;

后端则主要是滤波与非线性优化算法。

回环检测

又称闭环检测，主要解决位置估计随时间漂移的问题

为了实现回环检测，需要让机器人具有识别到过的场景的能力

可以判断图像间的相似性来完成回环检测

建图

一组空间点的集合、一个3D模型都可以称为地图，形式随SLAM的应用场合而定。

大体上可以分为度量地图和拓扑地图

度量地图（Metric Map）

度量地图强调精确地表示地图中物体的位置关系

通常用稀疏（Sparse） 与稠密（Dense） 对其分类

稀疏地图进行了一定程度的抽象，并不需要表达所有的物体。例如，选择一部分具有代表意义的东西，称之为路标（ Landmark )，那么一张稀疏地图就是由路标组成的地图，而不是路标的部分就可以忽略。

稠密地图着重于建模所有看到的东西

定位时用稀疏路标地图就足够了；而用于导航时，则往往需要稠密地图（否则撞上两个路标之间的墙怎么办？）

稠密地图通常按照某种分辨率，由许多个小块组成，在二维度量地图中体现为许多个小格子（Grid），而在三维度量地图中则体现为许多小方块（Voxel）。

一个小块含有占据、空闲、未知三种状态，以表达该格内是否有物体。这种地图需要存储每一个格点的状态，会耗费大量的存储空间，而且多数情况下地图的许多细节部分是无用的。

大规模度量地图有时会出现一致性问题。很小的一点转向误差，可能会导致两间屋子的墙出现重叠，使地图失效。

拓扑地图

相比于度量地图的精确性，拓扑地图更强调地图元素之间的关系。

拓扑地图是一个图（Graph），由节点和边组成，只考虑节点间的连通性。

放松了地图对精确位置的需要，去掉了地图的细节，是一种更为紧凑的表达方式。

然而，拓扑地图不擅长表达具有复杂结构的地图。

SLAM问题的数学表述

由于相机通常是在某些时刻采集数据的，所以只关心这些时刻的位置和地图。

把一段连续时间的运动变成了离散时刻 $t= 1,\cdots,K$ 当中发生的事情。

用χ表示位置，各时刻的位置记为$x_1,\cdots,x_K$，构成轨迹

地图方面，假设地图是由许多个路标组成的，而每个时刻，传感器会测量到一部分路标点，得到观测数据。

设路标点一共有$N$个，用$y_1,\cdots,y_n$表示

运动——考察从$k-1$时刻到$k$时刻，位置$x$是如何变化的？

用一个通用的抽象的数学模型说明：

$$x_k = f(x_{k-1}, u_k, w_k)$$

$u_k$是运动传感器的读数或者输入，$w_k$为该过程中加入的噪声。

用一个一般函数$f$来描述这个过程，而不指明$f$具体的作用方式。这使得整个函数可以指代任意的运动传感器/输入，成为一个通用的方程，而不必限定于某个特殊的传感器上。我们把它称为运动方程。

噪声的存在使模型变成了随机模型，每次运动过程中的噪声是随机的。

观测——在$k$时刻位于$x_k$处探测到了某一个路标$y_i$，如何用数学语言来描述？

观测方程描述的是，在$x_k$位置上看到某个路标点$y_j$时，产生了一个观测数据$z_{k,j}$。同样用一个抽象的函数$h$来描述这个关系：

$$z_{k,j}=h(y_j,x_k,v_{k,j})$$

这里，$v_{k,j}$是这次观测里的噪声。由于观测所用的传感器形式更多，这里的观测数据$z$及观测方程$h$也有许多不同的形式。

根据真实运动和传感器的种类，存在若干种参数化方式

关于运动方程，假设在平面中运动，那么位姿（位置+姿势）由两个位置和一个转角来描述，

即$x_k=[x_1,x_2,\theta]_k^T$，其中$x_1$，$x_2$是两个轴上的位置而$\theta$为转角

同时输入的指令是两个时间间隔位置和转角的变化量$u_K=[\Delta x_1,\Delta x_2,\Delta \theta]$

此时运动方程可具体化为

$$\begin{bmatrix} x_1\\ x_2\\ \theta\\ \end{bmatrix}_k=\begin{bmatrix} x_1\\ x_2\\ \theta\\ \end{bmatrix}_{k-1}+\begin{bmatrix} \Delta x_1\\ \Delta x_2\\ \Delta \theta\\ \end{bmatrix}_k+w_k$$

关于观测方程，以携带着一个二维激光传感器为例。激光传感器观测一个2D路标点时，能够测到两个量：路标点与机器本体之间的距离$r$和夹角$\phi$。记路标点为 $y_j= [y_1,y_2]_j^T$，位姿为$x_k=[x_1,x_2]_k^T$，观测数据为$z_{k,j}=[r_{k,j},\phi _{k,j}]^T$，那么观测方程就写为

$$\begin{bmatrix} r_{k,j}\\ \phi _{k,j}\\ \end{bmatrix}=\begin{bmatrix} \sqrt{(y_{1,j}-x_{1,k})^2+(y_{2,j}-x_{2,k})^2}\\ \operatorname{arctan}(\frac{y_{2,j}-x_{2,k}}{y_{1,j}-x_{1,k}})\\ \end{bmatrix}+v.$$

考虑视觉SLAM时，传感器是相机，那么观测方程就是“对路标点拍摄后，得到图像中的像素”的过程。

针对不同的传感器，这两个方程有不同的参数化形式。如果保持通用性，取成通用的抽象形式，那么SLAM过程可总结为两个基本方程：

$$\begin{cases} x_k = f(x_{k-1}, u_k, w_k),\quad k=1,\cdots,K\\ z_{k,j}=h(y_j,x_k,v_{k,j}),\quad (k,j)\in \Phi\\ \end{cases}$$

其中$\Phi$是一个集合，记录着在哪个时刻观察到了哪个路标（通常不是每个路标在每个时刻都能看到的，在单个时刻很可能只看到一小部分)。

这两个方程描述了最基本的SLAM问题：当知道运动测量的读数$u$，以及传感器的读数$z$时，如何求解定位问题（估计$x$）和建图问题（估计$y$）?

这时，SLAM问题建模成了一个状态估计问题：如何通过带有噪声的测量数据，估计内部的、隐藏着的状态变量。

状态估计问题的求解，与两个方程的具体形式，以及噪声服从哪种分布有关。按照运动和观测方程是否为线性，噪声是否服从高斯分布进行分类，分为线性/非线性和高斯/非高斯系统。

线性高斯系统（Linear Gaussian，LG系统）是最简单的，它的无偏的最优估计可以由卡尔曼滤波器（Kalman Filter，KF）给出。而在复杂的非线性非高斯系统（Non-Linear Non-Gaussian，NLNG系统）中，会使用扩展卡尔曼滤波器（Extended Kalman Filter，EKF）和非线性优化两大类方法去求解。

直至21世纪早期，以EKF为主的滤波器方法在SLAM中占据了主导地位。最早的实时视觉SLAM系统就是基于ERF开发的。随后，为了克服EKF的缺点（例如线性化误差和噪声高斯分布假设），人们开始使用粒子滤波器（Particle Filter）等其他滤波器，乃至使用非线性优化的方法。

时至今日，主流视觉SLAM使用以图优化（Graph Optimization）为代表的优化技术进行状态估计。认为优化技术已经明显优于滤波器技术，只要计算资源允许，通常都偏向于使用优化方法。

第3讲 三维空间刚体运动

旋转矩阵

点、向量和坐标系

用线性代数的知识来说，三维空间中的某个点的坐标也可以用$\mathbb{R}^3$来描述。

假设在这个线性空间内，该空间的一组基$(\boldsymbol e_1,\boldsymbol e_2,\boldsymbol e_3)$，那么，任意向量 $\boldsymbol a$ 在这组基下就有一个坐标：

$$\boldsymbol a =\begin{bmatrix} \boldsymbol e_1,\boldsymbol e_2,\boldsymbol e_3 \end{bmatrix}\begin{bmatrix} a_1\\ a_2\\ a_3\\ \end{bmatrix}=a_1\boldsymbol e_1+a_2\boldsymbol e_2+a_3\boldsymbol e_3$$

这里$(a_1,a_2,a_3)^T$称为 $\boldsymbol a$ 在此基下的坐标。

坐标的具体取值，一是和向量本身有关，二是和坐标系（基）的选取有关。

坐标系通常由3个正交的坐标轴组成（尽管也可以有非正交的，但实际中很少见)。

对于 $\boldsymbol a,\boldsymbol b \in \mathbb R^3$，向量内积：

$$\boldsymbol a\cdot \boldsymbol b=\boldsymbol a^T\boldsymbol b=\sum_{i=1}^3{a_ib_i}=\lvert \boldsymbol a \rvert \lvert \boldsymbol b \rvert \cos \langle \boldsymbol a,\boldsymbol b\rangle$$

其中 $\langle \boldsymbol a,\boldsymbol b\rangle$ 指向量 $\boldsymbol a, \boldsymbol b$ 的夹角，内积也可以描述向量间的投影关系

向量外积：

$$\boldsymbol{a} \times \boldsymbol{b}=\left\|\begin{array}{ccc} \boldsymbol{e}_{1} & \boldsymbol{e}_{2} & \boldsymbol{e}_{3} \\ a_{1} & a_{2} & a_{3} \\ b_{1} & b_{2} & b_{3} \end{array}\right\|=\left[\begin{array}{l} 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] \boldsymbol{b}^{\text {def }} \boldsymbol{a}^{\wedge} \boldsymbol{b} .$$

外积的结果是一个向量，其方向垂直于这两个向量，大小为$|\boldsymbol{a}||\boldsymbol{b}| \sin \langle\boldsymbol{a}, \boldsymbol{b}\rangle$，是两个向量张成的四边形的有向面积。

引入^符号，把 $\boldsymbol a$ 写成一个矩阵（反对称矩阵），外积 $\boldsymbol{a} \times \boldsymbol{b}$ 就写成矩阵向量乘法 $\boldsymbol{a}^{\wedge} \boldsymbol{b}$，变成线性运算。

任意向量都对应着唯一一个反对称矩阵，反之亦然：

$$\boldsymbol{a}^{\wedge}=\left[\begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0 \end{array}\right]$$

坐标系间的欧式变换

两个坐标系之间的运动由一个旋转加上一个平移组成，这种运动称为刚体运动

刚体运动过程中，同一个向量在各个坐标系下的长度和夹角都不会发生变化

两个坐标系之间相差了一个欧式变化

对于同一个向量 $\boldsymbol p$，它在世界坐标系（惯性坐标系，可认为是固定不变的）下的坐标 $\boldsymbol p_w$ 和在相机坐标系（移动坐标系）下的坐标 $\boldsymbol p_c$ 是不同的

这个变换关系由变化矩阵 $\boldsymbol T$ 来描述

欧式变换由旋转和平移组成

首先考虑旋转，设某个单位正交基$(\boldsymbol e_1,\boldsymbol e_2,\boldsymbol e_3)$经过一次旋转变成了$(\boldsymbol e^{\prime}_1,\boldsymbol e^{\prime}_2,\boldsymbol e^{\prime}_3)$，对于同一向量 $\boldsymbol a$（并没有随坐标系旋转发生运动），它在两个坐标系下的坐标为$\left[\boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3}\right]^T$和$\left[\boldsymbol{e}^{\prime}_{1}, \boldsymbol{e}^{\prime}_{2}, \boldsymbol{e}^{\prime}_{3}\right]^T$。

因为向量本身没变，所以根据坐标的定义有：

$$\left[\boldsymbol{e}_{1}, \boldsymbol{e}_{2}, \boldsymbol{e}_{3}\right]\left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[\boldsymbol{e}_{1}^{\prime}, \boldsymbol{e}_{2}^{\prime}, \boldsymbol{e}_{3}^{\prime}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right]$$

上述等式左右两边同时左乘$\left[\begin{array}{r} \boldsymbol{e}_{1}^{T} \\ \boldsymbol{e}_{2}^{\mathrm{T}} \\ \boldsymbol{e}_{3}^{\mathrm{T}} \end{array}\right]$，左边系数变成单位矩阵：

$$\left[\begin{array}{l} a_{1} \\ a_{2} \\ a_{3} \end{array}\right]=\left[\begin{array}{lll} \boldsymbol{e}_{1}^{T} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{1}^{\mathrm{T}} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{1}^{\mathrm{T}} \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{2}^{\mathrm{T}} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{2}^{\mathrm{T}} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{2}^{\mathrm{T}} \boldsymbol{e}_{3}^{\prime} \\ \boldsymbol{e}_{3}^{\mathrm{T}} \boldsymbol{e}_{1}^{\prime} & \boldsymbol{e}_{3}^{\mathrm{T}} \boldsymbol{e}_{2}^{\prime} & \boldsymbol{e}_{3}^{\mathrm{T}} \boldsymbol{e}_{3}^{\prime} \end{array}\right]\left[\begin{array}{c} a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime} \end{array}\right] \stackrel{\text { def }}{=} \boldsymbol{R} \boldsymbol{a}^{\prime}$$

中间矩阵 $\boldsymbol R$ 由两组基之间的内积组成，刻画了旋转前后同一个向量的坐标变换关系

只要旋转是一样的，这个矩阵就是一样的，$\boldsymbol R$ 描述了旋转本身，因此称为旋转矩阵（Rotation Matrix）

同时，该矩阵各分量是两个坐标系基的内积，基向量长度为1，所以实际上是各基向量夹角的余弦值，所以这个矩阵也叫方向余弦矩阵（Direction Cosine Matrix）。

特别的性质：事实上，它是一个行列式为1的正交矩阵；反之，行列式为1的正交矩阵也是一个旋转矩阵

所以可将$n$维旋转矩阵的集合定义如下：

$$\mathrm{SO}(n)=\left\{\boldsymbol{R} \in \mathbb{R}^{n \times n} \mid \boldsymbol{R} \boldsymbol{R}^{\mathrm{T}}=\boldsymbol{I}, \operatorname{det}(\boldsymbol{R})=1\right\}$$

$\mathrm{SO}(n)$是特殊正交群（Special Orthogonal Group），这个集合由n维空间的旋转矩阵组成，特别的，$\mathrm{SO}(3)$就是指三维空间的旋转。

旋转矩阵为正交矩阵，它的逆（即转置）描述了一个相反的旋转，有：

$$\boldsymbol{a}^{\prime}=\boldsymbol{R}^{-1} \boldsymbol{a}=\boldsymbol{R}^{\mathrm{T}} \boldsymbol{a}$$

$\boldsymbol{R}^{\mathrm{T}}$刻画了一个相反的旋转

考虑平移，世界坐标系中的向量 $\boldsymbol{a}$，经过一次旋转（用 $\boldsymbol R$ 描述）和一次平移 $t$ 后，得到了$\boldsymbol{a}^{\prime}$，把旋转平移合在一起，有：

$$\boldsymbol a^{\prime}=\boldsymbol R \boldsymbol a+\boldsymbol t$$

$\boldsymbol t$ 称为平移向量

在实际中，定义坐标系1、坐标系2，那么向量 $\boldsymbol a$ 在两个坐标系下的坐标为 $\boldsymbol a_1, \boldsymbol a_2$，它们之间的关系为：

$$\boldsymbol a_1=\boldsymbol R_{12} \boldsymbol a_2+\boldsymbol t_{12}$$

这里 $\boldsymbol R_{12}$ 是**指把坐标系2的向量变换到坐标系1中**，下标是**从右读到左**的

平移 $\boldsymbol t_{12}$ 对应的是坐标系1原点指向坐标系2原点的向量，**在坐标系1下取的坐标**

变换矩阵与齐次矩阵

变换关系不是一个线性关系，假设进行了两次变换：$\boldsymbol R_{1}, \boldsymbol t_{1}$ 和 $\boldsymbol R_{2}, \boldsymbol t_{2}$：

$$\boldsymbol{b}=\boldsymbol{R}_{1} \boldsymbol a+\boldsymbol t_{1}, \quad \boldsymbol c=\boldsymbol{R}_{2} \boldsymbol b+\boldsymbol t_{2}$$

那么，从 $a$ 到 $c$ 的变换为：

$$\boldsymbol{c}=\boldsymbol{R}_{2}\left(\boldsymbol{R}_{1} \boldsymbol{a}+\boldsymbol{t}_{1}\right)+\boldsymbol{t}_{2}$$

在变换多次之后会显得复杂，因此引入齐次坐标和变换矩阵，重写 $\boldsymbol a^{\prime}=\boldsymbol R \boldsymbol a+\boldsymbol t$：

$$\left[\begin{array}{c} \boldsymbol{a}^{\prime} \\ 1 \end{array}\right]=\left[\begin{array}{ll} \boldsymbol{R} & \boldsymbol{t} \\ \mathbf{0}^{\mathrm{T}} & 1 \end{array}\right]\left[\begin{array}{l} \boldsymbol{a} \\ 1 \end{array}\right] \stackrel{\text { def }}{=} \boldsymbol{T}\left[\begin{array}{l} \boldsymbol{a} \\ 1 \end{array}\right]$$

在一个三维向量的末尾添加1，将其变成四维向量，称为齐次坐标，对于这个四维向量，可以把旋转和平移写在一个矩阵里，使得整个关系变成线性关系

矩阵 $\boldsymbol T$ 称为变换矩阵（Transform Matrix）

用 $\boldsymbol {\tilde a}$ 表示 $\boldsymbol a$ 的齐次坐标，依靠齐次坐标和变换矩阵，两次变换的叠加可以表示为：

$$\boldsymbol {\tilde{b}}=\boldsymbol T_{1} \boldsymbol {\tilde{a}}, \boldsymbol { \tilde{c}}=\boldsymbol T_{2} \boldsymbol {\tilde{b}} \quad \Rightarrow \boldsymbol {\tilde{c}}=\boldsymbol T_{2} \boldsymbol T_{1} \boldsymbol {\tilde{a}}$$

但是区分齐次和非齐次坐标的符号令人厌烦，因为此处只需要在向量末尾添加1或去掉1，在不引起歧义的情况下，直接写成 $\boldsymbol b = \boldsymbol {Ta}$ 的样子，默认进行了齐次坐标的变换。

关于变换矩阵 $\boldsymbol T$，具有比较特别的结构：左上角为旋转矩阵，右侧为平移向量，左下角为 $\boldsymbol 0$ 向量，右下角为1，这种矩阵又称为特殊欧式群（Special Euclidean Group）：

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

与 $\mathrm{SO}(3)$一样，求解该矩阵的逆表示一个反向的变换：

$$\boldsymbol{T}^{-1}=\left[\begin{array}{cc} \boldsymbol{R}^{\mathrm{T}} & -\boldsymbol{R}^{\mathrm{T}} \boldsymbol{t} \\ \mathbf{0}^{\mathrm{T}} & 1 \end{array}\right]$$

同样，用 $T_{12}$ 表示从2到1的变换

回顾：

1. 介绍了向量及其坐标表示
2. 向量间的运算
3. 坐标系之间的运动欧式变换描述，由旋转平移组成，旋转由旋转矩阵 $\mathrm{SO}(3)$描述，平移由一个 $\mathbb{R}^{3}$ 向量描述
4. 变换矩阵 $\mathrm{SE}(3)$：平移和旋转放在一个矩阵中

旋转向量和欧拉角

旋转向量

矩阵表示方式的缺点：

1. $\mathrm{SO}(3)$的旋转矩阵有9个量，但一次旋转只有3个自由度，这种表示方式是冗余的；同理，变换矩阵用16个量表达了6个自由度的变换
2. 旋转矩阵自身带有约束，必须是正交矩阵，且行列式为1，变换矩阵也是如此，当想估计或优化一个旋转矩阵或变换矩阵时，这些约束使求解困难

事实上，任意旋转都可以用一个旋转轴和一个旋转角来刻画

使用一个向量，其方向与旋转轴一致，长度等于旋转角，这种向量称为旋转向量（或轴角/角轴，Axis-Angle），只需一个三维向量即可描述旋转；同样对于变换矩阵，使用一个旋转向量和一个平移向量即可表达一次变换，变量维数正好是六维。

考虑某个用 $\boldsymbol R$ 表示的旋转，用旋转向量描述，假设旋转轴为一个单位长度的向量 $\boldsymbol n$，角度为 $\theta$，那么向量 $\theta \boldsymbol n$ 也可以描述这个旋转。

从旋转向量到旋转矩阵的转换过程由罗德里格斯公式（Rodrigues’s Formula） 表明：

$$\boldsymbol{R}=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{n} \boldsymbol{n}^{\mathrm{T}}+\sin \theta \boldsymbol{n}^{\wedge} .$$

符号^是向量到反对称矩阵的转换符

反之，可以计算从一个旋转矩阵到旋转向量的转换，对于转角 $\theta$，取两边的迹（trace）（矩阵的对角线元素之和），有：

$$\begin{aligned} \operatorname{tr}(\boldsymbol{R}) &=\cos \theta \operatorname{tr}(\boldsymbol{I})+(1-\cos \theta) \operatorname{tr}\left(\boldsymbol{n} \boldsymbol{n}^{\mathrm{T}}\right)+\sin \theta \operatorname{tr}\left(\boldsymbol{n}^{\wedge}\right) \\ &=3 \cos \theta+(1-\cos \theta) \\ &=1+2 \cos \theta \end{aligned}$$

因此：

$$\theta=\arccos \frac{\operatorname{tr}(\boldsymbol{R})-1}{2}$$

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

$$\boldsymbol{R} \boldsymbol n=\boldsymbol{n}$$

因此，转轴 $\boldsymbol n$ 是矩阵 $\boldsymbol R$ 特征值1对应的特征向量。求解此方程，再归一化，就得到旋转轴。

欧拉角

旋转矩阵和旋转向量对人类来说都是不直观的

欧拉角提供了一种非常直观的方式来描述旋转——使用3个分离的转角，把一个旋转分解成3次绕不同轴的旋转

欧拉角中常用的一种，是用“偏航—俯仰—滚转”（yaw-pitch-roll）3个角度来描述一个旋转，等价于 $ZYX$ 轴的旋转。

以 $ZYX$ 为例，假设一个刚体的前方（朝向我们的方向）为 $X$ 轴，右侧为 $Y$ 轴，上方为 $Z$ 轴，那么 $ZYX$ 转角相当于把任意旋转分解成以下3个轴上的转角：

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

此时可以用$[r, p, y]^{\mathrm{T}}$三维向量描述任意旋转（rpy角）

欧拉角的缺点：

万向锁问题（Gimbal Lock）：在俯仰角为$\pm 90^{\circ}$时，第一次旋转与第三次旋转将使用同一个轴，使得系统丢失了一个自由度（由3次旋转变成了2次旋转）。这被称为奇异性问题。

理论上可以证明，只要想用3个实数来表达三维旋转，都会不可避免地碰到奇异性问题。由于这种原理，欧拉角不适用于插值和迭代，往往只用于人机交互中。

四元数

四元数的定义

旋转矩阵用9个量描述3个自由度的旋转，具有冗余性；

欧拉角和旋转向量是紧凑的，但具有奇异性。

事实上，找不到不带奇异性的三维向量描述方式。类似于两个坐标表示地球表面（经纬度），将必定存在奇异性（维度为$\pm 90^{\circ}$时经度无意义）。

四元数（Quaternion）既是紧凑的，也没有奇异性，缺点不够直观，运算稍复杂

类比复数，将复平面的向量旋转 $\theta$ 角时，可以给这个复向量乘以 $\mathrm{e}^{\mathrm{i} \theta}$。这是极坐标表示的复数，用欧拉公式写成普通公式：

$$\mathrm{e}^{\mathrm{i} \theta}=\cos \theta+\mathrm{i} \sin \theta$$

这正是一个单位长度的复数

在二位情况下，旋转可以由单位复数描述，类似三维旋转可以由单位四元数描述

一个四元数 $\boldsymbol q$ 拥有一个实部和三个虚部：

$$\boldsymbol{q}=q_{0}+q_{1} \mathrm{i}+\mathrm{q}_{2} \mathrm{j}+\mathrm{q}_{3} \mathrm{k}$$

其中，i，j，k为四元数的三个虚部，满足以下公式：

$$\left\{\begin{array}{l} i^{2}=j^{2}=k^{2}=-1 \\ i j=k, j i=-k \\ j k=i, k j=-i \\ k i=j, i k=-j \end{array}\right.$$

有时，也用一个标量和一个向量表达四元数：

$$\boldsymbol{q}=[s, \boldsymbol{v}]^{\mathrm{T}}, \quad s=q_{0} \in \mathbb{R}, \quad \boldsymbol{v}=\left[q_{1}, q_{2}, q_{3}\right]^{\mathrm{T}} \in \mathbb{R}^{3} .$$

$s$ 称为四元数的实部，$v$ 称为虚部，如果虚部为 $0$，则称为实四元数；反之若实部为0，则成为虚四元数。

可以用单位四元数表示三维空间中任意一个旋转

四元数的运算

常见的有：四则运算、共轭、求逆、数乘

现有两个四元数 $\boldsymbol q_a$，$\boldsymbol q_b$，它们的向量表示为$\left[s_{a}, \boldsymbol v_{a}\right]^{\mathrm{T}},\left[s_{b}, \boldsymbol v_{b}\right]^{\mathrm{T}}$，或者原始四元数表示为：

$$\boldsymbol{q}_{a}=s_{a}+x_{a} \mathrm{i}+\mathrm{y}_{\mathrm{a}} \mathrm{j}+\mathrm{z}_{\mathrm{a}} \mathrm{k}, \quad \boldsymbol{q}_{\mathrm{b}}=\mathrm{s}_{\mathrm{b}}+\mathrm{x}_{\mathrm{b}} \mathrm{i}+\mathrm{y}_{\mathrm{b}} \mathrm{j}+\mathrm{z}_{\mathrm{b}} \mathrm{k}$$

其运算可表示如下：

1. 加减法

   四元数 $\boldsymbol q_a$，$\boldsymbol q_b$ 的加减运算为：

   $$\boldsymbol{q}_{a} \pm \boldsymbol{q}_{b}=\left[s_{a} \pm s_{b}, \boldsymbol{v}_{a} \pm \boldsymbol{v}_{b}\right]^{\mathrm{T}}$$

2. 乘法

   乘法是把 $\boldsymbol q_a$ 的每一项与 $\boldsymbol q_b$ 的每一项相乘，最后相加，整理得：

   $$\begin{aligned} \boldsymbol{q}_{a} \boldsymbol{q}_{b}=& s_{a} s_{b}-x_{a} x_{b}-y_{a} y_{b}-z_{a} z_{b} \\ &+\left(s_{a} x_{b}+x_{a} s_{b}+y_{a} z_{b}-z_{a} y_{b}\right) \mathrm{i} \\ &+\left(s_{a} y_{b}-x_{a} z_{b}+y_{a} s_{b}+z_{a} x_{b}\right) \mathrm{j} \\ &+\left(s_{a} z_{b}+x_{a} y_{b}-y_{a} x_{b}+z_{a} s_{b}\right) \mathrm{k} \end{aligned}$$

   如果写成向量形式并利用内外积运算，表达式为：

   $$\boldsymbol{q}_{a} \boldsymbol{q}_{b}=\left[s_{a} s_{b}-\boldsymbol{v}_{a}^{\mathrm{T}} \boldsymbol{v}_{b}, s_{a} \boldsymbol{v}_{b}+s_{b} \boldsymbol{v}_{a}+\boldsymbol{v}_{a} \times \boldsymbol{v}_{b}\right]^{\mathrm{T}}$$

   两个实四元数乘积仍是实的，与复数一致

   由于最后一项外积的存在，四元数乘法是不可交换的，除非 $\boldsymbol{v}_{a}$ 和 $\boldsymbol{v}_{b}$ 在 $\mathbb{R}^{3}$ 中共线，此时外积项为0

3. 模长

   四元数的模长定义为：

   $$\left\|\boldsymbol{q}_{a}\right\|=\sqrt{s_{a}^{2}+x_{a}^{2}+y_{a}^{2}+z_{a}^{2}}$$

   两个四元数乘积的模即模的乘积，这使得单位四元数相乘后仍是单位四元数

   $$\left\|\boldsymbol{q}_{a} \boldsymbol{q}_{b}\right\|=\left\|\boldsymbol{q}_{a}\right\|\left\|\boldsymbol{q}_{b}\right\| .$$

4. 共轭

   四元数的共轭是把虚部取成相反数：

   $$\boldsymbol{q}_{a}^{*}=s_{a}-x_{a} \mathrm{i}-\mathrm{y}_{\mathrm{a}} \mathrm{j}-\mathrm{z}_{\mathrm{a}} \mathrm{k}=\left[s_{\mathrm{a}},-\boldsymbol {v}_{\mathrm{a}}\right]^{\mathrm{T}} \text {. }$$

   四元数共轭与其本身相乘，会得到一个实四元数，其实部为模长的平方：

   $$\boldsymbol{q}^{*} \boldsymbol{q}=\boldsymbol{q} \boldsymbol{q}^{*}=\left[s_{a}^{2}+\boldsymbol{v}^{\mathrm{T}} \boldsymbol{v}, \mathbf{0}\right]^{\mathrm{T}} \text {. }$$

5. 逆

   一个四元数的逆为：

   $$\boldsymbol{q}^{-1}=\boldsymbol{q}^{*} /\|\boldsymbol{q}\|^{2}$$

   按此定义，四元数和自己的逆的乘积为实四元数 $1$：

   $$\boldsymbol q \boldsymbol q^{-1}=\boldsymbol q^{-1} \boldsymbol q=\boldsymbol 1$$

   如果 $\boldsymbol{q}$ 为单位四元数，其逆和共轭就是同一个量

   乘积的逆具有和矩阵相似的性质：

   $$\left(\boldsymbol{q}_{a} \boldsymbol{q}_{b}\right)^{-1}=\boldsymbol{q}_{b}^{-1} \boldsymbol{q}_{a}^{-1}$$

6. 数乘

   和向量相似，四元数可以和数相乘：

   $$\mathrm{k} \boldsymbol{q}=[\mathrm{k} s, \mathrm{k} \boldsymbol{v}]^{\mathrm{T}}$$

用四元数表示旋转

假设有一个空间三维点 $\boldsymbol{p}=[x, y, z] \in \mathbb{R}^{3}$，以及一个由单位四元数 $\boldsymbol q$ 指定的旋转，三维点 $\boldsymbol p$ 经过旋转后变为 $\boldsymbol p^{\prime}$

用矩阵描述，有 $\boldsymbol p^{\prime}=\boldsymbol {Rp}$

用四元数描述：

首先把三维空间点用一个虚四元数来描述：

$$\boldsymbol p=[0, x, y, z]^{\mathrm{T}}=[0, \boldsymbol{v}]^{\mathrm{T}} .$$

相当于把四元数的3个虚部与空间中的3个轴相对应

旋转后的 $\boldsymbol p^{\prime}$ 可表示为：

$$\boldsymbol{p}^{\prime}=\boldsymbol{q} \boldsymbol p \boldsymbol{q}^{-1}$$

最后把 $\boldsymbol p^{\prime}$ 的虚部取出，即得旋转之后点的坐标

四元数到其他旋转表示的转换

考虑四元数与旋转向量、旋转矩阵之间的转换关系

四元数乘法也可以写成矩阵的乘法

设 $\boldsymbol{q}=[s, \boldsymbol{v}]^{\mathrm{T}}$，定义如下符号 $^{+}$ 和 $^{\oplus}$ 为：

$$\boldsymbol{q}^{+}=\left[\begin{array}{cc} s & -v^{\mathrm{T}} \\ \boldsymbol{v} & s \boldsymbol{I}+\boldsymbol{v}^{\wedge} \end{array}\right], \quad \boldsymbol{q}^{\oplus}=\left[\begin{array}{cc} s & -\boldsymbol{v}^{\mathrm{T}} \\ \boldsymbol{v} & s \boldsymbol{I}-\boldsymbol{v}^{\wedge} \end{array}\right]$$

这两个符号将四元数映射成为一个 $4×4$ 的矩阵，于是四元数乘法可以写成矩阵形式：

$$\boldsymbol{q}_{1}^{+} \boldsymbol{q}_{2}=\left[\begin{array}{cc} s_{1} & -\boldsymbol{v}_{1}^{\mathrm{T}} \\ \boldsymbol{v}_{1} & s_{1} \boldsymbol{I}+\boldsymbol{v}_{1}^{\wedge} \end{array}\right]\left[\begin{array}{l} s_{2} \\ \boldsymbol{v}_{2} \end{array}\right]=\left[\begin{array}{c} -\boldsymbol{v}_{1}^{\mathrm{T}} \boldsymbol{v}_{2}+s_{1} s_{2} \\ s_{1} \boldsymbol{v}_{2}+s_{2} \boldsymbol{v}_{1}+\boldsymbol{v}_{1}^{\wedge} \boldsymbol{v}_{2} \end{array}\right]=\boldsymbol{q}_{1} \boldsymbol{q}_{2}$$

同理亦可证：

$$\boldsymbol{q}_{1} \boldsymbol{q}_{2}=\boldsymbol{q}_{1}^{+} \boldsymbol{q}_{2}=\boldsymbol{q}_{2}^{\oplus} \boldsymbol{q}_{1}$$

用四元数对空间点进行旋转，有：

$$\begin{aligned} \boldsymbol{p}^{\prime} &=\boldsymbol{q} \boldsymbol{p} \boldsymbol{q}^{-1}=\boldsymbol{q}^{+} \boldsymbol{p} \boldsymbol{q}^{-1} \\ &=\boldsymbol{q}^{+} \boldsymbol{q}^{-1^{\oplus}} \boldsymbol{p} . \end{aligned}$$

代入两个符号对应的矩阵，得

$$\boldsymbol{q}^{+}\left(\boldsymbol{q}^{-1}\right)^{\oplus}=\left[\begin{array}{cc} s & -\boldsymbol{v}^{\mathrm{T}} \\ \boldsymbol{v} & s \boldsymbol{I}+\boldsymbol{v}^{\wedge} \end{array}\right]\left[\begin{array}{cc} s & \boldsymbol{v}^{\mathrm{T}} \\ -\boldsymbol{v} & s \boldsymbol{I}+\boldsymbol{v}^{\wedge} \end{array}\right]=\left[\begin{array}{cc} 1 & 0 \\ 0^{\mathrm{T}} & \boldsymbol{v} \boldsymbol{v}^{\mathrm{T}}+s^{2} \boldsymbol{I}+2 s \boldsymbol{v}^{\wedge}+\left(\boldsymbol{v}^{\wedge}\right)^{2} \end{array}\right]$$

因为 $\boldsymbol{p}^{\prime}$ 和 $\boldsymbol p$ 都是虚四元数，所以事实上该矩阵的右下角即给出了从四元数到旋转矩阵的变换关系：

$$\boldsymbol{R}=\boldsymbol{v} \boldsymbol{v}^{\mathrm{T}}+s^{2} \boldsymbol{I}+2 s \boldsymbol{v}^{\wedge}+\left(\boldsymbol{v}^{\wedge}\right)^{2}$$

为了得到四元数到旋转向量的转换公式，对上式两侧求迹，得：

$$\begin{aligned} \operatorname{tr}(\boldsymbol{R}) &=\operatorname{tr}\left(\boldsymbol{v} \boldsymbol{v}^{\top}+3 s^{2}+2 s \cdot 0+\operatorname{tr}\left(\left(\boldsymbol{v}^{\wedge}\right)^{2}\right)\right.\\ &=v_{1}^{2}+v_{2}^{2}+v_{3}^{2}+3 s^{2}-2\left(v_{1}^{2}+v_{2}^{2}+v_{3}^{2}\right) \\ &=\left(1-s^{2}\right)+3 s^{2}-2\left(1-s^{2}\right) \\ &=4 s^{2}-1 . \end{aligned}$$

由之前旋转矩阵到旋转向量的转换，得：

$$\begin{aligned} \theta &=\arccos \frac{\operatorname{tr}(\boldsymbol{R})-1}{2} \\ &=\arccos \left(2 s^{2}-1\right) . \end{aligned}$$

即

$$\cos \theta=2 s^{2}-1=2 \cos ^{2} \frac{\theta}{2}-1$$

所以：

$$\theta=2\arccos s$$

至于旋转轴，在 $\begin{aligned} \boldsymbol{p}^{\prime} &=\boldsymbol{q} \boldsymbol{p} \boldsymbol{q}^{-1}=\boldsymbol{q}^{+} \boldsymbol{p} \boldsymbol{q}^{-1} =\boldsymbol{q}^{+} \boldsymbol{q}^{-1^{\oplus}} \boldsymbol{p} . \end{aligned}$ 中用 $\boldsymbol q$ 的虚部代替 $\boldsymbol p$，易知 $\boldsymbol q$ 的虚部组成的向量在旋转时是不动的，即构成旋转轴。于是只要将他除以它的模长，即得。

四元数到旋转向量的转换公式如下：

$$\left\{\begin{array}{l} \theta=2 \arccos q_{0} \\ {\left[n_{x}, n_{y}, n_{z}\right]^{\mathrm{T}}=\left[q_{1}, q_{2}, q_{3}\right]^{\mathrm{T}} / \sin \frac{\theta}{2}} \end{array}\right.$$

相似、仿射、射影变换

除了欧式变换，3D空间还存在其他几种变换方式

欧式变换保持了向量的长度和夹角，相当于把一个刚体原封不动地进行了移动或旋转，不改变自身的样子

其他几种变换则会改变它的外形

1. 相似变换

   相似变换比欧式变换多了一个自由度，允许物体进行均匀缩放，其矩阵表示为：

   $$\boldsymbol{T}_{S}=\left[\begin{array}{ll} s \boldsymbol{R} & \boldsymbol t \\ \boldsymbol 0^{\mathrm{T}} & 1 \end{array}\right]$$

   旋转部分多了一个缩放因子 $s$，表示在对向量旋转之后，可以在 $x,y,z$ 三个坐标上进行均匀缩放。

   由于含有缩放，相似变换不再保持图形的面积不变。

   三维相似变换的集合也叫作相似变换群，记作 $\mathrm {Sim}(3)$。

2. 仿射变换

   仿射变换的矩阵形式如下：

   $$\boldsymbol T_{A}=\left[\begin{array}{ll} \boldsymbol A & \boldsymbol t \\ \boldsymbol 0^{\mathrm{T}} & 1 \end{array}\right]$$

   与欧氏变换不同的是，仿射变换只要求 $\boldsymbol A$ 是一个可逆矩阵，而不必是正交矩阵。

   仿射变换也叫正交投影

   经过仿射变换之后，立方体就不再是方的了，但是各个面仍然是平行四边形。

3. 射影变换

   射影变换是最一般的变换，矩阵形式为：

   $$\boldsymbol{T}_{P}=\left[\begin{array}{ll} \boldsymbol{A} & \boldsymbol{t} \\ \boldsymbol{a}^{\mathrm{T}} & v \end{array}\right]$$

   左上角为可逆矩阵 $\boldsymbol A$，右上角为平移 $\boldsymbol t$，左下角为缩放 $\boldsymbol a^T$

   由于采用齐次坐标，当 $v \neq 0$ 时，整个矩阵除以 $v$ 得到一个右下角为1的矩阵；否则得到右下角为0的矩阵。

   2D的射影变换一共有8个自由度，3D则有15个自由度

   射影变换是形式最为一般的

   从真实世界到相机世界的变换可以看成一个射影变换，例如一个原本方形的地板砖，在照片的样子：不再是方形的，由于近大远小的关系，不是一个平行四边形而是不规则的四边形。

常见变换的性质比较：

注：不变性质中，从上到下是包含关系，例如欧式变换除了保体积也有保平行、相交等性质

第4讲 李群和李代数

第3讲介绍了三维世界中刚体运动的描述方式，包括旋转矩阵、旋转向量、欧拉角、四元数等若干种方式。

重点介绍了旋转的表示，但是在SLAM中，除了表示，还要对它们进行估计和优化。

因为在SLAM中位姿是未知的，需要解决形如“什么样的相机位姿最符合当前观测数据”这样的问题。

一种典型的方式是构建成一个优化问题，求解最优的 $\boldsymbol R,\boldsymbol t$，使得误差最小化。

如前所言，旋转矩阵自身是带有约束的（正交且行列式为1）。它们作为优化变量时，会引入额外的约束，使优化变得困难。

通过李群——李代数间的转换关系，把位姿估计变成无约束的优化问题，简化求解方式。

李群和李代数基础

三维矩阵构成了特殊正交群 $\mathrm{SO}(3)$，而变换矩阵构成了特殊欧式群 $\mathrm{SE}(3)$：

$$\begin{array}{l} \mathrm{SO}(3)=\left\{\boldsymbol{R} \in \mathbb{R}^{3 \times 3} \mid \boldsymbol{R} \boldsymbol{R}^{\mathrm{T}}=\boldsymbol{I}, \operatorname{det}(\boldsymbol{R})=1\right\}, \\ \mathrm{SE}(3)=\left\{\boldsymbol{T}=\left[\begin{array}{ll} \boldsymbol{R} & \boldsymbol t \\ \mathbf{0}^{\mathrm{T}} & 1 \end{array}\right] \in \mathbb{R}^{4 \times 4} \mid \boldsymbol{R} \in \mathrm{SO}(3), \boldsymbol{t} \in \mathbb{R}^{3}\right\} . \end{array}$$

旋转矩阵和变换矩阵对加法是不封闭的，i.e.，对于两个任意旋转矩阵 $\boldsymbol R_1$，$\boldsymbol R_2$，按照矩阵加法的定义，和不再是一个旋转矩阵：

$$\boldsymbol{R}_{1}+\boldsymbol{R}_{2} \notin \mathrm{SO}(3), \quad \boldsymbol{T}_{1}+\boldsymbol{T}_{2} \notin \mathrm{SE}(3)$$

相对的，只有一种较好的运算：乘法，$\mathrm{SO}(3)$ 和 $\mathrm{SE}(3)$ 关于乘法是封闭的：

$$\boldsymbol{R}_{1}\boldsymbol{R}_{2} \in \mathrm{SO}(3), \quad \boldsymbol{T}_{1}\boldsymbol{T}_{2} \in \mathrm{SE}(3)$$

同时，也可以对任何一个旋转或变换矩阵（在乘法的意义上）求逆。

乘法对应着旋转或变换的复合，两个旋转矩阵相乘表示做了两次旋转。

对于这种只有一个（良好的）运算的集合，我们称之为群。

群

群（Group）是一种集合加上一种运算的代数结构

把集合记作 $A$，运算记作 $·$，那么群可以记作 $G=(A,·)$，满足以下几个条件：

1. 封闭性：$\forall a_{1}, a_{2} \in A, \quad a_{1} \cdot a_{2} \in A$
2. 结合律：$\forall a_{1}, a_{2}, a_{3} \in A, \quad\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, \quad \text { s.t. } \quad \forall a \in A, \quad a_{0} \cdot a=a \cdot a_{0}=a$
4. 逆：$\forall a \in A, \quad \exists a^{-1} \in A, \quad \text { s.t. } \quad a \cdot a^{-1}=a_{0}$

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

其他常见的群包括：整数的加法 $(\mathbb{Z},+)$，去掉0后的有理数的乘法（幺元为1）$(\mathbb{Q} \backslash 0, \cdot)$

矩阵中常见的群有：

• 一般线性群 $\mathrm{GL}(n)$：指 $n \times n$ 的可逆矩阵，对矩阵乘法成群
• 特殊正交群 $\mathrm{SO}(n)$：所谓的旋转矩阵群，其中 $\mathrm{SO}(2)$ 和 $\mathrm{SO}(3)$ 最常见
• 特殊欧式群 $\mathrm{SE}(n)$：$n$维欧式变换，如 $\mathrm{SE}(2)$ 和 $\mathrm{SE}(3)$

群结构保证了在群上的运算具有良好的性质

李群是指具有连续（光滑）性质的群，像整数群 $\mathbb{Z}$ 离散的群没有连续性质，不是李群，而 $\mathrm{SO}(n)$ 和 $\mathrm{SE}(n)$ 在实数空间上是连续的

李代数的引出

考虑任意旋转矩阵 $\boldsymbol R$，满足：

$$\boldsymbol R\boldsymbol R^T=\boldsymbol I$$

现在说 $\boldsymbol R$ 是某个相机的旋转，会随时间连续地变化，即为时间的函数：$\boldsymbol R(t)$。

由于仍是旋转矩阵，有：

$$\boldsymbol R(t)\boldsymbol R(t)^T=\boldsymbol I$$

在等式两边对时间求导，得到：

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

整理得：

$$\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{\mathrm{T}}=-\left(\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{\mathrm{T}}\right)^{\mathrm{T}}$$

可以看出，$\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{\mathrm{T}}$ 是一个反对称矩阵，之前引入了^符号，将一个向量变成反对称矩阵，同理对于任意反对称矩阵，有唯一与之对应的向量，符号用 $^\vee$ 表示：

$$\boldsymbol{a}^{\wedge}=\boldsymbol{A}=\left[\begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0 \end{array}\right], \quad \boldsymbol{A}^{\vee}=\boldsymbol{a}$$

由于 $\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{\mathrm{T}}$ 是一个反对称矩阵，可以找到一个三维向量 $\boldsymbol{\phi}(t) \in \mathbb R^3$ 与之对应：

$$\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{\mathrm{T}}=\boldsymbol{\phi}(t)^{\wedge}$$

等式两边右乘 $\boldsymbol{R}(t)$，由于 $\boldsymbol{R}$ 为正交矩阵，有：

$$\dot{\boldsymbol{R}}(t)=\boldsymbol \phi(t)^{\wedge} \boldsymbol{R}(t)=\left[\begin{array}{ccc} 0 & -\phi_{3} & \phi_{2} \\ \phi_{3} & 0 & -\phi_{1} \\ -\phi_{2} & \phi_{1} & 0 \end{array}\right] \boldsymbol{R}(t)$$

可以看到，每对旋转矩阵求一次导数，只需左乘一个 $\phi(t)^{\wedge}$ 矩阵即可

$t_0=0$ 时，设此时旋转矩阵为 $\boldsymbol R(0)=\boldsymbol I$

把 $\boldsymbol R(t)$ 在 $t=0$ 附近进行一阶泰勒展开：

$$\begin{aligned} \boldsymbol{R}(t) & \approx \boldsymbol{R}\left(t_{0}\right)+\dot{\boldsymbol{R}}\left(t_{0}\right)\left(t-t_{0}\right) \\ &=\boldsymbol{I}+\boldsymbol{\phi}\left(t_{0}\right)^{\wedge}(t) \end{aligned}$$

$\boldsymbol \phi$ 反映了 $\boldsymbol R$ 的导数性质，故称它在 $\mathrm{SO}(3)$ 原点附近的正切空间（Tangent Space）上

同时在 $t_0$ 附近，设 $\boldsymbol \phi$ 保持为常数 $\boldsymbol \phi (t_0)=\boldsymbol \phi_0$，那么有：

$$\dot{\boldsymbol{R}}(t)=\boldsymbol \phi(t_0)^{\wedge} \boldsymbol{R}(t)=\boldsymbol \phi_0^{\wedge}\boldsymbol R(t)$$

上式是关于 $\boldsymbol R$ 的微分方程，有初始值 $\boldsymbol R(0)=\boldsymbol I$，解得：

$$\boldsymbol{R}(t)=\exp \left(\boldsymbol{\phi}_{0}^{\wedge} t\right)$$

在 $t=0$ 附近，旋转矩阵可以由 $\exp \left(\boldsymbol{\phi}_{0}^{\wedge} t\right)$ 计算出来

旋转矩阵 $\boldsymbol R$ 与另一个反对称矩阵 $\boldsymbol{\phi}_{0}^{\wedge} t$ 通过指数关系发生联系

1. 给定某时刻的 $\boldsymbol R$，能求得一个 $\boldsymbol \phi$，描述了 $\boldsymbol R$ 在局部的导数关系。

   $\boldsymbol \phi$ 正是对应到 $\mathrm{SO}(3)$ 上的李代数 $\mathfrak{s o}(3)$；

2. 给定某个向量$\mathrm{SO}(3)$时，矩阵指数 $\exp \left(\boldsymbol{\phi}_{0}^{\wedge} \right)$ 如何计算？反之，给定 $\boldsymbol R$ 时，能否有相反的运算来计算 $\boldsymbol \phi$ ？

事实上，这正是李群与李代数间的指数/对数映射。

李代数的定义

每个李群都有与之对应的李代数

李代数描述了李群的局部性质，准确地说，是单位元附近的正切空间。

一般的李代数的定义如下:

李代数由一个集合 $\mathbb V$、一个数域 $\mathbb F$ 和一个二元运算 $[,]$ 组成

如果它们满足以下几条性质，则称 $(\mathbb V,\mathbb F,[,])$ 为一个李代数，记作 $\mathfrak g$。

1. 封闭性 $\forall \boldsymbol{X}, \boldsymbol{Y} \in \mathbb{V},[\boldsymbol{X}, \boldsymbol{Y}] \in \mathbb{V}$
2. 双线性 $\forall \boldsymbol{X}, \boldsymbol{Y}, \boldsymbol{Z} \in \mathbb{V}, a, b \in \mathbb{F}$ ，有 ${[a \boldsymbol{X}+b \boldsymbol{Y}, \boldsymbol{Z}]=a[\boldsymbol{X}, \boldsymbol{Z}]+b[\boldsymbol{Y}, \boldsymbol{Z}], \quad[\boldsymbol{Z}, a \boldsymbol{X}+b \boldsymbol{Y}]=a[\boldsymbol{Z}, \boldsymbol{X}]+b[\boldsymbol{Z}, \boldsymbol{Y}] .}$
3. 自反性（自己与自己的运算为零） $\forall \boldsymbol{X} \in \mathbb{V},[\boldsymbol{X}, \boldsymbol{X}]=\mathbf{0}$
4. 雅可比等价 $\forall \boldsymbol{X}, \boldsymbol{Y}, \boldsymbol{Z} \in \mathbb{V},[\boldsymbol{X},[\boldsymbol{Y}, \boldsymbol{Z}]]+[\boldsymbol{Z},[\boldsymbol{X}, \boldsymbol{Y}]]+[\boldsymbol{Y},[\boldsymbol{Z}, \boldsymbol{X}]]=\mathbf{0}$

其中二元运算被称为李括号

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

例子：三维向量 $\mathbb R$ 上定义的叉积 $\times$ 是一种李括号，因此 $\mathfrak g=(\mathbb R^3,\mathbb R,\times)$ 构成了一个李代数

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

之前提到的 $\boldsymbol \phi$，事实上是一种李代数

$\mathrm{SO}(3)$ 对应的李代数是定义在 $\mathbb R^3$ 上的向量，记作 $\boldsymbol \phi$

每个 $\boldsymbol \phi$ 可以生成一个反对称矩阵：

$$\boldsymbol \Phi=\boldsymbol \phi^{\wedge}=\left[\begin{array}{ccc} 0 & -\phi_{3} & \phi_{2} \\ \phi_{3} & 0 & -\phi_{1} \\ -\phi_{2} & \phi_{1} & 0 \end{array}\right] \in \mathbb{R}^{3 \times 3}$$

在此定义下，两个向量 $\boldsymbol \phi_1,\boldsymbol \phi_2$ 的李括号为：

$$\left[\boldsymbol{\phi}_{1}, \boldsymbol{\phi}_{2}\right]=\left(\boldsymbol{\Phi}_{1} \boldsymbol{\Phi}_{2}-\boldsymbol{\Phi}_{2} \boldsymbol{\Phi}_{1}\right)^{\vee} .$$

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

$$\mathfrak{s o}(3)=\left\{\phi \in \mathbb{R}^{3}, \boldsymbol{\Phi}=\phi^{\wedge} \in \mathbb{R}^{3 \times 3}\right\}$$

$\mathfrak {so}(3)$ 是一个由三维向量组成的集合，每个向量对应一个反对称矩阵，可以用于表达旋转矩阵的导数

$\mathfrak {so}(3)$ 与 $\mathrm{SO}(3)$ 的关系由指数映射给定：

$$\boldsymbol{R}=\exp \left(\phi^{\wedge}\right)$$

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

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

$$\mathfrak{s e}(3)=\left\{\boldsymbol \xi=\left[\begin{array}{l} \boldsymbol \rho \\ \boldsymbol \phi \end{array}\right] \in \mathbb{R}^{6}, \boldsymbol \rho \in \mathbb{R}^{3}, \boldsymbol \phi \in \mathfrak{s o}(3), \boldsymbol{\xi}^{\wedge}=\left[\begin{array}{cc} \boldsymbol \phi^{\wedge} & \boldsymbol \rho \\ \boldsymbol 0^{\mathrm{T}} & 0 \end{array}\right] \in \mathbb{R}^{4 \times 4}\right\}$$

把每个 $\mathfrak {se}(3)$ 元素记作 $\boldsymbol \xi$，它是一个六维向量。

前三维为平移（但含义与变换矩阵中的平移不同）记作 $\boldsymbol \rho$；

后三维为旋转，记作 $\boldsymbol \phi$，实质上是 $\mathfrak {so}(3)$ 元素

在 $\mathfrak {se}(3)$ 中，同样使用^符号，将一个六维向量转换成四维矩阵，但这里不再表示反对称：

$$\boldsymbol \xi^{\wedge}=\left[\begin{array}{ll} \boldsymbol \phi^{\wedge} & \boldsymbol \rho \\ \boldsymbol 0^{T} & 0 \end{array}\right] \in \mathbb{R}^{4 \times 4}$$

仍使用 $^{\wedge}$ 和 $^{\vee}$ 符号指代“从向量到矩阵”和“从矩阵到向量”的关系，以保持和 $\mathfrak {so}(3)$ 上的一致性

依旧是一一对应的

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

$$\left[\boldsymbol{\xi}_{1}, \boldsymbol{\xi}_{2}\right]=\left(\boldsymbol{\xi}_{1}^{\wedge} \boldsymbol{\xi}_{2}^{\wedge}-\boldsymbol{\xi}_{2}^{\wedge} \boldsymbol{\xi}_{1}^{\wedge}\right)^{\vee}$$

指数与对数映射

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

$\exp \left(\boldsymbol \phi^{\wedge}\right)$ 是一个矩阵的指数，在李群和李代数中称为指数映射（Exponential Map）

任意矩阵的指数映射可以写成一个泰勒展开，只有在收敛的情况下会有结果，其结果仍是一个矩阵：

$$\exp (\boldsymbol{A})=\sum_{n=0}^{\infty} \frac{1}{n !} \boldsymbol{A}^{n}$$

同样地，对 $\mathfrak {so}(3)$ 中的任意元素 $\boldsymbol \phi$，按此方式定义指数映射：

$$\exp \left(\boldsymbol \phi^{\wedge}\right)=\sum_{n=0}^{\infty} \frac{1}{n !}\left(\boldsymbol \phi^{\wedge}\right)^{n}$$

由于没法直接计算（计算矩阵的无穷次幂），推导一种计算指数映射的简便方法

由于 $\boldsymbol \phi$ 是三维向量，定义其模长和方向，分别记作 $\theta$ 和 $\boldsymbol a$，于是有 $\boldsymbol \phi=\theta \boldsymbol a$，$\boldsymbol a$ 是一个长度为1的方向向量，即 $|\boldsymbol{a}|=1$

对于 $\boldsymbol a^{\wedge}$，有以下两条性质：

$$\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}=\left[\begin{array}{ccc} -a_{2}^{2}-a_{3}^{2} & a_{1} a_{2} & a_{1} a_{3} \\ a_{1} a_{2} & -a_{1}^{2}-a_{3}^{2} & a_{2} a_{3} \\ a_{1} a_{3} & a_{2} a_{3} & -a_{1}^{2}-a_{2}^{2} \end{array}\right]=\boldsymbol{a a ^ { \mathrm { T } } - \boldsymbol { I }}$$

以及

$$\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}=\boldsymbol{a}^{\wedge}\left(\boldsymbol{a a ^ { \mathrm { T } }}-\boldsymbol{I}\right)=-\boldsymbol{a}^{\wedge} .$$

这两个式子提供了处理 $\boldsymbol a^{\wedge}$ 高阶项的方法

把指数映射写成

$$\begin{aligned} \exp \left(\phi^{\wedge}\right) &=\exp \left(\theta \boldsymbol{a}^{\wedge}\right)=\sum_{n=0}^{\infty} \frac{1}{n !}\left(\theta \boldsymbol{a}^{\wedge}\right)^{n} \\ &=\boldsymbol{I}+\theta \boldsymbol{a}^{\wedge}+\frac{1}{2 !} \theta^{2} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\frac{1}{3 !} \theta^{3} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\frac{1}{4 !} \theta^{4}\left(\boldsymbol{a}^{\wedge}\right)^{4}+\cdots \\ &=\boldsymbol{a} \boldsymbol{a}^{\mathrm{T}}-\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\theta \boldsymbol{a}^{\wedge}+\frac{1}{2 !} \theta^{2} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}-\frac{1}{3 !} \theta^{3} \boldsymbol{a}^{\wedge}-\frac{1}{4 !} \theta^{4}\left(\boldsymbol{a}^{\wedge}\right)^{2}+\cdots \\ &=\boldsymbol{a} \boldsymbol{a}^{\mathrm{T}}+\underbrace{\left(\theta-\frac{1}{3 !} \theta^{3}+\frac{1}{5 !} \theta^{5}-\cdots\right)}_{\sin \theta} \boldsymbol{a}^{\wedge}-\underbrace{\left(1-\frac{1}{2 !} \theta^{2}+\frac{1}{4 !} \theta^{4}-\cdots\right)}_{\cos \theta} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \\ &=\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\boldsymbol{I}+\sin \theta \boldsymbol{a}^{\wedge}-\cos \theta \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \\ &=(1-\cos \theta) \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\boldsymbol{I}+\sin \theta \boldsymbol{a}^{\wedge} \\ &=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{a} \boldsymbol a^{\mathrm{T}}+\sin \theta \boldsymbol{a}^{\wedge} \end{aligned}$$

最后得到似曾相识的公式（第3讲罗德里格斯公式）：

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

这表明，$\mathfrak {so}(3)$ 实际上就是由所谓的旋转向量组成的空间，而指数映射即罗德里格斯公式

通过它们，把 $\mathfrak {so}(3)$ 中任意一个向量对应到了一个位于 $\mathrm{SO}(3)$ 中的旋转矩阵

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

$$\boldsymbol \phi=\ln (\boldsymbol{R})^{\vee}=\left(\sum_{n=0}^{\infty} \frac{(-1)^{n}}{n+1}(\boldsymbol{R}-\boldsymbol{I})^{n+1}\right)^{\vee} .$$

指数映射只是一个满射并不是单射

意味着每个 $\mathrm{SO}(3)$ 中的元素都可以找到一个 $\mathfrak {so}(3)$ 元素与之对应；但可能存在多个 $\mathfrak {so}(3)$ 中的元素对应到同一个 $\mathrm{SO}(3)$。

至少对于旋转角 $\theta$，多转360°和没有转是一样的——具有周期性

但如果把旋转角度固定在 $\pm \pi$ 之间，李群和李代数是一一对应的

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

$\mathfrak {se}(3)$ 上的指数映射形式如下：

$$\begin{aligned} \exp \left(\boldsymbol{\xi}^{\wedge}\right) &=\left[\begin{array}{cc} \sum_{n=0}^{\infty} \frac{1}{n !}\left(\boldsymbol \phi^{\wedge}\right)^{n} & \sum_{n=0}^{\infty} \frac{1}{(n+1) !}\left(\boldsymbol \phi^{\wedge}\right)^{n} \boldsymbol{\rho} \\ \boldsymbol{0}^{\mathrm{T}} & 1 \end{array}\right] \\ & \triangleq\left[\begin{array}{cc} \boldsymbol{R} & \boldsymbol{J} \boldsymbol{\rho} \\ \mathbf{0}^{\mathrm{T}} & 1 \end{array}\right]=\boldsymbol{T} . \end{aligned}$$

把 $\exp$ 进行泰勒展开推导此式，令 $\boldsymbol \phi=\theta \boldsymbol a$，其中 $\boldsymbol a$ 为单位向量，则

$$\begin{aligned} \sum_{n=0}^{\infty} \frac{1}{(n+1) !}\left(\boldsymbol{\phi}^{\wedge}\right)^{n} &=\boldsymbol{I}+\frac{1}{2 !} \theta \boldsymbol{a}^{\wedge}+\frac{1}{3 !} \theta^{2}\left(\boldsymbol{a}^{\wedge}\right)^{2}+\frac{1}{4 !} \theta^{3}\left(\boldsymbol{a}^{\wedge}\right)^{3}+\frac{1}{5 !} \theta^{4}\left(\boldsymbol{a}^{\wedge}\right)^{4} \cdots \\ &=\frac{1}{\theta}\left(\frac{1}{2 !} \theta^{2}-\frac{1}{4 !} \theta^{4}+\cdots\right)\left(\boldsymbol{a}^{\wedge}\right)+\frac{1}{\theta}\left(\frac{1}{3 !} \theta^{3}-\frac{1}{5} \theta^{5}+\cdots\right)\left(\boldsymbol{a}^{\wedge}\right)^{2}+\boldsymbol{I} \\ &=\frac{1}{\theta}(1-\cos \theta)\left(\boldsymbol{a}^{\wedge}\right)+\frac{\theta-\sin \theta}{\theta}\left(\boldsymbol{a} \boldsymbol{a}^{\mathrm{T}}-\boldsymbol{I}\right)+\boldsymbol{I} \\ &=\frac{\sin \theta}{\theta} \boldsymbol{I}+\left(1-\frac{\sin \theta}{\theta}\right) \boldsymbol{a} \boldsymbol{a}^{\top}+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge} \stackrel{\text { def }}{=} \boldsymbol{J} \end{aligned}$$

$\boldsymbol{\xi}$ 的指数映射左上角的 $\boldsymbol{R}$ 是 $\mathrm{SO}(3)$ 中的元素，与 $\mathfrak {se}(3)$ 中的旋转部分 $\boldsymbol \phi$ 对应

右上角的 $\boldsymbol{J}$ 由上面的推导公式给出：

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

平移部分经过指数映射之后，发生了一次以 $\boldsymbol{J}$ 为稀疏矩阵的线性变换

对数映射：从左上角的 $\boldsymbol{R}$ 计算旋转向量，右上角的 $\boldsymbol t$ 满足 $\boldsymbol t = \boldsymbol {J \rho}$，$\boldsymbol J$ 可由 $\boldsymbol \phi$ 得到，所以 $\boldsymbol \rho$ 也可由此线性方程解得。

李群、李代数的定义与相互转换关系：

李代数求导与扰动模型

BCH公式与近似形式

使用李代数的一大动机时进行优化，优化过程中导数是非常必要的信息

两个李代数指数映射乘积的完整形式由Baker-Campbell-Hausdorff公式（BCH公式）给出

由于其完整形式复杂，只给出其展开式的前几项：

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

其中 $[\quad]$ 为李括号

BCH公式告诉我们，当处理两个矩阵指数之积时，会产生一些由李括号组成的余项。

特别地，考虑 $\mathrm{SO}(3)$ 上的李代数 $\ln \left(\exp \left(\boldsymbol \phi_{1}^{\wedge}\right) \exp \left(\boldsymbol \phi_{2}^{\wedge}\right)\right)^{\vee}$，当 $\boldsymbol \phi_{1}$ 或 $\boldsymbol \phi_{2}$ 为小量时，小量二次以上的项都可以被忽略。

此时，BCH 拥有线性近似表达：

$$\ln \left(\exp \left(\boldsymbol \phi_{1}^{\wedge}\right) \exp \left(\boldsymbol \phi_{2}^{\wedge}\right)\right)^{\vee} \approx\left\{\begin{array}{ll} \boldsymbol{J}_{l}\left(\boldsymbol \phi_{2}\right)^{-1} \boldsymbol \phi_{1}+\boldsymbol \phi_{2} & \text { 当 } \boldsymbol \phi_{1} \text { 为小量 } \\ J_{r}\left(\boldsymbol \phi_{1}\right)^{-1} \boldsymbol \phi_{2}+\boldsymbol \phi_{1} & \text { 当 } \boldsymbol \phi_{2} \text { 为小量. } \end{array}\right.$$

第一个近似，该式告诉我们，当对一个旋转矩阵 $\boldsymbol R_2$（李代数为 $\boldsymbol \phi_{2}$）左乘一个微小旋转矩阵 $\boldsymbol R_1$（李代数为 $\boldsymbol \phi_{1}$）时，可以近似地看作在原有的李代数 $\boldsymbol \phi_{2}$ 上加上了一项 $\boldsymbol{J}_{l}\left(\boldsymbol \phi_{2}\right)^{-1} \boldsymbol \phi_{1}$。

同理第二个近似描述了右乘一个微小位移的情况

李代数在BCH近似下，分成了左乘近似和右乘近似两种

左乘BCH近似雅可比 $\boldsymbol J_l$ 事实上就是 $\boldsymbol{\xi}$ 的指数映射右上角的 $\boldsymbol J$:

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

它的逆为

$$\boldsymbol{J}_{l}^{-1}=\frac{\theta}{2} \cot \frac{\theta}{2} \boldsymbol{I}+\left(1-\frac{\theta}{2} \cot \frac{\theta}{2}\right) \boldsymbol{a} \boldsymbol{a}^{\mathrm{T}}-\frac{\theta}{2} \boldsymbol{a}^{\wedge}$$

而右乘雅可比仅需要对自变量取负号即可：

$$\boldsymbol{J}_{r}(\boldsymbol{\phi})=\boldsymbol{J}_{l}(-\boldsymbol{\phi})$$

BCH近似的意义：假定对某个旋转 $\boldsymbol R$，对应的李代数为 $\boldsymbol \phi$，给它左乘一个微小旋转，记作 $\Delta \boldsymbol{R}$，对应的李代数为 $\Delta \boldsymbol{\phi}$。那么在李群上，得到的结果就是 $\Delta \boldsymbol R \cdot \boldsymbol R$，而在李代数上，根据BCH近似为 $\boldsymbol{J}_{l}\left(\boldsymbol \phi\right)^{-1} \Delta \boldsymbol \phi+\boldsymbol \phi$。

合并起来，可以简单写成：

$$\exp \left(\Delta \boldsymbol \phi^{\wedge}\right) \exp \left(\boldsymbol \phi^{\wedge}\right)=\exp \left(\left(\boldsymbol \phi+\boldsymbol{J}_{l}^{-1}(\boldsymbol \phi) \Delta \boldsymbol \phi\right)^{\wedge}\right)$$

反之，在李代数上进行加法，让一个 $\boldsymbol{\phi}$ 加上 $\Delta \boldsymbol{\phi}$，可以近似为李群上带左右雅可比的乘法：

$$\exp \left((\boldsymbol{\phi}+\Delta \boldsymbol{\phi})^{\wedge}\right)=\exp \left(\left(\boldsymbol{J}_{l} \Delta \boldsymbol{\phi}\right)^{\wedge}\right) \exp \left(\boldsymbol{\phi}^{\wedge}\right)=\exp \left(\boldsymbol{\phi}^{\wedge}\right) \exp \left(\left(\boldsymbol{J}_{r} \Delta \boldsymbol{\phi}\right)^{\wedge}\right)$$

同样地对于 $\mathrm{SE}(3)$，也有类似的BCH近似：

$$\begin{array}{l} \exp \left(\Delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) \approx \exp \left(\left(\boldsymbol {\mathcal{J}}_{l}^{-1} \Delta \boldsymbol{\xi}+\boldsymbol{\xi}\right)^{\wedge}\right) \\ \exp \left(\boldsymbol{\xi}^{\wedge}\right) \exp \left(\Delta \boldsymbol{\xi}^{\wedge}\right) \approx \exp \left(\left(\boldsymbol {\mathcal{J}}_{r}^{-1} \Delta \boldsymbol{\xi}+\boldsymbol{\xi}\right)^{\wedge}\right) \end{array}$$

这里 $\boldsymbol {\mathcal{J}}_{l}$ 是一个 $6\times 6$ 的矩阵

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

在SLAM中，要估计一个相机的位置和姿态，该位姿是由 $\mathrm{SO}(3)$ 上的旋转矩阵或 $\mathrm{SE}(3)$ 上的变换矩阵描述的。

设某个时刻的位姿为 $\boldsymbol T$，观察到了一个世界坐标位于 $\boldsymbol p$ 的点，产生了一个观测数据 $\boldsymbol z$。

那么，由坐标变换关系知：

$$\boldsymbol z=\boldsymbol T\boldsymbol p+\boldsymbol w$$

其中 $\boldsymbol w$ 为随机噪声，由于它的存在，$\boldsymbol z$ 往往不可能满足 $\boldsymbol z=\boldsymbol T \boldsymbol p$ 的关系

计算理想的观测与实际数据的误差：

$$\boldsymbol e=\boldsymbol z - \boldsymbol T\boldsymbol p$$

假设一共有 $N$ 个这样的路标点和观测，于是就有 $N$ 个上式

那么进行位姿估计，相当于寻找一个最优的 $\boldsymbol T$，使得整体误差最小化：

$$\min _{T} J(\boldsymbol{T})=\sum_{i=1}^{N}\left\|\boldsymbol z_{i}-\boldsymbol{T} \boldsymbol{p}_{i}\right\|_{2}^{2}$$

求解此问题，需要计算目标函数J关于变换矩阵T的导数。

我们经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。

然而， $\mathrm{SO}(3)$，$\mathrm{SE}(3)$ 上并没有良好定义的加法，它们只是群。

如果把 $\boldsymbol T$ 当成一个普通矩阵来处理优化，就必须对它加以约束。

而从李代数角度来说，由于李代数由向量组成，具有良好的加法运算，因此，使用李代数解决求导问题的思路分为两种：

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

第一种方式对应到李代数的求导模型，而第二种方式则对应到扰动模型。

李代数求导

考虑 $\mathrm{SO}(3)$ 上，假设对一个空间点 $\boldsymbol p$ 进行了旋转，得到了 $\boldsymbol {Rp}$

计算旋转之后点的坐标相对于旋转的导数，非正式地记为

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

由于 $\mathrm{SO}(3)$ 没有加法，所以该导数无法按照导数定义进行计算

设 $\boldsymbol R$ 对应的李代数为 $\boldsymbol \phi$，转而计算：

$$\frac{\partial\left(\exp \left(\boldsymbol \phi^{\wedge}\right) \boldsymbol{p}\right)}{\partial \boldsymbol{\phi}}$$

按照导数定义，有：

$$\begin{aligned} \frac{\partial\left(\exp \left(\boldsymbol \phi^{\wedge}\right) p\right)}{\partial \boldsymbol{\boldsymbol \phi}} &=\lim _{\delta \boldsymbol \phi \rightarrow 0} \frac{\exp \left((\boldsymbol{\phi}+\delta \boldsymbol{\phi})^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\phi}} \\ &=\lim _{\delta \boldsymbol \phi \rightarrow 0} \frac{\exp \left(\left(\boldsymbol{J}_{l} \delta \boldsymbol{\phi}\right)^{\wedge}\right) \exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\phi}} \\ &=\lim _{\delta \boldsymbol \phi \rightarrow 0} \frac{\left(\boldsymbol{I}+\left(\boldsymbol{J}_{l} \delta \boldsymbol{\phi}\right)^{\wedge}\right) \exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol \phi} \\ &=\lim _{\delta \boldsymbol{\phi} \rightarrow 0} \frac{\left(\boldsymbol{J}_{l} \delta \boldsymbol{\phi}\right)^{\wedge} \exp \left(\boldsymbol \phi^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol \phi} \\ &=\lim _{\delta \boldsymbol{\phi} \rightarrow 0} \frac{-\left(\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}\right)^{\wedge} \boldsymbol{J}_{l} \delta \boldsymbol{\phi}}{\delta \boldsymbol{\phi}}=-(\boldsymbol{R} \boldsymbol{p})^{\wedge} \boldsymbol{J}_{l} . \end{aligned}$$

第2行的近似为BCH线性近似，第3行为泰勒展开舍去高阶项后的近似（由于取了极限，可以写等号)，第4行至第5行将反对称符号看作叉积，交换之后变号。

推导出了旋转后的点相对于李代数的导数：

$$\frac{\partial(\boldsymbol{R} \boldsymbol{p})}{\partial \boldsymbol{\phi}}=(-\boldsymbol{R} \boldsymbol{p})^{\wedge} \boldsymbol{J}_{l}$$

扰动模型（左乘）

另一种求导方式是对 $\boldsymbol R$ 进行一次扰动 $\Delta \boldsymbol R$，看结果相对于扰动的变化率。

这个扰动可以乘在左边也可以乘在右边，最后结果会有一点儿微小的差异

以左扰动为例，设左扰动 $\Delta \boldsymbol R$ 对应的李代数为 $\boldsymbol \varphi$。然后，对 $\boldsymbol \varphi$ 求导，即：

$$\frac{\partial(\boldsymbol{R} \boldsymbol p)}{\partial \boldsymbol{\varphi}}=\lim _{\boldsymbol \varphi \rightarrow 0} \frac{\exp \left(\boldsymbol \varphi^{\wedge}\right) \exp \left(\boldsymbol \phi^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol \phi^{\wedge}\right) \boldsymbol{p}}{\boldsymbol \varphi}$$

求导过程：

$$\begin{aligned} \frac{\partial(\boldsymbol{R} \boldsymbol p)}{\partial \boldsymbol\varphi} &=\lim _{\boldsymbol\varphi \rightarrow 0} \frac{\exp \left(\boldsymbol{\varphi}^{\wedge}\right) \exp \left(\boldsymbol\phi^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol\phi^{\wedge}\right) \boldsymbol{p}}{\boldsymbol\varphi} \\ &=\lim _{\boldsymbol\varphi \rightarrow 0} \frac{\left(\boldsymbol{I}+\boldsymbol\varphi^{\wedge}\right) \exp \left(\boldsymbol\phi^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol\phi^{\wedge}\right) \boldsymbol{p}}{\boldsymbol\varphi} \\ &=\lim _{\boldsymbol\varphi \rightarrow 0} \frac{\boldsymbol\varphi^{\wedge} \boldsymbol{R} \boldsymbol p}{\boldsymbol \varphi}=\lim _{\boldsymbol\varphi \rightarrow 0} \frac{-(\boldsymbol{R} \boldsymbol{p})^{\wedge} \boldsymbol \varphi}{\boldsymbol \varphi}=-(\boldsymbol{R} \boldsymbol p)^{\wedge} . \end{aligned}$$

相比于直接对李代数求导，省去了一个雅可比 $\boldsymbol J_l$ 的计算。这使得扰动模型更为实用。

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

$\mathrm{SE}(3)$ 上的扰动模型

假设某空间点 $\boldsymbol p$ 经过一次变换 $\boldsymbol T$（对应李代数为 $\boldsymbol \xi$），得到 $\boldsymbol {Tp}$

为了使乘法成立，$\boldsymbol {p}$ 必须使用齐次坐标

给 $\boldsymbol T$ 左乘一个扰动 $\Delta \boldsymbol{T}=\exp \left(\delta \boldsymbol{\xi}^{\wedge}\right)$，设扰动项的李代数为 $\delta \boldsymbol \xi=[\delta \boldsymbol{\rho}, \delta \boldsymbol \phi]^{\mathrm{T}}$，那么：

$$\begin{array}{l} \frac{\partial(\boldsymbol{T} \boldsymbol{p})}{\partial \delta \boldsymbol{\xi}}=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\exp \left(\delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\xi}}\\ =\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\left(\boldsymbol{I}+\delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\xi}}\\ =\lim _{\delta \boldsymbol\xi \rightarrow 0} \frac{\delta \boldsymbol{\xi}^{\wedge} \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol p}{\delta \boldsymbol{\xi}}\\ =\lim _{\delta \boldsymbol\xi \rightarrow 0} \frac{\left[\begin{array}{cc} \delta \boldsymbol\phi^{\wedge} & \delta \boldsymbol\rho \\ \mathbf{0}^{\mathrm{T}} & 0 \end{array}\right]\left[\begin{array}{c} \boldsymbol{R} \boldsymbol p+t \\ 1 \end{array}\right]}{\delta \boldsymbol\xi}\\ =\lim _{\delta \boldsymbol\xi \rightarrow 0} \frac{\left[\begin{array}{c} \delta \boldsymbol\phi^{\wedge}(\boldsymbol{R} \boldsymbol p+t)+\delta \boldsymbol{\rho} \\ \mathbf{0}^{\mathrm{T}} \end{array}\right]}{[\delta \boldsymbol{\rho}, \delta \boldsymbol{\phi}]^{\mathrm{T}}}=\left[\begin{array}{cc} \boldsymbol{I} & -(\boldsymbol{R} \boldsymbol{p}+t)^{\wedge} \\ \mathbf{0}^{\mathrm{T}} & \mathbf{0}^{\mathrm{T}} \end{array}\right] \stackrel{\text { def }}{=}(\boldsymbol{T} \boldsymbol{p})^{\odot} . \end{array}$$

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

此式稍微需要解释的是矩阵求导方面的顺序，假设 $\boldsymbol a, \boldsymbol b, \boldsymbol x, \boldsymbol y$ 都是列向量，那么在符号写法下，有如下的规则：

$$\frac{\mathrm{d}\left[\begin{array}{l} \boldsymbol{a} \\ \boldsymbol{b} \end{array}\right]}{\mathrm{d}\left[\begin{array}{l} \boldsymbol{x} \\ \boldsymbol{y} \end{array}\right]}=\left(\frac{\mathrm{d}[\boldsymbol{a}, \boldsymbol{b}]^{\mathrm{T}}}{\mathrm{d}\left[\begin{array}{l} \boldsymbol{x} \\ \boldsymbol{y} \end{array}\right]}\right)^{\mathrm{T}}=\left[\begin{array}{ll} \frac{\mathrm{d} \boldsymbol{a}}{\mathrm{d} \boldsymbol{x}} & \frac{\mathrm{d} \boldsymbol{b}}{\mathrm{d} \boldsymbol{x}} \\ \frac{\mathrm{d} \boldsymbol{a}}{\mathrm{d} \boldsymbol{y}} & \frac{\mathrm{d} \boldsymbol b}{\mathrm{~d} \boldsymbol{y}} \end{array}\right]^{\mathrm{T}}=\left[\begin{array}{ll} \frac{\mathrm{d} \boldsymbol{a}}{\mathrm{d} \boldsymbol{x}} & \frac{\mathrm{d} \boldsymbol{a}}{\mathrm{d} \boldsymbol{y}} \\ \frac{\mathrm{d} \boldsymbol{b}}{\mathrm{d} \boldsymbol{x}} & \frac{\mathrm{d} \boldsymbol{b}}{\mathrm{d} \boldsymbol{y}} \end{array}\right]$$

*相似变换群与李代数

单目视觉中使用相似变换群 $\mathrm{Sim}(3)$，对应的李代数 $\mathfrak {sim}(3)$

如果在单目SLAM中使用 $\mathrm{SE}(3)$ 表示位姿，那么由于尺度不确定性与尺度漂移，整个SLAM过程中的尺度会发生变化，这在 $\mathrm{SE}(3)$ 中未能体现出来。

因此，在单目情况下一般会显式地把尺度因子表达出来。

用数学语言来说，对于位于空间的点 $\boldsymbol p$，在相机坐标系下要经过一个相似变换，而非欧氏变换：

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

在相似变换中，把尺度 $s$ 表达出来了

同时作用在 $\boldsymbol p$ 的3个坐标之上，对 $\boldsymbol p$ 进行了一次缩放

与 $\mathrm{SO}(3)$、$\mathrm{SE}(3)$ 相似，相似变换也对矩阵乘法构成群，称为相似变换群 $\mathrm{Sim}(3)$：

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

同样地，$\mathrm{Sim}(3)$ 也有对应的李代数、指数映射、对数映射等。

李代数 $\mathrm{Sim}(3)$ 元素是一个7维向量 $\boldsymbol \zeta$。它的前6维与 $\mathfrak {se}(3)$ 相同，最后多了一项 $\sigma$：

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

关联 $\mathrm{Sim}(3)$ 和 $\mathfrak {sim}(3)$ 的仍是指数映射和对数映射，指数映射为：

$$\exp \left(\boldsymbol{\zeta}^{\wedge}\right)=\left[\begin{array}{cc} \mathrm{e}^{\sigma} \exp \left(\boldsymbol\phi^{\wedge}\right) & \boldsymbol{J}_{s} \boldsymbol\rho \\ \boldsymbol 0^{\mathrm{T}} & 1 \end{array}\right] .$$

其中，$\boldsymbol{J}_{s}$ 的形式为：

$$\begin{aligned} \boldsymbol{J}_{s}=& \frac{\mathrm{e}^{\sigma}-1}{\sigma} \boldsymbol{I}+\frac{\sigma \mathrm{e}^{\sigma} \sin \theta+\left(1-\mathrm{e}^{\sigma} \cos \theta\right) \theta}{\sigma^{2}+\theta^{2}} \boldsymbol{a}^{\wedge} \\ &+\left(\frac{\mathrm{e}^{\sigma}-1}{\sigma}-\frac{\left(\mathrm{e}^{\sigma} \cos \theta-1\right) \sigma+\left(\mathrm{e}^{\sigma} \sin \theta\right) \theta}{\sigma^{2}+\theta^{2}}\right) \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} . \end{aligned}$$

通过指数映射，可以找到李代数和李群的关系

对于李代数 $\boldsymbol \zeta$，它与李群的对应关系为：

$$s=\mathrm{e}^{\sigma}, \boldsymbol{R}=\exp \left(\boldsymbol{\phi}^{\wedge}\right), \boldsymbol{t}=\boldsymbol{J}_{s} \boldsymbol{\rho}$$

旋转部分和 $\mathrm{SO}(3)$ 是一致的

平移部分，在 $\mathfrak {se}(3)$ 中需要乘一个雅可比 $\mathcal{J}$，而相似变换的雅可比更复杂。

对于尺度因子，可以看到李群中的 $s$ 即为李代数中 $\sigma$ 的指数函数。

$\mathrm{Sim}(3)$ 的BCH近似与 $\mathrm{SE}(3)$ 是类似的

可以讨论一个点 $\boldsymbol p$ 经过相似变换 $\boldsymbol {Sp}$ 后，相对于S的导数。同样地，存在微分模型和扰动模型两种方式，而扰动模型较为简单。

直接给出扰动模型的结果

设给予 $\boldsymbol {Sp}$ 左侧一个小扰动 $\exp \left(\boldsymbol{\zeta}^{\wedge}\right)$，并求 $\boldsymbol {Sp}$ 对于扰动的导数。

因为 $\boldsymbol {Sp}$ 是4维的齐次坐标，$\boldsymbol \zeta$ 是7维向量，所以该导数应该是 $4×7$ 的雅可比。

方便起见，记 $\boldsymbol {Sp}$ 的前3维组成向量为 $\boldsymbol q$，那么：

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

本讲引入了李群 $\mathrm{SO}(3)$ 和 $\mathrm{SE}(3)$，以及它们对应的李代数 $\mathfrak {so}(3)$ 和 $\mathfrak {se}(3)$。

介绍了位姿在它们上面的表达和转换，然后通过BCH的线性近似，就可以对位姿进行扰动并求导。

第5讲 相机与图像

第3讲和第4讲，介绍了“机器人如何表示自身位姿”的问题，部分地解释了SLAM经典模型中变量的含义和运动方程部分。

本讲将讨论“机器人如何观测外部世界”，也就是观测方程部分。

而在以相机为主的视觉SLAM 中，观测主要是指相机成像的过程。

在计算机中，一张照片由很多个像素组成，每个像素记录了色彩或亮度的信息。

三维世界中的一个物体反射或发出的光线，穿过相机光心后，投影在相机的成像平面上。

相机的感光器件接收到光线后，产生测量值，就得到了像素，形成了我们见到的照片。

相机模型

相机将三维世界中的坐标点（单位为米）映射到二维图像平面（单位为像素）的过程能够用一个几何模型进行描述。

最简单的模型称为针孔模型。

针孔模型是很常用而且有效的模型，描述了一束光线通过针孔之后，在针孔背面投影成像的关系。

由于相机镜头上的透镜的存在，使得光线投影到成像平面的过程中会产生畸变。

因此，使用针孔和畸变两个模型来描述整个投影过程。

这两个模型能够把外部的三维点投影到相机内部成像平面，构成相机的内参数（Intrinsics）。

针孔相机模型

设 $O-x-y-z$ 为相机坐标系，习惯上我们让 $z$ 轴指向相机前方，$x$ 轴向右，$y$ 轴向下。

$O$ 为摄像机的光心，也是针孔模型中的针孔。

现实世界的空间点 $P$，经过小孔 $O$ 投影之后，落在物理成像平面 $O^{\prime}-x^{\prime}-y^{\prime}$ 上，成像点为 $P^{\prime}$。

设 $P$ 的坐标为 $[X,Y,Z]^T$，$P^{\prime}$ 为 $[X^{\prime},Y^{\prime},Z^{\prime}]^T$，并且设物理成像平面到小孔的距离为 $f$ (焦距)。

那么，根据三角形相似关系，有：

$$\frac{Z}{f}=-\frac{X}{X^{\prime}}=-\frac{Y}{Y^{\prime}}$$

其中负号表示成的像是倒立的

不过，实际相机得到的图像并不是倒像（否则相机的使用会非常不方便)。

为了让模型更符合实际，等价地把成像平面对称地放到相机前方，和三维空间点一起放在摄像机坐标系的同一侧，这样做可以把公式中的负号去掉，使式子更加简洁:

$$\frac{Z}{f}=\frac{X}{X^{\prime}}=\frac{Y}{Y^{\prime}} .$$

把 $X^{\prime},Y^{\prime}$ 放到等式左侧，整理得：

$$\begin{aligned} X^{\prime} &=f \frac{X}{Z} \\ Y^{\prime} &=f \frac{Y}{Z} \end{aligned}$$

描述了点 $P$ 和它的像之间的空间关系

不过，在相机中，最终获得的是一个个的像素，还需要在成像平面上对像进行采样和量化。

为了描述传感器将感受到的光线转换成图像像素的过程，设在物理成像平面上固定着一个像素平面 $o-u-v$。

在像素平面得到了 $P^{\prime}$ 的像素坐标：$[u,v]^T$。

像素坐标系（或图像坐标系）通常的定义方式是：原点 $o^{\prime}$ 位于图像的左上角，$u$ 轴向右与 $x$ 轴平行，$v$ 轴向下与 $y$ 轴平行。

像素坐标系与成像平面之间，相差了一个缩放和一个原点的平移。

设像素坐标在 $u$ 轴上缩放了 $α$ 倍，在 $v$ 轴上缩放了 $β$ 倍。同时，原点平移了 $[c_x,c_y]^T$。

那么，$P^{\prime}$ 的坐标与像素坐标 $[u,v]^T$ 的关系为：

$$\left\{\begin{array}{l} u=\alpha X^{\prime}+c_{x} \\ v=\beta Y^{\prime}+c_{y} \end{array}\right.$$

代入式 $\begin{aligned} X^{\prime} &=f \frac{X}{Z} \\ Y^{\prime} &=f \frac{Y}{Z} \end{aligned}$ 并把 $\alpha f$ 合并成 $f_x$，$\beta f$ 合并成 $f_y$，得：

$$\left\{\begin{array}{l} u=f_{x} \frac{X}{Z}+c_{x} \\ v=f_{y} \frac{Y}{Z}+c_{y} \end{array}\right.$$

其中，$f$ 的单位为米，$α,β$ 的单位为像素/米，所以 $f_x,f_y$ 和 $c_x,c_y$ 的单位为像素。

写成矩阵形式，左侧需要用齐次坐标，右侧则是非齐次坐标：

$$\left(\begin{array}{l} u \\ v \\ 1 \end{array}\right)=\frac{1}{Z}\left(\begin{array}{ccc} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{array}\right)\left(\begin{array}{c} X \\ Y \\ Z \end{array}\right) \stackrel{\text { def }}{=} \frac{1}{Z} \boldsymbol{K} \boldsymbol{P} \text {. }$$

习惯性把 $Z$ 移到左侧：

$$Z\left(\begin{array}{l} u \\ v \\ 1 \end{array}\right)=\left(\begin{array}{ccc} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{array}\right)\left(\begin{array}{c} X \\ Y \\ Z \end{array}\right) \stackrel{\text { def }}{=} \boldsymbol{K} \boldsymbol{P} \text {. }$$

该式中，把中间的量组成的矩阵称为相机的内参数(Camera Intrinsics )矩阵 $K$。

通常认为，相机的内参在出厂之后是固定的，不会在使用过程中发生变化。

标定：有的相机生产厂商会告诉相机的内参，而有时需要自己确定相机的内参，也就是所谓的标定。

有内参，自然也有相对的外参。在上式中，使用的是 $P$ 在相机坐标系下的坐标，但实际上由于相机在运动，所以 $P$ 的相机坐标应该是它的世界坐标（记为 $P_w$）根据相机的当前位姿变换到相机坐标系下的结果。

相机的位姿由它的旋转矩阵 $\boldsymbol R$ 和平移向量 $\boldsymbol t$ 来描述。那么有：

$$Z \boldsymbol{P}_{u v}=Z\left[\begin{array}{l} u \\ v \\ 1 \end{array}\right]=\boldsymbol{K}\left(\boldsymbol{R} \boldsymbol{P}_{\mathrm{w}}+\boldsymbol{t}\right)=\boldsymbol{K} \boldsymbol{T} \boldsymbol{P}_{\mathrm{w}} .$$

描述了 $P$ 的世界坐标到像素坐标的投影关系。

其中，相机的位姿 $\boldsymbol R,\boldsymbol t$ 又称为相机的外参数(Camera Extrinsics )。

相比于不变的内参，外参会随着相机运动发生改变，也是SLAM中待估计的目标，代表着机器人的轨迹。

投影过程还可以从另一个角度来看。上式表明，可以把一个世界坐标点先转换到相机坐标系，再除掉它最后一维的数值（即该点距离相机成像平面的深度），这相当于把最后一维进行归一化处理，得到点 $P$ 在相机归一化平面上的投影：

$$\left(\boldsymbol{R} \boldsymbol{P}_{\mathrm{w}}+\boldsymbol{t}\right)=\underbrace{[X, Y, Z]^{\mathrm{T}}}_{\text {相机坐标 }} \rightarrow \underbrace{[X / Z, Y / Z, 1]^{\mathrm{T}}}_{\text {归一化坐标 }} .$$

归一化坐标可看成相机前方 $z=1$ 处的平面上的一个点，这个 $z=1$ 平面也称为归一化平面。

归一化坐标再左乘内参就得到了像素坐标，所以把像素坐标 $[u,v]^T$ 看成对归一化平面上的点进行量化测量的结果。

从这个模型中也可以看出，如果对相机坐标同时乘以任意非零常数，归一化坐标都是一样的，这说明点的深度在投影过程中被丢失了，所以单目视觉中没法得到像素点的深度值。

畸变模型

为了获得好的成像效果，在相机的前方加了透镜。

透镜的加入会对成像过程中光线的传播产生新的影响：

1. 透镜自身的形状对光线传播的影响
2. 在机械组装过程中，透镜和成像平面不可能完全平行，会使光线穿过透镜投影到成像面时的位置发生变化

由透镜形状引起的畸变（Distortion，也叫失真）称为径向畸变

在针孔模型中，一条直线投影到像素平面上还是一条直线

在实际拍摄的照片中，摄像机的透镜往往使得真实环境中的一条直线在图片中变成了曲线。越靠近图像的边缘，这种现象越明显。由于实际加工制作的透镜往往是中心对称的，这使得不规则的畸变通常径向对称。

主要分为两大类：桶形畸变和枕形畸变

桶形畸变图像放大率随着与光轴之间的距离增加而减小，而枕形畸变则恰好相反。

在这两种畸变中,穿过图像中心和光轴有交点的直线还能保持形状不变。

除了透镜的形状会引入径向畸变，由于在相机的组装过程中不能使透镜和成像面严格平行，所以也会引入切向畸变：

用更严格的数学形式对径向畸变和切向畸变进行描述

考虑归一化平面上的任意一点 $p$，它的坐标为 $[x,y]^T$，也可写成极坐标的形式 $[r,\theta]^T$，其中 $r$ 表示点 $p$ 与坐标系原点之间的距离，$\theta$ 表示与水平轴的夹角。

径向畸变可以看成坐标点沿着长度方向发生了变化，也就是其距离原点的长度发生了变化。切向畸变可以看成坐标点沿着切线方向发生了变化,也就是水平夹角发生了变化。

通常假设这些畸变呈多项式关系，即：

$$\begin{array}{l} x_{\text {distorted }}=x\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right) \\ y_{\text {distorted }}=y\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right) \end{array}$$

其中，$[x_{\text {distorted }},y_{\text {distorted }}]^T$ 是畸变后点的**归一化坐标**

对于切向畸变，使用另外两个参数 $p_1,p_2$ 进行纠正：

$$\begin{array}{l} x_{\text {distorted }}=x+2 p_{1} x y+p_{2}\left(r^{2}+2 x^{2}\right) \\ y_{\text {distorted }}=y+p_{1}\left(r^{2}+2 y^{2}\right)+2 p_{2} x y \end{array} .$$

联合上面两式，对于相机坐标系中的一点 $P$，能够通过5个畸变参数找到这个点在像素平面上的正确位置：

1. 将三维空间点投影到归一化图像平面，设归一化坐标为 $[x,y]^T$

2. 对归一化平面上的点计算径向畸变和切向畸变

   $$\left\{\begin{array}{l} x_{\text {distorted }}=x\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right)+2 p_{1} x y+p_{2}\left(r^{2}+2 x^{2}\right) \\ y_{\text {distorted }}=y\left(1+k_{1} r^{2}+k_{2} r^{4}+k_{3} r^{6}\right)+p_{1}\left(r^{2}+2 y^{2}\right)+2 p_{2} x y \end{array}\right.$$

3. 将畸变后的点通过内参数矩阵投影到像素平面，得到该点在图像上的正确位置

   $$\left\{\begin{array}{l} u=f_{x} x_{\text {distorted }}+c_{x} \\ v=f_{y} y_{\text {distorted }}+c_{y} \end{array}\right.$$

在上面的纠正畸变的过程中，使用了5个畸变项。

实际应用中，可以灵活选择纠正模型，比如只选择 $k_1, p_1,p_2$ 这3项。

值得一提的是，存在两种去畸变处理（Undistort，或称畸变校正）做法：

1. 选择先对整张图像进行去畸变，得到去畸变后的图像，然后讨论此图像上的点的空间位置。
2. 从畸变图像上的某个点出发，按照畸变方程，讨论其畸变前的空间位置。

二者都是可行的，不过前者在视觉SLAM中似乎更常见。

所以，当一个图像去畸变之后，可以直接用针孔模型建立投影关系，而不用考虑畸变。

单目相机的成像过程：

1. 世界坐标系下有一个固定的点 $P$，世界坐标为 $\boldsymbol P_w$。
2. 由于相机在运动，它的运动由 $\boldsymbol R,\boldsymbol t$ 或变换矩阵 $T\in \mathrm{SE}(3)$ 描述。$P$ 的相机坐标为：$\tilde{\boldsymbol{P}_{\mathrm{c}}}=\left(\boldsymbol{R} \boldsymbol{P}_{\mathrm{w}}+\boldsymbol{t}\right)$ 。
3. 这时的 $\tilde{\boldsymbol{P}_{\mathrm{c}}}$ 的分量为 $X,Y,Z$，投影到归一化平面 $Z=1$ 上，得到P的归一化坐标：$\boldsymbol{P}_{\mathrm{c}}=[X / Z, Y / Z, 1]^{\mathrm{T}}$。
4. 有畸变时，根据畸变参数计算 $\boldsymbol{P}_{\mathrm{c}}$ 发生畸变后的坐标。
5. $P$ 的归一化坐标经过内参后，对应到它的像素坐标：$\boldsymbol P_{uv}= \boldsymbol {KP}_c$。

世界坐标 -> 相机坐标 -> 归一化坐标 -> 像素坐标

双目相机模型

针孔相机模型描述了单个相机的成像模型

然而，仅根据一个像素，无法确定这个空间点的具体位置。因为从相机光心到归一化平面连线上的所有点，都可以投影至该像素上。只有当 $P$ 的深度确定时（比如通过双目或RGB-D相机），才能确切地知道空间位置：

测量像素距离（或深度）的方式有很多种，比如人眼根据左右眼看到的景物差异（或称视差）判断物体与我们的距离。

双目相机的原理亦是如此：通过同步采集左右相机的图像，计算图像间视差，以便估计每一个像素的深度。

在左右双目相机中，可以把两个相机都看作针孔相机，它们是水平放置的，两个相机的光圈中心都位于 $x$ 轴上

两者之间的距离称为双目相机的基线（记作 $b$）是双目相机的重要参数。

考虑一个空间点 $P$，在左眼相机和右眼相机各成一像，记作 $P_L,P_R$。

由于相机基线的存在，这两个成像位置是不同的。理想情况下，由于左右相机只在 $x$ 轴上有位移，所以 $P$ 的像也只在 $x$ 轴（对应图像的 $u$ 轴）上有差异。

记它的左侧坐标为 $u_L$，右侧坐标为 $u_R$

根据 $△PP_LP_R$ 和 $△PO_LO_R$ 的相似关系，有：

$$\frac{z-f}{z}=\frac{b-u_{\mathrm{L}}+u_{\mathrm{R}}}{b} .$$

整理得：

$$z=\frac{f b}{d}, \quad d \stackrel{\text { def }}{=} u_{\mathrm{L}}-u_{\mathrm{R}}$$

其中 $d$ 定义为左右图的横坐标之差，称为视差。

根据视差，可以估计一个像素与相机之间的距离。

视差与距离成反比：视差越大，距离越近。

同时，由于视差最小为一个像素，于是双目的深度存在一个理论上的最大值，由 $fb$ 确定。

基线越长，双目能测到的最大距离就越远；反之，小型双目器件则只能测量很近的距离。相似地，人眼在看非常远的物体时（如很远的飞机），通常不能准确判断它的距离。

虽然由视差计算深度的公式很简洁，但视差 $d$ 本身的计算却比较困难。需要确切地知道左眼图像的某个像素出现在右眼图像的哪一个位置（即对应关系），这件事也属于“人类觉得容易而计算机觉得困难”的任务。

当想计算每个像素的深度时，其计算量与精度都将成为问题，而且只有在图像纹理变化丰富的地方才能计算视差。由于计算量的原因，双目深度估计仍需要使用GPU 或FPGA来实时计算。

RGB-D相机模型

相比于双目相机通过视差计算深度的方式，RGB-D相机能够主动测量每个像素的深度。

目前的RGB-D相机按原理可分为两大类：

1. 通过红外结构光（Structured Light）原理测量像素距离。
2. 通过飞行时间（Time-of-Flight，ToF）原理测量像素距离。

无论是哪种类型，RGB-D相机都需要向探测目标发射一束光线（通常是红外光）。

在红外结构光原理中，相机根据返回的结构光图案，计算物体与自身之间的距离。

而在ToF原理中，相机向目标发射脉冲光，然后根据发送到返回之间的光束飞行时间，确定物体与自身的距离。

ToF的原理和激光传感器十分相似，只不过激光是通过逐点扫描获取距离的，而ToF相机则可以获得整个图像的像素深度，这也正是RGB-D相机的特点。

在测量深度之后，RGB-D相机通常按照生产时的各相机摆放位置，自己完成深度与彩色图像素之间的配对，输出一一对应的彩色图和深度图。

在同一个图像位置，读取到色彩信息和距离信息，计算像素的3D相机坐标，生成点云（Point Cloud）。既可以在图像层面对RGB-D数据进行处理，也可在点云层面处理。

图像

在数学中，图像可以用一个矩阵来描述；而在计算机中，它们占据一段连续的磁盘或内存空间，可以用二维数组来表示。

通过计算机图像处理的一些基本操作。特别地，通过OpenCV中图像数据的处理，理解计算机中处理图像的常见步骤。

最简单的图像——灰度图。在一张灰度图中，每个像素位置 $(x, y)$ 对应一个灰度值 $I$，所以，一张宽度为 $w$、高度为 $h$ 的图像，数学上可以记为一个函数：

$$I(x, y): \mathbb{R}^{2} \mapsto \mathbb{R}$$

其中，$(x, y)$ 是像素的坐标

然而，计算机并不能表达实数空间，所以我们需要对下标和图像读数在某个范围内进行量化。例如，$x, y$ 通常是从 $0$ 开始的整数。在常见的灰度图中，用 $0 \sim 255$ 的整数（即一个unsigned char，1个字节）来表达图像的灰度读数。那么，一张宽度为 $640$ 像素、高度为 $480$ 像素分辨率的灰度图就可以表示为：

数组的行数对应图像的高度，而列数对于图像的宽度

一个像素的灰度可以用 $8$ 位来记录，也就是一个 $0\sim 255$ 的值。

当要记录的信息更多时，一个字节恐怕就不够了。

例如，在RGB-D相机的深度图中，记录了各个像素与相机之间的距离。这个距离通常以毫米为单位，而RGB-D相机的量程通常在十几米左右，超过了 $255$。这时，会采用 $16$ 位整数（C++中的unsigned short）来记录深度图的信息，也就是位于 $0\sim 65535$ 的值。换算成米的话，最大可以表示 $65$ 米，足够RGB-D相机使用了。

彩色图像的表示则需要通道（channel）的概念。

在计算机中，用红色、绿色和蓝色这三种颜色的组合来表达任意一种色彩。于是对于每一个像素，就要记录其 $\mathrm{R、G、B}$ 三个数值，每一个数值就称为一个通道。

例如，最常见的彩色图像有三个通道，每个通道都由 $8$ 位表示。在这种规定下，一个像素占据 $24$ 位空间。

通道的数量、顺序都是可以自由定义的。在OpenCV的彩色图像中，通道的默认顺序是 $\mathrm{B、G、R}$。也就是说，当得到一个 $24$ 位的像素时，前 $8$ 位表示蓝色数值，中间 $8$ 位为绿色数值，最后 $8$ 位为红色数值。同理，也可使用 $\mathrm{R、G、B}$ 的顺序表示一个彩色图。如果还想表达图像的透明度，就使用 $\mathrm{R、G、B、A}$ 四个通道。

第6讲 非线性优化

运动方程中的位姿可以由变换矩阵来描述，然后用李代数进行优化。

观测方程由相机成像模型给出，其中内参是随相机固定的，而外参则是相机的位姿。

然而，由于噪声的存在，运动方程和观测方程的等式必定不是精确成立的。

尽管相机可以非常好地符合针孔模型，但遗憾的是，得到的数据通常是受各种未知噪声影响的。即使有高精度的相机，运动方程和观测方程也只能近似成立。所以，与其假设数据必须符合方程，不如讨论如何在有噪声的数据中进行准确的状态估计。

状态估计问题

批量状态估计与最大后验估计

经典的SLAM模型由一个运动方程和一个观测方程构成：

$$\left\{\begin{array}{l} \boldsymbol{x}_{k}=f\left(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}\right)+\boldsymbol{w}_{k} \\ \boldsymbol{z}_{k, j}=h\left(\boldsymbol{y}_{j}, \boldsymbol{x}_{k}\right)+\boldsymbol{v}_{k, j} \end{array}\right.$$

通过第4讲的知识，这里的 $x_k$ 乃是相机的位姿，可以用 $\mathrm{SE}(3)$ 来描述。

至于观测方程，第5讲已经说明，即针孔相机模型。

首先，位姿变量 $x_k$ 可以由 $\boldsymbol T_k \in \mathrm{SE}(3)$ 表达。其次，运动方程与输入的具体形式有关，但在视觉SLAM中没有特殊性（和普通的机器人、车辆的情况一样)。

观测方程则由针孔模型给定。假设在 $x_k$ 处对路标 $y_j$ 进行了一次观测，对应到图像上的像素位置 $z_{k,j}$，那么，观测方程可以表示成：

$$s \boldsymbol{z}_{k, j}=\boldsymbol{K}\left(\boldsymbol{R}_{k} \boldsymbol{y}_{j}+\boldsymbol{t}_{k}\right)$$

其中 $\boldsymbol K$ 为相机内参，$s$ 为像素点的距离，也是 $\left(\boldsymbol{R}_{k} \boldsymbol{y}_{j}+\boldsymbol{t}_{k}\right)$ 的第三个分量。

如果使用变换矩阵 $\boldsymbol T_k$ 描述位姿，那么路标点 $y_j$ 必须以齐次坐标来描述，计算完成后要转换为非齐次坐标（第5讲）。

现在，考虑数据受噪声影响后会发生什么改变。在运动和观测方程中，通常假设两个噪声项 $w_k,v_{k,j}$ 满足零均值的高斯分布，像这样：

$$\boldsymbol{w}_{k} \sim \mathcal{N}\left(\mathbf{0}, \boldsymbol{R}_{k}\right), \boldsymbol{v}_{k} \sim \mathcal{N}\left(\mathbf{0}, \boldsymbol{Q}_{k, j}\right)$$

其中 $\mathcal{N}$ 表示高斯分布，$0$ 表示零均值，$\boldsymbol{R}_{k},\boldsymbol{Q}_{k,j}$ 为协方差矩阵。

在这些噪声的影响下，希望通过带噪声的数据 $z$ 和 $u$ 推断位姿 $x$ 和地图 $y$（以及它们的概率分布），这构成了一个状态估计问题。

处理这个状态估计问题的方法大致分成两种。

由于在 SLAM过程中，这些数据是随时间逐渐到来的，所以，应该持有一个当前时刻的估计状态，然后用新的数据来更新它。这种方式称为增量/渐进（incremental）的方法，或者叫滤波器。在历史上很长一段时间内，研究者们使用滤波器，尤其是扩展卡尔曼滤波器及其衍生方法求解它。

另一种方式，则是把数据“攒”起来一并处理，这种方式称为批量（batch）的方法。例如，可以把 $0$ 到 $k$ 时刻所有的输入和观测数据都放在一起。

这两种不同的处理方式引出了很多不同的估计手段。

大体来说，增量方法仅关心当前时刻的状态估计 $x_k$，而对之前的状态则不多考虑；

相对地，批量方法可以在更大的范围达到最优化，被认为优于传统的滤波器，而成为当前视觉SLAM的主流方法。

极端情况下，可以让机器人或无人机收集所有时刻的数据，再带回计算中心统一处理，这也正是SfM（Structure from Motion）的主流做法。当然，这种极端情况显然是不实时的，不符合SLAM的运用场景。

所以在SLAM中，实用的方法通常是一些折衷的手段。例如，固定一些历史轨迹，仅对当前时刻附近的一些轨迹进行优化（滑动窗口估计法）。

以非线性优化为主的批量优化方法：

考虑从 $1$ 到 $N$ 的所有时刻，并假设有 $M$ 个路标点。定义所有时刻的机器人位姿和路标点坐标为：

$$\boldsymbol{x}=\left\{\boldsymbol{x}_{1}, \ldots, \boldsymbol{x}_{N}\right\}, \quad \boldsymbol{y}=\left\{\boldsymbol{y}_{1}, \ldots, \boldsymbol{y}_{M}\right\}$$

同样地，用不带下标的 $\boldsymbol u$ 表示所有时刻的输入，$\boldsymbol z$ 表示所有时刻的观测数据。

那么对机器人状态的估计，从概率学的观点来看，就是已知输入数据 $\boldsymbol u$ 和观测数据 $\boldsymbol z$ 的条件下，求状态 $\boldsymbol x ,\boldsymbol y$ 的条件概率分布：

$$P(\boldsymbol{x}, \boldsymbol{y} \mid \boldsymbol{z}, \boldsymbol{u})$$

特别地，当不知道控制输入，只有一张张的图像时，即只考虑观测方程带来的数据时，相当于估计 $P(\boldsymbol{x}, \boldsymbol{y} \mid \boldsymbol{z})$ 的条件概率分布，此问题也称为SfM，即如何从许多图像中重建三维空间结构。

为了估计状态变量的条件分布，利用贝叶斯法则，有：

$$P(\boldsymbol{x}, \boldsymbol{y} \mid \boldsymbol{z}, \boldsymbol{u})=\frac{P(\boldsymbol{z}, \boldsymbol{u} \mid \boldsymbol{x}, \boldsymbol{y}) P(\boldsymbol{x}, \boldsymbol{y})}{P(\boldsymbol{z}, \boldsymbol{u})} \propto \underbrace{P(\boldsymbol{z}, \boldsymbol{u} \mid \boldsymbol{x}, \boldsymbol{y})}_{\text {似然 }} \underbrace{P(\boldsymbol{x}, \boldsymbol{y})}_{\text {先捡 }} .$$

贝叶斯法则左侧称为后验概率，右侧的 $P(\boldsymbol{z}\mid \boldsymbol{x})$ 称为似然（Likehood），另一部分 $P(\boldsymbol x)$ 称为先验（Prior）

直接求后验分布是困难的，但是求一个状态最优估计，使得该状态下后验概率最大化，则是可行的：

$$(\boldsymbol{x}, \boldsymbol{y})^{*}{ }_{\text {MAP }}=\arg \max P(\boldsymbol{x}, \boldsymbol{y} \mid \boldsymbol{z}, \boldsymbol{u})=\arg \max P(\boldsymbol{z}, \boldsymbol{u} \mid \boldsymbol{x}, \boldsymbol{y}) P(\boldsymbol{x}, \boldsymbol{y})$$

请注意贝叶斯法则的分母部分与待估计的状态 $x, y$ 无关，因而可以忽略。

贝叶斯法则告诉我们，求解最大后验概率等价于最大化似然和先验的乘积。

如果不知道机器人位姿或路标大概在什么地方，此时就没有了先验，那么可以求解最大似然估计（Maximize Likelihood Estimation，MLF）：

$$(\boldsymbol{x}, \boldsymbol{y})^{*}{ }_{\text {MLE }}=\arg \max P(\boldsymbol{z}, \boldsymbol{u} \mid \boldsymbol{x}, \boldsymbol{y})$$

直观地讲，似然是指”在现在的位姿下，可能产生怎样的观测数据”。

由于我们知道观测数据，所以最大似然估计可以理解成：“在什么样的状态下，最可能产生现在观测到的数据”。这就是最大似然估计的直观意义。

最小二乘的引出

求最大似然估计，在高斯分布的假设下，最大似然有较简单的形式

对于某一次观测：

$$\boldsymbol{z}_{k, j}=h\left(\boldsymbol{y}_{j}, \boldsymbol{x}_{k}\right)+\boldsymbol{v}_{k, j}$$

由于假设了噪声项 $\boldsymbol{v}_{k} \sim \mathcal{N}\left(\mathbf{0}, \boldsymbol{Q}_{k, j}\right)$，所以观测数据的条件概率为：

$$P\left(\boldsymbol{z}_{j, k} \mid \boldsymbol{x}_{k}, \boldsymbol{y}_{j}\right)=N\left(h\left(\boldsymbol{y}_{j}, \boldsymbol{x}_{k}\right), \boldsymbol{Q}_{k, j}\right)$$

它依然是一个高斯分布。考虑单次观测的最大似然估计，可以使用最小化负对数来求一个高斯分布的最大似然。

高斯分布在负对数下有较好的数学形式。考虑任意高维高斯分布 $\boldsymbol{x} \sim \mathcal{N}(\boldsymbol{\mu}, \boldsymbol{\Sigma})$，它的概率密度函数展开形式为：

$$P(\boldsymbol{x})=\frac{1}{\sqrt{(2 \pi)^{N} \operatorname{det}(\boldsymbol{\Sigma})}} \exp \left(-\frac{1}{2}(\boldsymbol{x}-\boldsymbol{\mu})^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}(\boldsymbol{x}-\boldsymbol{\mu})\right) \text {. }$$

对其取负对数，则变为：

$$-\ln (P(\boldsymbol{x}))=\frac{1}{2} \ln \left((2 \pi)^{N} \operatorname{det}(\boldsymbol{\Sigma})\right)+\frac{1}{2}(\boldsymbol{x}-\boldsymbol{\mu})^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}(\boldsymbol{x}-\boldsymbol{\mu}) .$$

因为对数函数是单调递增的，所以对原函数求最大化相当于对负对数求最小化。

在最小化上式的 $x$ 时，第一项与 $x$ 无关，可以略去。于是，只要最小化右侧的二次型项，就得到了对状态的最大似然估计。

代入SLAM的观测模型，相当于在求：

$$\begin{aligned} \left(\boldsymbol{x}_{k}, \boldsymbol{y}_{j}\right)^{*} &=\arg \max \mathcal{N}\left(h\left(\boldsymbol{y}_{j}, \boldsymbol{x}_{k}\right), \boldsymbol{Q}_{k, j}\right) \\ &=\arg \min \left(\left(\boldsymbol{z}_{k, j}-h\left(\boldsymbol{x}_{k}, \boldsymbol{y}_{j}\right)\right)^{\mathrm{T}} \boldsymbol{Q}_{k, j}^{-1}\left(\boldsymbol{z}_{k, j}-h\left(\boldsymbol{x}_{k}, \boldsymbol{y}_{j}\right)\right)\right) . \end{aligned}$$

该式等价于最小化噪声项（即误差）的一个二次型。这个二次型称为马哈拉诺比斯距离（Mahalanobis distance），又叫马氏距离。

它也可以看成由 $\boldsymbol{Q}_{k, j}^{-1}$ 加权之后的欧氏距离（二范数），这里 $\boldsymbol{Q}_{k, j}^{-1}$ 也叫作信息矩阵，即高斯分布协方差矩阵之逆。

现在考虑批量时刻的数据。

通常假设各个时刻的输入和观测是相互独立的，这意味着各个输入之间是独立的，各个观测之间是独立的、并且输入和观测也是独立的。

于是可以对联合分布进行因式分解：

$$P(\boldsymbol{z}, \boldsymbol{u} \mid \boldsymbol{x}, \boldsymbol{y})=\prod_{k} P\left(\boldsymbol{u}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{x}_{k}\right) \prod_{k, j} P\left(\boldsymbol{z}_{k, j} \mid \boldsymbol{x}_{k}, \boldsymbol{y}_{j}\right)$$

这说明我们可以独立地处理各时刻的运动和观测。定义各次输入和观测数据与模型之间的误差：

$$\begin{aligned} \boldsymbol{e}_{\boldsymbol{u}, k} &=\boldsymbol{x}_{k}-f\left(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}\right) \\ \boldsymbol{e}_{\boldsymbol{z}, j, k} &=\boldsymbol{z}_{k, j}-h\left(\boldsymbol{x}_{k}, \boldsymbol{y}_{j}\right) \end{aligned}$$

那么，最小化所有时刻估计值与真实读数之间的马氏距离，等价于求最大似然估计。负对数允许我们把乘积变成求和：

$$\min J(\boldsymbol{x}, \boldsymbol{y})=\sum_{k} e_{\boldsymbol{u}, k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} e_{u, k}+\sum_{k} \sum_{j} e_{\boldsymbol{z}, k, j}^{\mathrm{T}} \boldsymbol{Q}_{k, j}^{-1} e_{\boldsymbol{z}, k, j}$$

这样就得到了一个最小二乘问题（Least Square Problem），它的解等价于状态的最大似然估计。

直观上看，由于噪声的存在，当我们把估计的轨迹与地图代入SLAM的运动、观测方程中时，它们并不会完美地成立。这时我们对状态的估计值进行微调，使得整体的误差下降一些。当然，这个下降也有限度，它一般会到达一个极小值。这就是一个典型的非线性优化的过程。

仔细观察上式，SLAM中的最小二乘问题具有一些特定的结构：

• 首先，整个问题的目标函数由许多个误差的（加权的）二次型组成。

  虽然总体的状态变量维数很高，但每个误差项都是简单的，仅与一两个状态变量有关。

  例如，运动误差只与 $x_{k-1}, x_k$ 有关，观测误差只与 $x_{k}, y_j$ 有关。这种关系会让整个问题有一种**稀疏**的形式。

• 其次，如果使用李代数表示增量，则该问题是无约束的最小二乘问题。但如果用旋转矩阵/变换矩阵描述位姿，则会引入旋转矩阵自身的约束．即需在问题中加入 $\text { s.t. } \boldsymbol R^{\mathrm{T}} \boldsymbol{R}=\boldsymbol I$ 且 $\mathrm {det}(\boldsymbol R)= 1$ 这样令人头大的条件。

  额外的约束会使优化变得更困难。这体现了李代数的优势。

• 最后，使用了二次型度量误差。误差的分布将影响此项在整个问题中的权重。

  例如，某次的观测非常准确，那么协方差矩阵就会“小”，而信息矩阵就会“大”，所以这个误差项会在整个问题中占有较高的权重。

求解这个最小二乘问题

例子：批量状态估计

考虑一个非常简单的离散时间系统：

$$\begin{array}{ll} \boldsymbol{x}_{k}=\boldsymbol{x}_{k-1}+\boldsymbol{u}_{k}+\boldsymbol{w}_{k}, & \boldsymbol{w}_{k} \sim \mathcal{N}\left(0, \boldsymbol{Q}_{k}\right) \\ \boldsymbol{z}_{k}=\boldsymbol{x}_{k}+\boldsymbol{n}_{k}, & \boldsymbol{n}_{k} \sim \mathcal{N}\left(0, \boldsymbol{R}_{k}\right) \end{array}$$

这可以表达一辆沿 $x$ 轴前进或后退的汽车。

第一个公式为运动方程，$u_k$ 为输入，$w_k$ 为噪声；

第二个公式为观测方程，$z_k$ 为对汽车位置的测量。

取时间 $k = 1,\dots,3$，现希望根据已有的 $u, z$ 进行状态估计。

设初始状态 $x_0$ 已知。下面来推导批量状态的最大似然估计。

首先，令批量状态变量为 $\boldsymbol{x}=\left[\boldsymbol{x}_{0}, \boldsymbol{x}_{1}, \boldsymbol{x}_{2}, \boldsymbol{x}_{3}\right]^{\mathrm{T}}$，令批量观测为 $\boldsymbol{z}=\left[\boldsymbol{z}_{1}, \boldsymbol{z}_{2}, \boldsymbol{z}_{3}\right]^{\mathrm{T}}$，按同样方式定义 $\boldsymbol{u}=\left[\boldsymbol{u}_{1}, \boldsymbol{u}_{2}, \boldsymbol{u}_{3}\right]^{\mathrm{T}}$。

按照先前的推导，最大似然估计为：

$$\begin{aligned} \boldsymbol{x}_{\text {map }}^{*} &=\arg \max P(\boldsymbol{x} \mid \boldsymbol{u}, \boldsymbol{z})=\arg \max P(\boldsymbol{u}, \boldsymbol{z} \mid \boldsymbol{x}) \\ &=\prod_{k=1}^{3} P\left(\boldsymbol{u}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{x}_{k}\right) \prod_{k=1}^{3} P\left(\boldsymbol{z}_{k} \mid \boldsymbol{x}_{k}\right), \end{aligned}$$

对于具体每一项，比如运动方程：

$$P\left(\boldsymbol{u}_{k} \mid \boldsymbol{x}_{k-1}, \boldsymbol{x}_{k}\right)=\mathcal{N}\left(\boldsymbol{x}_{k}-\boldsymbol{x}_{k-1}, \boldsymbol{Q}_{k}\right)$$

观测方程也是类似的：

$$P\left(\boldsymbol{z}_{k} \mid \boldsymbol{x}_{k}\right)=\mathcal{N}\left(\boldsymbol{x}_{k}, \boldsymbol{R}_{k}\right)$$

构建误差变量：

$$\boldsymbol{e}_{\boldsymbol{u}, k}=\boldsymbol{x}_{k}-\boldsymbol{x}_{k-1}-\boldsymbol{u}_{k}, \quad \boldsymbol{e}_{z, k}=\boldsymbol{z}_{k}-\boldsymbol{x}_{k}$$

于是最小二乘的目标函数为：

$$\min \sum_{k=1}^{3} \boldsymbol{e}_{\boldsymbol{u}, k}^{\mathrm{T}} \boldsymbol{Q}_{k}^{-1} \boldsymbol{e}_{\boldsymbol{u}, k}+\sum_{k=1}^{3} \boldsymbol{e}_{\boldsymbol{z}, k}^{\mathrm{T}} \boldsymbol{R}_{k}^{-1} \boldsymbol{e}_{z, k}$$

此外，这个系统是线性系统，写成向量形式。

定义向量 $\boldsymbol{y}=\left[\boldsymbol{u}, \boldsymbol{z}\right]^{\mathrm{T}}$，写出矩阵 $\boldsymbol H$，使得

$$\boldsymbol{y}-\boldsymbol{H} \boldsymbol{x}=e \sim \mathcal{N}(\mathbf{0}, \boldsymbol{\Sigma})$$

那么：

$$\boldsymbol{H}=\left[\begin{array}{cccc} 1 & -1 & 0 & 0 \\ 0 & 1 & -1 & 0 \\ 0 & 0 & 1 & -1 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right],$$

且 $\boldsymbol{\Sigma}=\operatorname{diag}\left(Q_{1}, \boldsymbol{Q}_{2}, \boldsymbol{Q}_{3}, \boldsymbol{R}_{1}, \boldsymbol{R}_{2}, \boldsymbol{R}_{3}\right)$。整个问题可以写成：

$$x_{\operatorname{map}}^{*}=\arg \min e^{\mathrm{T}} \mathbf{\Sigma}^{-1} e$$

这个问题有唯一解：

$$\boldsymbol{x}_{\text {map }}^{*}=\left(\boldsymbol{H}^{\mathrm{T}} \boldsymbol{\Sigma}^{-1} \boldsymbol{H}\right)^{-1} \boldsymbol{H}^{\mathrm{T}} \boldsymbol{\Sigma}^{-1} \boldsymbol{y} .$$

非线性最小二乘

考虑一个简单的最小二乘问题：

$$\min _{x} F(\boldsymbol{x})=\frac{1}{2}\|f(\boldsymbol{x})\|_{2^{2}}^{2}$$

其中，自变量 $\boldsymbol x \in \mathbb R^n$，$f$ 是任意标量非线性函数 $f( \boldsymbol x):\mathbb R^n \mapsto \mathbb R$。注意这里的系数 $\frac{1}{2}$ 是无关紧要的，有些文献上带有这个系数，有些文献则不带，它不会影响之后的结论。

下面讨论如何求解这样一个优化问题。显然，如果 $f$ 是个数学形式上很简单的函数，那么该问题可以用解析形式来求。令目标函数的导数为零，然后求解 $\boldsymbol x$ 的最优值，就和求二元函数的极值一样：

$$\frac{\mathrm{d} F}{\mathrm{~d} \boldsymbol{x}}=\mathbf{0}$$

解此方程，就得到了导数为零处的极值。它们可能是极大、极小或鞍点处的值，只要逐个比较它们的函数值大小即可。

如果 $f$ 为简单的线性函数，那么这个问题就是简单的线性最小二乘问题，但是有些导函数可能形式复杂，使得该方程可能不容易求解。求解这个方程需要我们知道关于目标函数的全局性质，而通常这是不大可能的。

对于不方便直接求解的最小二乘问题，可以用迭代的方式，从一个初始值出发，不断地更新当前的优化变量,使目标函数下降。具体步骤可列写如下：

1. 给定某个初始值 $\boldsymbol x_0$
2. 对于第 $k$ 次迭代，寻找一个增量 $\Delta x_k$， 使得 $\left\|f\left(\boldsymbol x_{k}+\Delta \boldsymbol x_{k}\right)\right\|_{2}^{2}$ 达到极小值
3. 若 $\Delta \boldsymbol x_k$ 足够小，则停止
4. 否则，令 $\boldsymbol x_{k+1}= \boldsymbol x_k+\Delta \boldsymbol x_k$，返回第二步

这让求解导函数为零的问题变成了一个不断寻找下降增量 $\Delta x_k$ 的问题

由于可以对 $f$ 进行线性化，增量的计算将简单很多。当函数下降直到增量非常小的时候，就认为算法收敛，目标函数达到了一个极小值。

一阶和二阶梯度法

考虑第k次迭代，假设在 $x_k$ 处，想要寻到 $\Delta x_k$，最直观方式将目标函数在 $x_k$ 附近进行泰勒展开：

$$F\left(\boldsymbol{x}_{k}+\Delta \boldsymbol{x}_{k}\right) \approx F\left(\boldsymbol{x}_{k}\right)+\boldsymbol{J}\left(\boldsymbol{x}_{k}\right)^{\mathrm{T}} \Delta \boldsymbol{x}_{k}+\frac{1}{2} \Delta \boldsymbol{x}_{k}^{\mathrm{T}} \boldsymbol{H}\left(\boldsymbol{x}_{k}\right) \Delta \boldsymbol{x}_{k} .$$

其中 $\boldsymbol{J}\left(\boldsymbol{x}_{k}\right)^{\mathrm{T}}$ 是 $F(\boldsymbol x)$ 关于 $\boldsymbol x$ 的一阶导数〔也叫梯度、雅可比（Jacobian）矩阵〕，$\boldsymbol H$ 则是二阶导数〔海塞（Hessian）矩阵〕，它们都在 $\boldsymbol x_k$ 处取值

选择保留泰勒展开的一阶或二阶项，那么对应的求解方法则称为一阶梯度或二阶梯度法。

如果保留一阶梯度，那么取增量为反向的梯度．即可保证函数下降：

$$\Delta \boldsymbol{x}^{*}=-\boldsymbol{J}\left(\boldsymbol{x}_{k}\right)$$

当然这只是个方向，通常还要再指定一个步长 $\lambda$。步长可以根据一定的条件来计算，这种方法被称为最速下降法。

它的直观意义非常简单，只要沿着反向梯度方向前进，在一阶（线性）的近似下，目标函数必定会下降。

另外可旋转保留二阶梯度信息，此时增量方程为：

$$\Delta \boldsymbol{x}^{*}=\arg \min \left(F(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}+\frac{1}{2} \Delta \boldsymbol{x}^{\mathrm{T}} \boldsymbol{H} \Delta \boldsymbol{x}\right)$$

右侧只含 $\Delta \boldsymbol x$ 的零次、一次和二次项

求右侧等式关于 $\Delta \boldsymbol x$ 的导数并令它为零，得到：

$$\boldsymbol J+\boldsymbol H \Delta \boldsymbol x=0 \Rightarrow \boldsymbol H \Delta \boldsymbol x=-\boldsymbol J .$$

求解这个线性方程，就得到了增量。该方法又称为牛顿法。

一阶和二阶梯度法都十分直观，只要把函数在迭代点附近进行泰勒展开，并针对更新量做最小化即可。

事实上，我们用一个一次或二次的函数近似了原函数，然后用近似函数的最小值来猜测原函数的极小值。只要原目标函数局部看起来像一次或二次函数，这类算法就是成立的（这也是现实中的情形）。

不过，这两种方法也存在它们自身的问题。最速下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数。而牛顿法则需要计算目标函数的 $\boldsymbol H$ 矩阵，这在问题规模较大时非常困难，我们通常倾向于避免 $\boldsymbol H$ 的计算。

对于一般的问题，一些拟牛顿法可以得到较好的结果，而对于最小二乘问题，还有几类更实用的方法：高斯牛顿法和列文伯格—马夸尔特方法。

高斯牛顿法

高斯牛顿法是最优化算法中最简单的方法之一。

它的思想是将 $f(\boldsymbol x)$ 进行一阶的泰勒展开。

注：这里不是目标函数 $F(\boldsymbol x)$ 而是 $f(\boldsymbol x)$，否则就变成牛顿法了。

$$f(\boldsymbol{x}+\Delta \boldsymbol{x}) \approx f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}$$

这里 $\boldsymbol{J}\left(\boldsymbol{x}_{k}\right)^{\mathrm{T}}$ 为 $f(\boldsymbol x)$ 关于 $\boldsymbol x$ 的导数，为 $n \times 1$ 的列向量

目标是寻找增量 $\Delta \boldsymbol x$，使得 $\|f(\boldsymbol{x}+\Delta \boldsymbol{x})\|^{2}$ 达到最小

$$\Delta \boldsymbol{x}^{*}=\arg \min _{\Delta \boldsymbol{x}} \frac{1}{2}\left\|f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}\right\|^{2} .$$

根据极值条件，将上述目标函数对 $\Delta \boldsymbol x$ 求导，并令导数为零

先展开目标函数的平方项：

$$\begin{aligned} \frac{1}{2}\left\|f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}\right\|^{2} &=\frac{1}{2}\left(f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}\right)^{\mathrm{T}}\left(f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}\right) \\ &=\frac{1}{2}\left(\|f(\boldsymbol{x})\|_{2}^{2}+2 f(\boldsymbol{x}) \boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}+\Delta \boldsymbol{x}^{\mathrm{T}} \boldsymbol{J}(\boldsymbol{x}) \boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}\right) . \end{aligned}$$

求上式关于 $\Delta \boldsymbol x$ 的导数，并令其为零：

$$\boldsymbol{J}(\boldsymbol{x}) f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x}) \boldsymbol{J}^{\mathrm{T}}(\boldsymbol{x}) \Delta \boldsymbol{x}=\mathbf{0}$$

得到如下方程组：

$$\underbrace{\boldsymbol{J}(\boldsymbol{x}) \boldsymbol{J}^{\mathrm{T}}}_{\boldsymbol{H}(\boldsymbol{x})}(\boldsymbol{x}) \Delta \boldsymbol{x}=\underbrace{-\boldsymbol{J}(\boldsymbol{x}) f(\boldsymbol{x})}_{\boldsymbol{g}(\boldsymbol{x})} .$$

这个方程是关于变量 $\Delta \boldsymbol x$ 的线性方程组，称它为增量方程，也可以称为高斯牛顿方程（Gauss-Newton equation）或者正规方程（Normal equation）。

把左边的系数定义为 $\boldsymbol H$，右边定义为 $\boldsymbol g$，那么上式变为：

$$\boldsymbol{H} \Delta \boldsymbol{x}=\boldsymbol{g} .$$

这里把左侧记作 $\boldsymbol H$ 是有意义的。对比牛顿法可见,高斯牛顿法用 $\boldsymbol {JJ}^T$ 作为牛顿法中二阶Hessian矩阵的近似，从而省略了计算 $\boldsymbol H$ 的过程。

求解增量方程是整个优化问题的核心所在。

高斯牛顿法的算法步骤可以写成：

1. 给定初始值 $\boldsymbol x_0$
2. 对于第 $k$ 次迭代，求出当前的雅可比矩阵 $\boldsymbol J(\boldsymbol x _k)$ 和误差 $f(\boldsymbol x_k)$
3. 求解增量方程：$\boldsymbol{H} \Delta \boldsymbol{x}_k=\boldsymbol{g}$
4. 若 $\Delta \boldsymbol{x}_k$ 足够小，则停止。否则，令 $\boldsymbol x_{k+1}=\boldsymbol x_k +\Delta \boldsymbol x_k$，返回第2步。

为了求解增量方程，需要求解 $\boldsymbol H^{-1}$，这需要 $\boldsymbol H$ 矩阵可逆，但实际数据中计算得到的 $\boldsymbol {JJ}^T$ 却只有半正定性。

i.e.，在使用高斯牛顿法时，可能出现 $\boldsymbol {JJ}^T$ 为奇异矩阵或者病态（ill-condition）的情况，此时增量的稳定性较差，导致算法不收敛。

直观地说，原函数在这个点的局部近似不像一个二次函数。

更严重的是，就算假设 $\boldsymbol H$ 非奇异也非病态，如果求出来的步长 $\Delta \boldsymbol{x}$ 太大，也会导致采用的局部近似式 $f(\boldsymbol{x}+\Delta \boldsymbol{x}) \approx f(\boldsymbol{x})+\boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}$ 不够准确，这样一来甚至无法保证它的迭代收敛，哪怕是让目标函数变得更大都是有可能的。

尽管高斯牛顿法有这些缺点，但它依然算是非线性优化方面一种简单有效的方法。

在非线性优化领域，相当多的算法都可以归结为高斯牛顿法的变种。这些算法都借助了高斯牛顿法的思想并且通过自己的改进修正其缺点。

例如，一些线搜索方法（Line Search Method） 加入了一个步长 $\alpha$，在确定了 $\Delta \boldsymbol{x}$ 后进一步找到 $\alpha$ 使得 $\|f(\boldsymbol{x}+\alpha \Delta \boldsymbol{x})\|^{2}$ 达到最小，而不是简单地令 $\alpha=1$。

列文伯格—马夸尔特方法在一定程度上修正了这些问题。一般认为它比高斯牛顿法更为健壮，但它的收敛速度可能比高斯牛顿法更慢，被称为阻尼牛顿法（Damped Newton Method）。

列文伯格—马夸尔特方法

高斯牛顿法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，所以很自然地想到应该给 $\Delta \boldsymbol{x}$ 添加一个范围，称为信赖区域（Trust Region）。

这个范围定义了在什么情况下二阶近似是有效的，这类方法也称为信赖区域方法（Trust Region Method）。

在信赖区域里，认为近似是有效的；出了这个区域，近似可能会出问题。

根据我们的近似模型跟实际函数之间的差异来确定信赖区域的范围：如果差异小，说明近似效果好，扩大近似的范围；反之，如果差异大，就缩小近似的范围。

定义一个指标 $\rho$ 来刻画近似的好坏程度：

$$\rho=\frac{f(\boldsymbol{x}+\Delta \boldsymbol{x})-f(\boldsymbol{x})}{\boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}} .$$

$\rho$ 的分子是实际函数下降的值，分母是近似模型下降的值。

如果 $\rho$ 接近于 $1$，则近似是好的。如果 $\rho$ 太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，如果 $\rho$ 比较大，则说明实际下降的比预计的更大，可以放大近似范围。

于是，构建一个改良版的非线性优化框架，该框架会比高斯牛顿法有更好的效果：

1. 给定初始值 $\boldsymbol x_0$，以及初始优化半径 $\mu$

2. 对于第 $k$ 次迭代，在高斯牛顿法的基础上加上信赖区域，求解：

   $$\min _{\Delta \boldsymbol{x}_{k}} \frac{1}{2}\left\|f\left(\boldsymbol{x}_{k}\right)+\boldsymbol{J}\left(\boldsymbol{x}_{k}\right)^{\mathrm{T}} \Delta \boldsymbol{x}_{k}\right\|^{2}, \quad \text { s.t. } \quad\left\|\boldsymbol{D} \Delta \boldsymbol{x}_{k}\right\|^{2} \leqslant \mu \text {, }$$

   其中，$\mu$ 是信赖区域的半径，$\boldsymbol D$ 为系数矩阵

3. 按式 $\rho=\frac{f(\boldsymbol{x}+\Delta \boldsymbol{x})-f(\boldsymbol{x})}{\boldsymbol{J}(\boldsymbol{x})^{\mathrm{T}} \Delta \boldsymbol{x}}$ 计算 $\rho$

4. 若 $p > \frac{3}{4}$，则设置 $\mu = 2\mu$

5. 若 $p<\frac{1}{4}$，则设置 $\mu = 0.5\mu$

6. 如果 $\rho$ 大于某阈值，则认为近似可行。令 $\boldsymbol x_{k+1}=\boldsymbol x_k +\Delta \boldsymbol x_k$

7. 判断算法是否收敛。如不收敛则返回第2步，否则结束

这里近似范围扩大的倍数和阈值都是经验值，可以替换成别的数值。

在第2步式子中，把增量限定于一个半径为 $\mu$ 的球中，认为只在这个球内才是有效的。带上 $\boldsymbol D$ 之后，这个球可以看成一个椭球。

在列文伯格提出的优化方法中，把 $\boldsymbol D$ 取成单位阵 $\boldsymbol I$，相当于直接把 $\Delta \boldsymbol{x}_k$ 约束在一个球中。

随后，马夸尔特提出将 $\boldsymbol D$ 取成非负数对角阵——实际中通常用 $\boldsymbol {JJ}^T$ 的对角元素平方根，使得在梯度小的维度上约束范围更大一些。

在列文伯格—马夸尔特优化中，都需要解第2步式子那样一个子问题来获得梯度。

这个子问题是带不等式约束的优化问题，用拉格朗日乘子把约束项放到目标函数中，构成拉格朗日函数：

$$\mathcal{L}\left(\Delta \boldsymbol{x}_{k}, \lambda\right)=\frac{1}{2}\left\|f\left(\boldsymbol{x}_{k}\right)+\boldsymbol{J}\left(\boldsymbol{x}_{k}\right)^{\mathrm{T}} \Delta \boldsymbol{x}_{k}\right\|^{2}+\frac{\lambda}{2}\left(\left\|\boldsymbol{D} \Delta \boldsymbol{x}_{k}\right\|^{2}-\mu\right)$$

这里 $\lambda$ 为拉格朗日乘子。类似于高斯牛顿法中的做法，令该拉格朗日函数关于 $\Delta \boldsymbol{x}$ 的导数为零，它的核心仍是计算增量的线性方程：

$$\left(\boldsymbol{H}+\lambda \boldsymbol{D}^{\mathrm{T}} \boldsymbol{D}\right) \Delta \boldsymbol{x}_{k}=\boldsymbol{g} .$$

可以看到，相比于高斯牛顿法，增量方程多了一项 $\lambda \boldsymbol{D}^{\mathrm{T}} \boldsymbol{D}$。如果考虑它的简化形式，即 $\boldsymbol D=\boldsymbol I$，那么相当于求解：

$$(\boldsymbol{H}+\lambda \boldsymbol{I}) \Delta \boldsymbol{x}_{k}=\boldsymbol{g}$$

当参数 $\lambda$ 比较小时，$\boldsymbol H$ 占主要地位，这说明二次近似模型在该范围内是比较好的，列文伯格—马夸尔特方法更接近于高斯牛顿法。

当 $\lambda$ 比较大时，$\lambda\boldsymbol I$ 占据主要地位，列文伯格—马夸尔特方法更接近于一阶梯度下降法（即最速下降），这说明附近的二次近似不够好。

列文伯格—马夸尔特方法的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定、更准确的增量 $\Delta \boldsymbol{x}$ 。

在实际中，还存在许多其他的方式来求解增量，例如 Dog-Leg 等方法。这里所介绍的，只是最常见而且最基本的方法，也是视觉SLAM中用得最多的方法。实际问题中，通常选择高斯牛顿法或列文伯格—马夸尔特方法中的一种作为梯度下降策略。当问题性质较好时，用高斯牛顿。如果问题接近病态，则用列文伯格—马夸尔特方法。

无论是高斯牛顿法还是列文伯格—马夸尔特方法，在做最优化计算时，都需要提供变量的初始值。这个初始值不能随意设置。

实际上，非线性优化的所有迭代求解方案，都需要用户提供一个良好的初始值。由于目标函数太复杂，导致在求解空间上的变化难以预测，对问题提供不同的初始值往往会导致不同的计算结果。

这种情况是非线性优化的通病：大多数算法都容易陷入局部极小值。因此，无论是哪类科学问题，提供初始值都应该有科学依据，例如视觉SLAM问题中，会用 ICP、PnP 之类的算法提供优化初始值。总之，一个良好的初始值对最优化问题非常重要！