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

前6讲（数学基础）知识笔记见 视觉SLAM十四讲从理论到实践第2版学习笔记(数学基础)

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

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

第二部分 实践应用

第7讲 视觉里程计 1

视觉里程计常用的方法：特征点法和光流法

特征点法

SLAM系统分为前端和后端，其中前端也称为视觉里程计。视觉里程计根据相邻图像的信息估计出粗略的相机运动，给后端提供较好的初始值。

视觉里程计的算法主要分为两个大类：特征点法和直接法。

基于特征点法的前端，被认为是视觉里程计的主流方法。它具有稳定，对光照、动态物体不敏感的优势，是目前比较成熟的解决方案。

特征点

视觉里程计的核心问题是如何根据图像估计相机运动。

然而，图像本身是一个由亮度和色彩组成的矩阵，如果直接从矩阵层面考虑运动估计，将会非常困难。

所以，比较方便的做法是：

首先，从图像中选取比较有代表性的点。这些点在相机视角发生少量变化后会保持不变，于是能在各个图像中找到相同的点。然后，在这些点的基础上，讨论相机位姿估计问题，以及这些点的定位问题。在经典SLAM模型中，称这些点为路标。而在视觉SLAM中，路标则是指图像特征（Feature）。

特征是图像信息的另一种数字表达形式。一组好的特征对在指定任务上的最终表现至关重要。

数字图像在计算机中以灰度值矩阵的方式存储，单个图像像素也是一种“特征”。

但是，在视觉里程计中，希望特征点在相机运动之后保持稳定，而灰度值受光照、形变、物体材质的影响严重，在不同图像间变化非常大，不够稳定。

所以，仅凭灰度值是不够的，需要对图像提取特征点。

特征点是图像里一些特别的地方

以上图为例，可以把图像中的角点、边缘和区块都当成图像中有代表性的地方。图像中的角点、边缘相比于像素区块而言更加“特别”，在不同图像之间的辨识度更强。

所以，一种直观的提取特征的方式就是在不同图像间辨认角点，确定它们的对应关系。在这种做法中，角点就是所谓的特征。角点的提取算法有很多，例如Harris角点、FAST角点、GFTT角点，等等。它们大部分是2000年以前提出的算法。

然而，在大多数应用中，单纯的角点依然不能满足我们的很多需求。例如，从远处看上去是角点的地方，当相机离近之后，可能就不显示为角点了。或者，当旋转相机时，角点的外观会发生变化，也就不容易辨认出那是同一个角点了。

为此，计算机视觉领域的研究者们在长年的研究中设计了许多更加稳定的局部图像特征，如著名的SIFT、SURF、ORB，等等。相比于朴素的角点，这些人工设计的特征点能够拥有如下性质：

1. 可重复性（Repeatability）：相同的特征可以在不同的图像中找到。
2. 可区别性（Distinctiveness）：不同的特征有不同的表达。
3. 高效率（Efficiency）：同一图像中，特征点的数量应远小于像素的数量。
4. 本地性（Locality）：特征仅与一小片图像区域相关。

特征点由关键点（Key-point） 和描述子（Descriptor） 两部分组成。

关键点是指该特征点在图像里的位置，有些特征点还具有朝向、大小等信息。

描述子通常是一个向量，按照某种人为设计的方式，描述了该关键点周围像素的信息。

描述子是按照“外观相似的特征应该有相似的描述子”的原则设计的。因此，只要两个特征点的描述子在向量空间上的距离相近，就可以认为它们是同样的特征点。

SIFT（尺度不变特征变换，Scale-Invariant FeatureTransform）：图像特征有些很精确，在相机的运动和光照变化下仍具有相似的表达，但相应地计算量较大。

它充分考虑了在图像变换过程中出现的光照、尺度、旋转等变化，但随之而来的是极大的计算量。由于整个SLAM过程中图像特征的提取与匹配仅仅是诸多环节中的一个，普通计算机的CPU还无法实时地计算SIFT特征，进行定位与建图，所以在SLAM中很少使用这种“奢侈”的图像特征。

FAST关键点属于计算特别快的一种特征点（注意，这里“关键点”的表述，说明它没有描述子），适当降低精度和鲁棒性，以提升计算的速度。

ORB（Oriented FAST and Rotated BRIEF）特征则是目前看来非常具有代表性的实时图像特征。

它改进了FAST检测子不具有方向性的问题，并采用速度极快的二进制描述子BRIEF（Binary Robust Independent Elementary Feature），使整个图像特征提取的环节大大加速。

ORB在保持了特征子具有旋转、尺度不变性的同时，在速度方面提升明显，对于实时性要求很高的SLAM来说是一个很好的选择。

ORB特征

ORB特征由关键点和描述子两部分组成。

它的关键点称为“Oriented FAST”，是一种改进的FAST角点。

它的描述子称为BRIEF。

因此，提取ORB特征分为如下两个步骤：

1. FAST角点提取：找出图像中的“角点”。相较于原版的FAST，ORB 中计算了特征点的主方向，为后续的BRIEF描述子增加了旋转不变特性。
2. BRIEF描述子：对前一步提取出特征点的周围图像区域进行描述。ORB对BRIEF进行了一些改进，主要是指在BRIEF中使用了先前计算的方向信息。

FAST关键点

FAST是一种角点，主要检测局部像素灰度变化明显的地方，以速度快著称。

它的思想是：如果一个像素与邻域的像素差别较大（过亮或过暗），那么它更可能是角点。

相比于其他角点检测算法，FAST只需比较像素亮度的大小，十分快捷。它的检测过程如下：

1. 在图像中选取像素 $p$，假设它的亮度为 $I_p$
2. 设置一个阈值 $T$（比如，$I_p$ 的20%)
3. 以像素 $p$ 为中心，选取半径为 $3$ 的圆上的 $16$ 个像素点
4. 假如选取的圆上有连续的 $N$ 个点的亮度大于 $I_p+T$ 或小于 $I_p-T$，那么像素 $p$ 可以被认为是特征点（$N$通常取 $12$，即 FAST-12。其他常用的 $N$ 取值为 $9$ 和$11$，分别被称为 FAST-9 和 FAST-11）
5. 循环以上四步，对每一个像素执行相同的操作

在FAST-12算法中，为了更高效，可以添加一项预测试操作，以快速地排除绝大多数不是角点的像素。

具体操作为，对于每个像素，直接检测邻域圆上的第 $1,5,9,13$ 个像素的亮度。只有当这 $4$ 个像素中有 $3$ 个同时大于 $I_p+T$ 或小于 $I_p-T$ 时，当前像素才有可能是一个角点，否则应该直接排除。这样的预测试操作大大加速了角点检测。

此外，原始的FAST角点经常出现“扎堆”的现象。所以在第一遍检测之后，还需要用非极大值抑制（Non-maximal suppression），在一定区域内仅保留响应极大值的角点，避免角点集中的问题。

FAST特征点的计算仅仅是比较像素间亮度的差异，所以速度非常快

缺点：重复性不强、分布不均匀、FAST角点不具有方向信息、由于它固定取半径为3的圆，存在尺度问题：远处看着像是角点的地方，接近后看可能就不是角点。

针对FAST角点不具有方向性和尺度的弱点，ORB添加了尺度和旋转的描述。

尺度不变性由构建图像金字塔（对图像进行不同层次的降采样，以获得不同分辨率的图像），并在金字塔的每一层上检测角点来实现。

金字塔是计算图视觉中常用的一种处理方法。金字塔底层是原始图像。每往上一层，就对图像进行一个固定倍率的缩放，这样就有了不同分辨率的图像。较小的图像可以看成是远处看过来的场景。

在特征匹配算法中，可以匹配不同层上的图像，从而实现尺度不变性。例如，如果相机在后退，那么应该能够在上一个图像金字塔的上层和下一个图像金字塔的下层中找到匹配。

而特征的旋转是由灰度质心法（Intensity Centroid）实现。

计算特征点附近的图像灰度质心。

所谓质心是指以图像块灰度值作为权重的中心。其具体操作步骤如下：

1. 在一个小的图像块 $B$ 中，定义图像块的矩为：

   $$m_{p q}=\sum_{x, y \in B} x^{p} y^{q} I(x, y), \quad p, q=\{0,1\} .$$

2. 通过矩找到图像块的质心：

   $$C=\left(\frac{m_{10}}{m_{00}}, \frac{m_{01}}{m_{00}}\right)$$

3. 连接图像块的几何中心 $O$ 与质心 $C$，得到一个方向向量 $\overrightarrow{O C}$，特征点的方向可以定义为：

   $$\theta=\arctan \left(m_{01} / m_{10}\right)$$

通过以上方法，FAST角点便具有了尺度与旋转的描述，从而大大提升了其在不同图像之间表述的鲁棒性。

所以在ORB中，把这种改进后的FAST称为Oriented FAST。

BRIEF描述子

在提取Oriented FAST关键点后，对每个点计算其描述子。ORB使用改进的BRIEF特征描述。

BRIEF是一种二进制描述子，其描述向量由许多个 $0$ 和 $1$ 组成，这里的 $0$ 和 $1$ 编码了关键点附近两个随机像素（比如 $p$ 和 $q$）的大小关系：如果 $p$ 比 $q$ 大，则取 $1$，反之就取 $0$。

如果取了 $128$ 个这样的 $p,q$，则最后得到 $128$ 维由 $0、1$ 组成的向量。

BRIEF使用了随机选点的比较，速度非常快，而且由于使用了二进制表达，存储起来也十分方便，适用于实时的图像匹配。

原始的BRIEF描述子不具有旋转不变性，因此在图像发生旋转时容易丢失。而ORB在FAST特征点提取阶段计算了关键点的方向，所以可以利用方向信息，计算旋转之后的“Steer BRIEF”特征使ORB的描述子具有较好的旋转不变性。

特征匹配

特征匹配解决了SLAM中的数据关联问题（data association），即确定当前看到的路标与之前看到的路标之间的对应关系。

通过对图像与图像或者图像与地图之间的描述子进行准确匹配，可以为后续的姿态估计、优化等操作减轻大量负担。

然而，由于图像特征的局部特性，误匹配的情况广泛存在，而且长期以来一直没有得到有效解决，目前已经成为视觉SLAM中制约性能提升的一大瓶颈。部分原因是场景中经常存在大量的重复纹理，使得特征描述非常相似。

正确匹配的情况考虑两个时刻的图像。

在图像 $I_t$ 中提取到特征点 $x_{t}^{m}, m=1,2, \ldots, M$，在图像 $I_{t+1}$ 中提取到特征点 $x_{t+1}^{n}, n=1,2, \ldots, N$

最简单的特征匹配方法就是暴力匹配（Brute-Force Matcher）

即对每一个特征点 $x_{t}^{m}$ 与所有的 $x_{t+1}^{n}$ 测量描述子的距离，然后排序，取最近的一个作为匹配点。

描述子距离表示了两个特征之间的相似程度，不过在实际运用中还可以取不同的距离度量范数。

对于浮点类型的描述子，使用欧氏距离进行度量即可。而对于二进制的描述子（比如BRIEF），往往使用汉明距离（Hamming distance）作为度量——两个二进制串之间的汉明距离，指的是其不同位数的个数。

然而，当特征点数量很大时，暴力匹配法的运算量将变得很大，特别是当想要匹配某个帧和一张地图的时候。这不符合我们在SLAM 中的实时性需求。此时，快速近似最近邻（FLANN） 算法更加适合于匹配点数量极多的情况，实现上也已集成到OpenCV。

计算相机运动

已经有了匹配好的点对，要根据点对估计相机的运动。

由于相机的原理不同，有以下几种：

1. 当相机为单目时，只知道2D的像素坐标，因而问题是根据两组2D点估计运动。

   用对极几何解决。

2. 当相机为双目、RGB-D时，或者通过某种方法得到了距离信息，问题就是根据两组3D点估计运动。

   用ICP解决。

3. 如果一组为3D，一组为2D，即，我们得到了一些3D点和它们在相机的投影位置，也能估计相机的运动。

   通过PnP求解。

2D-2D：对极几何

对极约束

通过若干对匹配点（二维图像点的对应关系），恢复出在两帧之间摄像机的运动。

两个图像中的匹配点的几何关系：

求取两帧图像 $I_1,I_2$ 之间的运动，设第一帧到第二帧的运动为 $\boldsymbol R,\boldsymbol t$

两个相机中心分别为 $O_1,O_2$

考虑 $I_1$ 中有一个特征点 $p_1$，它在 $I_2$ 中对应着特征点 $p_2$

两者是通过特征匹配得到的。如果匹配正确，说明它们确实是同一个空间点在两个成像平面上的投影。

连线 $\overrightarrow {O_1p_1}$ 和连线 $\overrightarrow {O_2p_2}$ 在三维空间中会相交于点 $P$

这时 $O_1,O_2,P$ 三个点可以确定一个平面，称为极平面（Epipolar plane）

$O_1O_2$ 连线与像平面 $I_1,I_2$ 的交点分别为 $e_1,e_2$，$e_1,e_2$ 称为极点（Epipoles），$O_1O_2$ 被称为基线。

称极平面与两个像平面 $I_1,I_2$ 之间的相交线 $l_1,l_2$ 为极线（Epipolar line）

从第一帧的角度看，射线 $\overrightarrow {O_1 p_1}$ 是某个像素可能出现的空间位置——因为该射线上的所有点都会投影到同一个像素点。

同时，如果不知道 $P$ 的位置，那么当在第二幅图像上看时，连线 $\overrightarrow {e_2 p_2}$（第二幅图像中的极线）就是 $P$ 可能出现的投影的位置，也就是射线 $\overrightarrow {O_1 p_1}$ 在第二个相机中的投影。

由于通过特征点匹配确定了 $p_2$ 的像素位置，所以能够推断 $P$ 的空间位置，以及相机的运动。

如果没有特征匹配，就没法确定 $p_2$ 到底在极线的哪个位置。那时，就必须在极线上搜索以获得正确的匹配。

从代数角度来分析这里的几何关系。

在第一帧的坐标系下，设 $P$ 的空间位置为：

$$\boldsymbol{P}=[X, Y, Z]^{\mathrm{T}}$$

根据第5讲的针孔相机模型，能知道两个像素点 $p_1,p_2$ 的像素位置为：

$$s_{1} \boldsymbol{p}_{1}=\boldsymbol{K} \boldsymbol{P}, \quad s_{2} \boldsymbol{p}_{2}=\boldsymbol{K}(\boldsymbol{R} \boldsymbol{P}+\boldsymbol{t})$$

这里 $\boldsymbol K$ 为相机内参矩阵，$\boldsymbol R,\boldsymbol t$ 为两个坐标系的相机运动。

具体来说，这里计算的是 $\boldsymbol R_{21}$ 和 $\boldsymbol t_{21}$，因为它们把第一个坐标系下的坐标转换到第二个坐标系下。也可以把它们写成李代数形式。

有时，会使用齐次坐标表示像素点。

在使用齐次坐标时，一个向量将等于它自身乘上任意的非零常数。

这通常用于表达一个投影关系。例如，$s_{1} \boldsymbol{p}_{1}$ 和 $\boldsymbol{p}_{1}$ 成投影关系，它们在齐次坐标的意义下是相等的。

称这种相等关系为尺度意义下相等（equal up to a scale），记作：

$$s \boldsymbol p \simeq \boldsymbol{p}$$

那么，上述两个投影关系可写为：

$$\boldsymbol p_{1} \simeq \boldsymbol{K P}, \quad \boldsymbol p_{2} \simeq \boldsymbol{K}(\boldsymbol{R P}+\boldsymbol{t})$$

现在，取：

$$\boldsymbol{x}_{1}=\boldsymbol{K}^{-1} \boldsymbol{p}_{1}, \quad \boldsymbol{x}_{2}=\boldsymbol{K}^{-1} \boldsymbol{p}_{2} .$$

这里的 $\boldsymbol{x}_{1},\boldsymbol{x}_{2}$ 是两个像素点的归一化平面上的坐标，代入上式，得：

$$\boldsymbol x_{2} \simeq \boldsymbol{R} \boldsymbol x_{1}+\boldsymbol t$$

两边同时左乘 $\boldsymbol{t}^{\wedge}$（相当于两侧同时与 $\boldsymbol t$ 做外积）：

$$\boldsymbol{t}^{\wedge} \boldsymbol{x}_{2} \simeq \boldsymbol{t}^{\wedge} \boldsymbol{R} \boldsymbol{x}_{1}$$

然后，两侧同时左乘 $\boldsymbol{x}_{2}^{\mathrm{T}}$：

$$\boldsymbol{x}_{2}^{\mathrm{T}} \boldsymbol{t}^{\wedge} \boldsymbol{x}_{2} \simeq \boldsymbol{x}_{2}^{\mathrm T} \boldsymbol{t}^{\wedge} \boldsymbol{R} \boldsymbol{x}_{1}$$

等式左侧，$\boldsymbol{t}^{\wedge} \boldsymbol{x}_{2}$ 是一个与 $\boldsymbol{t}$ 和 $\boldsymbol{x}_{2}$ 都垂直的向量，再和 $\boldsymbol{x}_{2}^{\mathrm{T}}$ 做内积时，得到 $0$。

由于等式左侧严格为零，乘以任意非零常数之后也为零，于是可以把 $\simeq$ 写成通常的等号，得：

$$\boldsymbol{x}_{2}^{\mathrm T} \boldsymbol{t}^{\wedge} \boldsymbol{R} \boldsymbol{x}_{1}=0$$

重新代入 $p_1,p_2$，有：

$$\boldsymbol{p}_{2}^{\mathrm T} \boldsymbol{K}^{-\mathrm{T}} \boldsymbol{t}^{\wedge} \boldsymbol{R} \boldsymbol{K}^{-1} \boldsymbol{p}_{1}=0$$

这两个式子都称为对极约束，以形式简洁著名。

它的几何意义是 $O_1,P,O_2$ 三者共面。

对极约束中同时包含了平移和旋转。

把中间部分记作两个矩阵：基础矩阵（Fundamental Matrix）$\boldsymbol F$ 和本质矩阵（Essential Matrix）$\boldsymbol E$，于是可以进一步简化对极约束：

$$\boldsymbol{E}=\boldsymbol{t}^{\wedge} \boldsymbol{R}, \quad \boldsymbol{F}=\boldsymbol{K}^{-\mathrm{T}} \boldsymbol{E} \boldsymbol{K}^{-1}, \quad \boldsymbol{x}_{2}^{\mathrm{\top}} \boldsymbol{E} \boldsymbol{x}_{1}=\boldsymbol{p}_{2}^{\mathrm{T}} \boldsymbol{F} \boldsymbol{p}_{1}=0$$

对极约束简洁地给出了两个匹配点的空间位置关系。

于是，相机位姿估计问题变为以下两步：

1. 根据配对点的像素位置求出 $\boldsymbol E$ 或者 $\boldsymbol F$
2. 根据 $\boldsymbol E$ 或者 $\boldsymbol F$ 求出 $\boldsymbol R,\boldsymbol t$

由于 $\boldsymbol E$ 和 $\boldsymbol F$ 只相差了相机内参，而内参在SLAM中通常是已知的，所以实践中往往使用形式更简单的 $\boldsymbol E$

本质矩阵

根据定义，本质矩阵 $\boldsymbol{E}=\boldsymbol{t}^{\wedge} \boldsymbol{R}$

它是一个 $3×3$ 的矩阵，内有 $9$ 个未知数。

• 本质矩阵是由对极约束定义的。由于对极约束是等式为零的约束，所以对 $\boldsymbol E$ 乘以任意非零常数后，对极约束依然满足。称为 $\boldsymbol E$ 在不同尺度下是等价的。
• 根据 $\boldsymbol{E}=\boldsymbol{t}^{\wedge} \boldsymbol{R}$，可以证明，本质矩阵 $\boldsymbol E$ 的奇异值必定是 $[\sigma, \sigma, 0]^{\mathrm T}$ 的形式。这称为本质矩阵的内在性质。
• 由于平移和旋转各有 $3$ 个自由度，故 $\boldsymbol{t}^{\wedge} \boldsymbol{R}$ 共有 $6$ 个自由度。但由于尺度等价性，故 $\boldsymbol E$ 实际上有 $5$ 个自由度。

$\boldsymbol E$ 具有5个自由度的事实，表明最少可以用 $5$ 对点来求解 $\boldsymbol E$

但是，$\boldsymbol E$ 的内在性质是一种非线性性质，在估计时会带来麻烦，因此，也可以只考虑它的尺度等价性，使用 $8$ 对点来估计 $\boldsymbol E$——这就是经典的八点法（Eight-point-algorithm）。

八点法只利用了 $\boldsymbol E$ 的线性性质，因此可以在线性代数框架下求解。

考虑一对匹配点，它们的归一化坐标为 $\boldsymbol{x}_{1}=\left[u_{1}, v_{1}, 1\right]^{\mathrm{T}}, \boldsymbol{x}_{2}=\left[u_{2}, v_{2}, 1\right]^{\mathrm{T}}$。根据对极约束，有：

$$\left(u_{2}, v_{2}, 1\right)\left(\begin{array}{lll} e_{1} & e_{2} & e_{3} \\ e_{4} & e_{5} & e_{6} \\ e_{7} & e_{8} & e_{9} \end{array}\right)\left(\begin{array}{c} u_{1} \\ v_{1} \\ 1 \end{array}\right)=0 .$$

把矩阵 $\boldsymbol E$ 展开，写成向量形式：

$$\boldsymbol{e}=\left[e_{1}, e_{2}, e_{3}, e_{4}, e_{5}, e_{6}, e_{7}, e_{8}, e_{9}\right]^{\mathrm{T}}$$

那么，对极约束可以写成与 $\boldsymbol e$ 有关的线性形式：

$$\left[u_{2} u_{1}, u_{2} v_{1}, u_{2}, v_{2} u_{1}, v_{2} v_{1}, v_{2}, u_{1}, v_{1}, 1\right] \cdot \boldsymbol{e}=0$$

同理，对于其他点对也有相同的表示。把所有点都放到一个方程中，变成线性方程组（$u^i$，$v^i$ 表示第 $i$ 个特征点，依此类推)：

$$\left(\begin{array}{ccccccccc} u_{2}^{1} u_{1}^{1} & u_{2}^{1} v_{1}^{1} & u_{2}^{1} & v_{2}^{1} u_{1}^{1} & v_{2}^{1} v_{1}^{1} & v_{2}^{1} & u_{1}^{1} & v_{1}^{1} & 1 \\ u_{2}^{2} u_{1}^{2} & u_{2}^{2} v_{1}^{2} & u_{2}^{2} & v_{2}^{2} u_{1}^{2} & v_{2}^{2} v_{1}^{2} & v_{2}^{2} & u_{1}^{2} & v_{1}^{2} & 1 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \\ u_{2}^{8} u_{1}^{8} & u_{2}^{8} v_{1}^{8} & u_{2}^{8} & v_{2}^{8} u_{1}^{8} & v_{2}^{8} v_{1}^{8} & v_{2}^{8} & u_{1}^{8} & v_{1}^{8} & 1 \end{array}\right)\left(\begin{array}{c} e_{1} \\ e_{2} \\ e_{3} \\ e_{4} \\ e_{5} \\ e_{6} \\ e_{7} \\ e_{8} \\ e_{9} \end{array}\right)=0 .$$

这8个方程构成了一个线性方程组。它的系数矩阵由特征点位置构成，大小为 $8×9$

$e$ 位于该矩阵的零空间中。如果系数矩阵是满秩的（即秩为 $8$），那么它的零空间维数为 $1$，也就是 $e$ 构成一条线。

这与 $e$ 的尺度等价性是一致的。如果 $8$ 对匹配点组成的矩阵满足秩为 $8$ 的条件，那么 $\boldsymbol E$ 的各元素就可由上述方程解得。

接下来根据已经估得的本质矩阵 $\boldsymbol E$，恢复出相机的运动 $\boldsymbol R,\boldsymbol t$

由奇异值分解（SVD）得到的。设 $\boldsymbol E$ 的SVD为：

$$\boldsymbol{E}=\boldsymbol{U} \boldsymbol{\Sigma} \boldsymbol{V}^{\mathrm{T}}$$

其中 $\boldsymbol{U},\boldsymbol{V}$ 为正交阵，$\boldsymbol{\Sigma}$ 为奇异值矩阵。

根据 $\boldsymbol E$ 的内在性质，知道习 $\boldsymbol{\Sigma}=\operatorname{diag}(\sigma, \sigma, 0)$

在SVD分解中，对于任意一个 $\boldsymbol E$，存在两个可能的 $\boldsymbol R,\boldsymbol t$ 与它对应：

$$\begin{array}{l} \boldsymbol{t}_{1}^{\wedge}=\boldsymbol{U} \boldsymbol{R}_{Z}\left(\frac{\pi}{2}\right) \boldsymbol{\Sigma} \boldsymbol{U}^{\mathrm{T}}, \quad \boldsymbol{R}_{1}=\boldsymbol{U} \boldsymbol{R}_{Z}^{\mathrm{T}}\left(\frac{\pi}{2}\right) \boldsymbol{V}^{\mathrm{T}} \\ \boldsymbol{t}_{2}^{\wedge}=\boldsymbol{U} \boldsymbol{R}_{Z}\left(-\frac{\pi}{2}\right) \boldsymbol{\Sigma} \boldsymbol{U}^{\mathrm{T}}, \quad \boldsymbol{R}_{2}=\boldsymbol{U} \boldsymbol{R}_{Z}^{\mathrm{T}}\left(-\frac{\pi}{2}\right) \boldsymbol{V}^{\mathrm{T}} . \end{array}$$

其中，$\boldsymbol{R}_{Z}\left(\frac{\pi}{2}\right)$ 表示沿 $Z$ 轴旋转 $90°$ 得到旋转矩阵。

同时，由于 $-\boldsymbol E$ 和 $\boldsymbol E$ 等价，所以对任意一个 $\boldsymbol t$ 取负号，也会得到同样的结果。

因此，从 $\boldsymbol E$ 分解到 $\boldsymbol R,\boldsymbol t$ 时，一共存在4个可能的解。

上图形象地展示了分解本质矩阵得到的4个解。

已知空间点在相机（蓝色线）上的投影（红色点），想要求解相机的运动。在保持红色点不变的情况下，可以画出4种可能的情况。

只有第一种解中 $P$ 在两个相机中都具有正的深度。因此，只要把任意一点代入 $4$ 种解中，检测该点在两个相机下的深度，就可以确定哪个解是正确的。

剩下的一个问题是：

根据线性方程解出的 $\boldsymbol E$，可能不满足 $\boldsymbol E$ 的内在性质——它的奇异值不一定为 $[\sigma, \sigma, 0]^{\mathrm T}$ 的形式。

这时，会刻意地把矩阵调整成上面的样子。通常的做法是，对八点法求得的 $\boldsymbol E$ 进行SVD，会得到奇异值矩阵 $\boldsymbol{\Sigma}=\operatorname{diag}(\sigma_1, \sigma_2, \sigma_3)$，不妨设 $\sigma_{1} \geqslant \sigma_{2} \geqslant \sigma_{3}$。取:

$$\boldsymbol{E}=\boldsymbol{U} \operatorname{diag}\left(\frac{\sigma_{1}+\sigma_{2}}{2}, \frac{\sigma_{1}+\sigma_{2}}{2}, 0\right) \boldsymbol{V}^{\mathrm{T}} .$$

相当于是把求出来的矩阵投影到了 $\boldsymbol E$ 所在的流形上。

当然，更简单的做法是将奇异值矩阵取成 $\operatorname{diag}(1, 1, 0)$，因为 $\boldsymbol E$ 具有尺度等价性，所以这样做也是合理的。

单应矩阵

除了基本矩阵和本质矩阵，二视图几何中还存在另一种常见的矩阵：单应矩阵（Homography）$\boldsymbol H$

它描述了两个平面之间的映射关系。

若场景中的特征点都落在同一平面上（比如墙、地面等），则可以通过单应性进行运动估计。

单应矩阵通常描述处于共同平面上的一些点在两张图像之间的变换关系。

设图像 $I_1$ 和 $I_2$ 有一对匹配好的特征点 $\boldsymbol p_1$ 和 $\boldsymbol p_2$

这个特征点落在平面 $P$ 上，设这个平面满足方程：

$$\boldsymbol{n}^{\mathrm{T}} \boldsymbol{P}+d=0$$

整理得：

$$-\frac{\boldsymbol{n}^{\mathrm{T}} \boldsymbol{P}}{d}=1$$

由之前的式，代入得：

$$\begin{aligned} \boldsymbol{p}_{2} & \simeq \boldsymbol{K}(\boldsymbol{R} \boldsymbol{P}+\boldsymbol{t}) \\ & \simeq \boldsymbol{K}\left(\boldsymbol{R} \boldsymbol{P}+\boldsymbol{t} \cdot\left(-\frac{\boldsymbol{n}^{\mathrm{T}} \boldsymbol{P}}{d}\right)\right) \\ & \simeq \boldsymbol{K}\left(\boldsymbol{R}-\frac{\boldsymbol{\boldsymbol { n }} \boldsymbol{n}^{\mathrm{T}}}{d}\right) \boldsymbol{P} \\ & \simeq \boldsymbol{K}\left(\boldsymbol{R}-\frac{\boldsymbol{t} \boldsymbol{n}^{\mathrm{T}}}{d}\right) \boldsymbol{K}^{-1} \boldsymbol{p}_{1} . \end{aligned}$$

得到一个直接描述图像坐标 $\boldsymbol p_1$ 和 $\boldsymbol p_2$ 之间的变换，把中间部分记为 $\boldsymbol H$，得：

$$\boldsymbol{p}_{2} \simeq \boldsymbol{H} \boldsymbol{p}_{1}$$

它的定义与旋转、平移及平面的参数有关。

与基础矩阵 $\boldsymbol F$ 类似，单应矩阵 $\boldsymbol H$ 也是一个 $3×3$ 的矩阵，求解时的思路和 $\boldsymbol F$ 类似，同样可以先根据匹配点计算 $\boldsymbol H$，然后将它分解以计算旋转和平移。把上式展开，得：

$$\left(\begin{array}{c} u_{2} \\ v_{2} \\ 1 \end{array}\right) \simeq\left(\begin{array}{lll} h_{1} & h_{2} & h_{3} \\ h_{4} & h_{5} & h_{6} \\ h_{7} & h_{8} & h_{9} \end{array}\right)\left(\begin{array}{c} u_{1} \\ v_{1} \\ 1 \end{array}\right)$$

因为这里等号依然是 $\simeq$ 而不是普通的等号，所以 $\boldsymbol H$ 矩阵也可以乘以任意非零常数。

实际处理中可以令 $h_9 =1$（在它取非零值时）。然后根据第3行，去掉这个非零因子，于是有：

$$\begin{aligned} u_{2} &=\frac{h_{1} u_{1}+h_{2} v_{1}+h_{3}}{h_{7} u_{1}+h_{8} v_{1}+h_{9}} \\ v_{2} &=\frac{h_{4} u_{1}+h_{5} v_{1}+h_{6}}{h_{7} u_{1}+h_{8} v_{1}+h_{9}} . \end{aligned}$$

整理得：

$$\begin{array}{l} h_{1} u_{1}+h_{2} v_{1}+h_{3}-h_{7} u_{1} u_{2}-h_{8} v_{1} u_{2}=u_{2} \\ h_{4} u_{1}+h_{5} v_{1}+h_{6}-h_{7} u_{1} v_{2}-h_{8} v_{1} v_{2}=v_{2} \end{array}$$

这样一组匹配点对就可以构造出两项约束（事实上有三个约束，但是因为线性相关，只取前两个），于是自由度为 $8$ 的单应矩阵可以通过 $4$ 对匹配特征点算出（在非退化的情况下，即这些特征点不能有三点共线的情况)，即求解以下的线性方程组（当 $h_9 =0$ 时，右侧为零)：

$$\left(\begin{array}{cccccccc} u_{1}^{1} & v_{1}^{1} & 1 & 0 & 0 & 0 & -u_{1}^{1} u_{2}^{1} & -v_{1}^{1} u_{2}^{1} \\ 0 & 0 & 0 & u_{1}^{1} & v_{1}^{1} & 1 & -u_{1}^{1} v_{2}^{1} & -v_{1}^{1} v_{2}^{1} \\ u_{1}^{2} & v_{1}^{2} & 1 & 0 & 0 & 0 & -u_{1}^{2} u_{2}^{2} & -v_{1}^{2} u_{2}^{2} \\ 0 & 0 & 0 & u_{1}^{2} & v_{1}^{2} & 1 & -u_{1}^{2} v_{2}^{2} & -v_{1}^{2} v_{2}^{2} \\ u_{1}^{3} & v_{1}^{3} & 1 & 0 & 0 & 0 & -u_{1}^{3} u_{2}^{3} & -v_{1}^{3} u_{2}^{3} \\ 0 & 0 & 0 & u_{1}^{3} & v_{1}^{3} & 1 & -u_{1}^{3} v_{2}^{3} & -v_{1}^{3} v_{2}^{3} \\ u_{1}^{4} & v_{1}^{4} & 1 & 0 & 0 & 0 & -u_{1}^{4} u_{2}^{4} & -v_{1}^{4} u_{2}^{4} \\ 0 & 0 & 0 & u_{1}^{4} & v_{1}^{4} & 1 & -u_{1}^{4} v_{2}^{4} & -v_{1}^{4} v_{2}^{4} \end{array}\right)\left(\begin{array}{c} h_{1} \\ h_{2} \\ h_{3} \\ h_{4} \\ h_{5} \\ h_{6} \\ h_{7} \\ h_{8} \end{array}\right)=\left(\begin{array}{c} u_{2}^{1} \\ v_{2}^{1} \\ u_{2}^{2} \\ v_{2}^{2} \\ u_{2}^{3} \\ v_{2}^{3} \\ u_{2}^{4} \\ v_{2}^{4} \end{array}\right)$$

这种做法把 $\boldsymbol H$ 矩阵看成了向量，通过解该向量的线性方程来恢复 $\boldsymbol H$，又称直接线性变换法（Direct Linear Transform，DLT）。

与本质矩阵相似，求出单应矩阵以后需要对其进行分解，才可以得到相应的旋转矩阵 $\boldsymbol R$ 和平移向量 $\boldsymbol t$

分解的方法包括数值法与解析法。

与本质矩阵的分解类似，单应矩阵的分解同样会返回 $4$ 组旋转矩阵与平移向量，同时，可以计算出它们分别对应的场景点所在平面的法向量。

如果已知成像的地图点的深度全为正值（即在相机前方），则又可以排除两组解。

最后仅剩两组解，这时需要通过更多的先验信息进行判断。通常，可以通过假设已知场景平面的法向量来解决，如场景平面与相机平面平行，那么法向量 $\boldsymbol n$ 的理论值为 $1^\mathrm T$。

单应性在SLAM中具有重要意义。

当特征点共面或者相机发生纯旋转时，基础矩阵的自由度下降，这就出现了所谓的退化（degenerate）。

现实中的数据总包含一些噪声，这时如果继续使用八点法求解基础矩阵，基础矩阵多余出来的自由度将会主要由噪声决定。

为了能够避免退化现象造成的影响，通常会同时估计基础矩阵 $\boldsymbol F$ 和单应矩阵 $\boldsymbol H$，选择重投影误差比较小的那个作为最终的运动估计矩阵。

三角测量

在得到运动之后，下一步需要用相机的运动估计特征点的空间位置。

在单目SLAM中，仅通过单张图像无法获得像素的深度信息，需要通过三角测量（Triangulation）（或三角化） 估计地图点的深度。

三角测量是指，通过不同位置对同一个路标点进行观察，从观察到的位置推断路标点的距离。

三角测量最早由高斯提出并应用于测量学中，它在天文学、地理学的测量中都有应用。在SLAM中，主要用三角化来估计像素点的距离。

和上一节类似，考虑图像 $I_1$ 和 $I_2$，以左图为参考，右图的变换矩阵为 $\boldsymbol T$

相机光心为 $O_1$ 和 $O_2$

在 $I_1$ 中有特征点 $p_1$，对应 $I_2$ 中有特征点 $p_2$

理论上，直线 $O_1p_1$ 与 $O_2p_2$ 在场景中会相交于一点 $P$，该点即两个特征点所对应的地图点在三维场景中的位置。

然而由于噪声的影响，这两条直线往往无法相交。因此，可以通过最小二乘法求解。

按照对极几何中的定义，设 $\boldsymbol x_1,\boldsymbol x_2$ 为两个特征点的归一化坐标，那么它们满足：

$$s_{2} \boldsymbol x_{2}=s_{1} \boldsymbol{R} \boldsymbol{x}_{1}+\boldsymbol{t}$$

现在已知 $\boldsymbol R,\boldsymbol t$，想求解两个特征点的深度 $s_1, s_2$

从几何上看，可以在射线 $O_1p_1$ 上寻找 3D 点，使其投影位置接近 $p_2$

同理，也可以在 $O_2p_2$ 上找，或者在两条线的中间找。

不同的策略对应着不同的计算方式，大同小异。

例如，我们希望计算 $s_1$，那么先对上式两侧左乘一个 $\boldsymbol{x}_{2}^{\wedge}$，得：

$$s_{2} \boldsymbol{x}_{2}^{\wedge} \boldsymbol{x}_{2}=0=s_{1} \boldsymbol{x}_{2}^{\wedge} \boldsymbol{R} \boldsymbol{x}_{1}+\boldsymbol{x}_{2}^{\wedge} \boldsymbol{t} .$$

该式左侧为零，右侧可看成 $s_2$ 的一个方程，直接求得 $s_2$，然后再求得 $s_1$

于是，就得到了两帧下的点的深度，确定了它们的空间坐标。

由于噪声的存在，估得的 $\boldsymbol R,\boldsymbol t$ 不一定精确使上式为零，所以更常见的做法是求最小二乘解而不是直接的解。

三角测量是由平移得到的，有平移才会有对极几何中的三角形，才谈得上三角测量。

因此，纯旋转是无法使用三角测量的，因为在平移为零时，对极约束一直为零。

在平移存在的情况下，还要关心三角测量的不确定性，这会引出一个三角测量的矛盾。

当平移很小时，像素上的不确定性将导致较大的深度不确定性。

平移较大时，在同样的相机分辨率下，三角化测量将更精确。

因此，要提高三角化的精度

一种方式是提高特征点的提取精度，也就是提高图像分辨率——但这会导致图像变大，增加计算成本。

另一种方式是使平移量增大。但是，这会导致图像的外观发生明显的变化，外观变化会使得特征提取与匹配变得困难。

增大平移，可能导致匹配失效；而平移太小，则三角化精度不够——这就是三角化的矛盾。这个问题称为“视差”（parallax）

延迟三角化：在单目视觉中，由于单目图像没有深度信息，要等待特征点被追踪几帧之后，产生了足够的视角，再用三角化来确定新增特征点的深度值。

3D-2D：PnP

PnP（Perspective-n-Point）是求解 $3\mathrm D$ 到 $2\mathrm D$ 点对运动的方法。

它描述了当知道 $n$ 个 $3\mathrm D$ 空间点及其投影位置时，如何估计相机的位姿。

$2\mathrm D$-$2\mathrm D$ 的对极几何方法需要 $8$ 个或 $8$ 个以上的点对（以八点法为例)，且存在着初始化、纯旋转和尺度的问题。

然而，如果两张图像中的一张特征点的 $3\mathrm D$ 位置已知，那么最少只需 $3$ 个点对（以及至少一个额外点验证结果）就可以估计相机运动。

特征点的 $3\mathrm D$ 位置可以由三角化或者RGB-D相机的深度图确定。

因此，在双目或RGB-D的视觉里程计中，可以直接使用PnP估计相机运动。

而在单目视觉里程计中，必须先进行初始化，才能使用PnP。

$3\mathrm D$-$2\mathrm D$ 方法不需要使用对极约束，又可以在很少的匹配点中获得较好的运动估计，是一种最重要的姿态估计方法。

PnP问题求解方法：

• 用 $3$ 对点估计位姿的P3P
• 直接线性变换（DLT）
• EPnP（Efficient PnP）
• UPnP
• 光束法平差（Bundle Adjustment，BA），非线性优化的方式，构建最小二乘问题并迭代求解

直接线性变换

已知一组 $3\mathrm D$ 点的位置，以及它们在某个相机中的投影位置，求该相机的位姿。

考虑某个空间点 $P$，齐次坐标为 $\boldsymbol{P}=(X, Y, Z, 1)^{\mathrm{T}}$

在图像 $I_1$ 中，投影到特征点 $\boldsymbol x_1=(u_1, v_1,1)^\mathrm T$ (以归一化平面齐次坐标表示)（去掉了内参矩阵 $\boldsymbol K$ 的影响——这是因为内参 $\boldsymbol K$ 在SLAM中通常假设为已知）

此时，相机的位姿 $\boldsymbol R,\boldsymbol t$ 是未知的。与单应矩阵的求解类似，定义增广矩阵 $[\boldsymbol R|\boldsymbol t]$ 为一个 $3×4$ 的矩阵，包含了旋转与平移信息。

将其展开形式列写如下：

$$s\left(\begin{array}{c} u_{1} \\ v_{1} \\ 1 \end{array}\right)=\left(\begin{array}{cccc} t_{1} & t_{2} & t_{3} & t_{4} \\ t_{5} & t_{6} & t_{7} & t_{8} \\ t_{9} & t_{10} & t_{11} & t_{12} \end{array}\right)\left(\begin{array}{c} X \\ Y \\ Z \\ 1 \end{array}\right) .$$

用最后一行把 $s$ 消去，得到两个约束：

$$u_{1}=\frac{t_{1} X+t_{2} Y+t_{3} Z+t_{4}}{t_{9} X+t_{10} Y+t_{11} Z+t_{12}}, \quad v_{1}=\frac{t_{5} X+t_{6} Y+t_{7} Z+t_{8}}{t_{9} X+t_{10} Y+t_{11} Z+t_{12}}$$

为了简化表示，定义 $\boldsymbol T$ 的行向量：

$$\boldsymbol t_{1}=\left(t_{1}, t_{2}, t_{3}, t_{4}\right)^{\mathrm{T}}, \boldsymbol t_{2}=\left(t_{5}, t_{6}, t_{7}, t_{8}\right)^{\mathrm{T}}, \boldsymbol t_{3}=\left(t_{9}, t_{10}, t_{11}, t_{12}\right)^{\mathrm{T}}$$

代入得：

$$\begin{array}{l} \boldsymbol{t}_{1}^{\mathrm{T}} \boldsymbol{P}-\boldsymbol{t}_{3}^{\mathrm{T}} \boldsymbol{P} u_{1}=0, \\ \boldsymbol{t}_{2}^{\mathrm{T}} \boldsymbol{P}-\boldsymbol{t}_{3}^{\mathrm{T}} \boldsymbol{P} v_{1}=0 . \end{array}$$

$\boldsymbol t$ 是待求的变量，每个特征点提供了两个关于 $\boldsymbol t$ 的线性约束。

假设一共有 $N$ 个特征点，则可以列出如下线性方程组：

$$\left(\begin{array}{ccc} \boldsymbol{P}_{1}^{\mathrm{T}} & 0 & -u_{1} \boldsymbol{P}_{1}^{\mathrm{T}} \\ 0 & \boldsymbol{P}_{1}^{\mathrm{T}} & -v_{1} \boldsymbol{P}_{1}^{\mathrm{T}} \\ \vdots & \vdots & \vdots \\ \boldsymbol{P}_{N}^{\mathrm{T}} & 0 & -u_{N} \boldsymbol{P}_{N}^{\mathrm{T}} \\ 0 & \boldsymbol{P}_{N}^{\mathrm{T}} & -v_{N} \boldsymbol{P}_{N}^{\mathrm{T}} \end{array}\right)\left(\begin{array}{l} \boldsymbol{t}_{1} \\ \boldsymbol{t}_{2} \\ \boldsymbol{t}_{3} \end{array}\right)=0$$

$\boldsymbol t$ 一共有 $12$ 维，因此最少通过 $6$ 对匹配点即可实现矩阵 $\boldsymbol T$ 的线性求解，这种方法称为DLT。

当匹配点大于 $6$ 对时，也可以使用SVD等方法对超定方程求最小二乘解。

在DLT求解中，直接将 $\boldsymbol T$ 矩阵看成了 $12$ 个未知数，忽略了它们之间的联系。

因为旋转矩阵 $\boldsymbol R \in \mathrm{SO(3)}$，用DLT求出的解不一定满足该约束，它是一个一般矩阵。

平移向量属于向量空间。

对于旋转矩阵 $\boldsymbol R$，必须针对DLT估计的 $\boldsymbol T$ 左边 $3×3$ 的矩阵块，寻找一个最好的旋转矩阵对它进行近似。

这可以由QR分解完成，也可以像这样来计算：

$$\boldsymbol{R} \leftarrow\left(\boldsymbol{R} \boldsymbol{R}^{\mathrm{T}}\right)^{-\frac{1}{2}} \boldsymbol{R}$$

这相当于把结果从矩阵空间重新投影到 $\mathrm{SE(3)}$ 流形上，转换成旋转和平移两部分。

P3P

仅使用3对匹配点，对数据要求较少。

P3P需要利用给定的 $3$ 个点的几何关系。

它的输入数据为 $3$ 对 $3\mathrm D$-$2\mathrm D$ 匹配点。记 $3\mathrm D$ 点为 $A,B,C$，$2\mathrm D$ 点为 $a,b,c$，其中小写字母代表的点为对应大写字母代表的点在相机成像平面上的投影。

此外，P3P还需要使用一对验证点，以从可能的解中选出正确的那一个(类似于对极几何情形)。记验证点对为 $D$—$d$，相机光心为 $O$

这里知道的是 $A,B,C$ 在世界坐标系中的坐标，而不是在相机坐标系中的坐标。

一旦 $3\mathrm D$ 点在相机坐标系下的坐标能够算出，就得到了 $3\mathrm D$-$3\mathrm D$ 的对应点，把 PnP问题转换为了ICP问题。

三角形之间存在对应关系：

$$\triangle O a b-\triangle O A B, \quad \triangle O b c-\Delta O B C, \quad \triangle O a c-\triangle O A C .$$

考虑 $\triangle O a b-\triangle O A B$ 的关系，利用余弦定理有：

$$O A^{2}+O B^{2}-2 O A \cdot O B \cdot \cos \langle a, b\rangle=A B^{2} .$$

对于其他两个三角形也存在类似性质，于是有：

$$\begin{array}{l} O A^{2}+O B^{2}-2 O A \cdot O B \cdot \cos \langle a, b\rangle=A B^{2} \\ O B^{2}+O C^{2}-2 O B \cdot O C \cdot \cos \langle b, c\rangle=B C^{2} \\ O A^{2}+O C^{2}-2 O A \cdot O C \cdot \cos \langle a, c\rangle=A C^{2} . \end{array}$$

对三式同时除以 $OC^2$，并记 $x=OA/OC,y=OB/OC$，得：

$$\begin{array}{l} x^{2}+y^{2}-2 x y \cos \langle a, b\rangle=A B^{2} / O C^{2} \\ y^{2}+1^{2}-2 y \cos \langle b, c\rangle=B C^{2} / O C^{2} \\ x^{2}+1^{2}-2 x \cos \langle a, c\rangle=A C^{2} / O C^{2} \end{array}$$

记 $v=A B^{2} / O C^{2}, u v=B C^{2} / O C^{2}, w v=A C^{2} / O C^{2}$，有：

$$\begin{array}{l} x^{2}+y^{2}-2 x y \cos \langle a, b\rangle-v=0 \\ y^{2}+1^{2}-2 y \cos \langle b, c\rangle-u v=0 \\ x^{2}+1^{2}-2 x \cos \langle a, c\rangle-w v=0 . \end{array}$$

把第一个式子中 $v$ 放到等式一边，再代入其后两式，得：

$$\begin{array}{l} (1-u) y^{2}-u x^{2}-\cos \langle b, c\rangle y+2 u x y \cos \langle a, b\rangle+1=0 \\ (1-w) x^{2}-w y^{2}-\cos \langle a, c\rangle x+2 w x y \cos \langle a, b\rangle+1=0 \end{array}$$

方程中的已知量：

由于知道 $2\mathrm D$ 点的图像位置，3个余弦角 $\cos \langle a, b\rangle,\cos \langle a, c\rangle,\cos \langle b, c\rangle$ 是已知的。

同时，$u=B C^{2} / A B^{2}, w=A C^{2} / A B^{2}$ 可以通过 $A,B,C$ 在世界坐标系下的坐标算出，变换到相机坐标系下之后，这个比值并不改变。

方程中的未知量：$x, y$ 是未知的，随着相机移动会发生变化。

因此，该方程组是关于 $x, y$ 的一个二元二次方程（多项式方程）

求该方程组的解析解是一个复杂的过程，需要用吴消元法。

类似于分解 $\boldsymbol E$ 的情况，该方程最多可能得到4个解，用验证点来计算最可能的解，得到 $A,B,C$ 在相机坐标系下的 $3\mathrm D$ 坐标。然后，根据 $3\mathrm D$-$3\mathrm D$ 的点对，计算相机的运动 $\boldsymbol R,\boldsymbol t$。

从P3P的原理可以看出，为了求解PnP，利用三角形相似性质，求解投影点 $a,b,c$ 在相机坐标系下的 $3\mathrm D$ 坐标，最后把问题转换成一个 $3\mathrm D$ 到 $3\mathrm D$ 的位姿估计问题。

P3P也存在的问题：

1. P3P只利用 $3$ 个点的信息。当给定的配对点多于 $3$ 组时，难以利用更多的信息。
2. 如果 $3\mathrm D$ 点或 $2\mathrm D$ 点受噪声影响，或者存在误匹配，则算法失效。

在SLAM中，通常的做法是先使用P3P/EPnP等方法估计相机位姿，再构建最小二乘优化问题对估计值进行调整（即进行Bundle Adjustment）。

在相机运动足够连续时，也可以假设相机不动或匀速运动，用推测值作为初始值进行优化。

最小化重投影误差求解PnP

除了使用线性方法，还可以把PnP问题构建成一个关于重投影误差的非线性最小二乘问题。

涉及第4讲和第5讲的知识。

前面说的线性方法，往往是先求相机位姿，再求空间点位置，而非线性优化则是把它们都看成优化变量，放在一起优化。

这是一种非常通用的求解方式，可以用它对PnP或ICP给出的结果进行优化。这一类把相机和三维点放在一起进行最小化的问题，统称为Bundle Adjustment

考虑 $n$ 个三维空间点 $P$ 及其投影 $p$，我们希望计算相机的位姿 $\boldsymbol R,\boldsymbol t$，它的李群表示为 $\boldsymbol T$

假设某空间点坐标为 $\boldsymbol P_i=[X_i,Y_i,Z_i]^\mathrm T$，其投影的像素坐标为 $\boldsymbol u_i=[u_i, v_i]^\mathrm T$

根据第5讲的内容，像素位置与空间点位置的关系如下：

$$s_{i}\left[\begin{array}{c} u_{i} \\ v_{i} \\ 1 \end{array}\right]=\boldsymbol{K} \boldsymbol{T}\left[\begin{array}{c} X_{i} \\ Y_{i} \\ Z_{i} \\ 1 \end{array}\right] .$$

写成矩阵形式为：

$$s_{i} \boldsymbol{u}_{i}=\boldsymbol{K} \boldsymbol{T} \boldsymbol{P}_{i}$$

这个式子隐含了一次从齐次坐标到非齐次的转换，否则按矩阵的乘法来说，维度是不对的。

现在，由于相机位姿未知及观测点的噪声，该等式存在一个误差。

把误差求和，构建最小二乘问题，然后寻找最好的相机位姿，使它最小化：

$$\boldsymbol{T}^{*}=\arg \min _{\boldsymbol{T}} \frac{1}{2} \sum_{i=1}^{n}\left\|\boldsymbol{u}_{i}-\frac{1}{s_{i}} \boldsymbol{K} \boldsymbol{T} \boldsymbol{P}_{i}\right\|_{2}^{2}$$

该问题的误差项，是将 $3\mathrm D$ 点的投影位置与观测位置作差，所以称为重投影误差。

使用齐次坐标时，这个误差有 $3$ 维。不过，由于 $\boldsymbol u$ 最后一维为1，该维度的误差一直为零，因而更多使用非齐次坐标，于是误差就只有 $2$ 维了。

如上图所示，通过特征匹配知道了 $p_1$ 和 $p_2$ 是同一个空间点 $P$ 的投影，但是不知道相机的位姿。

在初始值中，$P$ 的投影 $\hat{p}_{2}$ 与实际的 $p_2$ 之间有一定的距离。

于是调整相机的位姿，使得这个距离变小。不过，由于这个调整需要考虑很多个点，所以最后的效果是整体误差的缩小，而每个点的误差通常都不会精确为零。

最小二乘优化问题第6讲介绍，使用李代数，可以构建无约束的优化问题，很方便地通过高斯牛顿法、列文伯格—马夸尔特方法等优化算法进行求解。

在使用高斯牛顿法和列文伯格—马夸尔特方法之前，需要知道每个误差项关于优化变量的导数，也就是线性化：

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

当 $\boldsymbol e$ 为像素坐标误差（2维），$\boldsymbol x$ 为相机位姿（6维）时，$\boldsymbol{J}^{\mathrm{T}}$ 将是一个 $2×6$ 的矩阵。

推导 $\boldsymbol{J}^{\mathrm{T}}$ 的形式：

使用扰动模型来求李代数的导数。

首先，记变换到相机坐标系下的空间点坐标为 $\boldsymbol{P}^{\prime}$，并且将其前 $3$ 维取出来：

$$\boldsymbol{P}^{\prime}=(\boldsymbol{T} \boldsymbol{P})_{1: 3}=\left[X^{\prime}, Y^{\prime}, Z^{\prime}\right]^{\mathrm{T}} .$$

那么，相机模型相对于 $\boldsymbol{P}^{\prime}$ 为：

$$s \boldsymbol{u}=\boldsymbol{K} \boldsymbol{P}^{\prime} .$$

展开：

$${\left[\begin{array}{c} s u \\ s v \\ s \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^{\prime} \\ Y^{\prime} \\ Z^{\prime} \end{array}\right] .}$$

利用第三行消去 $s$（实际上就是 $\boldsymbol{P}^{\prime}$ 的距离），得：

$$u=f_{x} \frac{X^{\prime}}{Z^{\prime}}+c_{x}, \quad v=f_{y} \frac{Y^{\prime}}{Z^{\prime}}+c_{y}$$

这与第5讲的相机模型是一致的。

当求误差时，可以把这里的 $u,v$ 与实际的测量值比较，求差。

在定义了中间变量后，对 $\boldsymbol T$ 左乘扰动量 $\delta \boldsymbol{\xi}$，然后考虑 $\boldsymbol{e}$ 的变化关于扰动量的导数。

利用链式法则，可以列写如下：

$$\frac{\partial \boldsymbol{e}}{\partial \delta \boldsymbol{\xi}}=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\boldsymbol{e}(\delta \boldsymbol{\xi} \oplus \boldsymbol{\xi})-\boldsymbol{e}(\boldsymbol{\xi})}{\delta \xi}=\frac{\partial \boldsymbol{e}}{\partial \boldsymbol{P}^{\prime}} \frac{\partial \boldsymbol{P}^{\prime}}{\partial \delta \xi}$$

这里的 $\oplus$ 指李代数上的左乘扰动。

第一项是误差关于投影点的导数，在式 $u=f_{x} \frac{X^{\prime}}{Z^{\prime}}+c_{x}, \quad v=f_{y} \frac{Y^{\prime}}{Z^{\prime}}+c_{y}$ 中已经列出了变量之间的关系，易得：

$$\frac{\partial \boldsymbol{e}}{\partial \boldsymbol{P}^{\prime}}=-\left[\begin{array}{ccc} \frac{\partial u}{\partial X^{\prime}} & \frac{\partial u}{\partial Y^{\prime}} & \frac{\partial u}{\partial Z^{\prime}} \\ \frac{\partial v}{\partial X^{\prime}} & \frac{\partial v}{\partial Y^{\prime}} & \frac{\partial v}{\partial Z^{\prime}} \end{array}\right]=-\left[\begin{array}{ccc} \frac{f_{x}}{Z^{\prime}} & 0 & -\frac{f_{x} X^{\prime}}{Z^{\prime 2}} \\ 0 & \frac{f_{y}}{Z^{\prime}} & -\frac{f_{y} Y^{\prime}}{Z^{\prime 2}} \end{array}\right] \text {. }$$

而第二项为变换后的点关于李代数的导数，根据第4讲李代数求导与扰动模型中 $\mathrm{SE(3)}$ 上的李代数求导推导，得：

$$\frac{\partial(\boldsymbol{T P})}{\partial \delta \boldsymbol{\xi}}=(\boldsymbol{T P})^{\odot}=\left[\begin{array}{cc} \boldsymbol{I} & -\boldsymbol{P}^{\prime \wedge} \\ \mathbf{0}^{\mathrm{T}} & \mathbf{0}^{\mathrm{T}} \end{array}\right]$$

而在 $\boldsymbol{P}^{\prime}$ 的定义中，取出了前 $3$ 维，于是得：

$$\frac{\partial \boldsymbol{P}^{\prime}}{\partial \delta \boldsymbol{\xi}}=\left[\boldsymbol{I},-\boldsymbol{P}^{\prime \wedge}\right]$$

将这两项相乘，就得到了 $2×6$ 的雅可比矩阵：

$$\frac{\partial \boldsymbol{e}}{\partial \delta \boldsymbol{\xi}}=-\left[\begin{array}{cccccc} \frac{f_{x}}{Z^{\prime}} & 0 & -\frac{f_{x} X^{\prime}}{Z^{\prime 2}} & -\frac{f_{x} X^{\prime} Y^{\prime}}{Z^{\prime 2}} & f_{x}+\frac{f_{x} X^{\prime 2}}{Z^{\prime 2}} & -\frac{f_{x} Y^{\prime}}{Z^{\prime}} \\ 0 & \frac{f_{y}}{Z^{\prime}} & -\frac{f_{y} Y^{\prime}}{Z^{\prime 2}} & -f_{y}-\frac{f_{y} Y^{\prime 2}}{Z^{\prime 2}} & \frac{f_{y} X^{\prime} Y^{\prime}}{Z^{\prime 2}} & \frac{f_{y} X^{\prime}}{Z^{\prime}} \end{array}\right] .$$

这个雅可比矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系。

保留了前面的负号，这是因为误差是由观测值减预测值定义的。

如果 $\mathfrak{se}(3)$ 的定义方式是旋转在前，平移在后，则只要把这个矩阵的前 $3$ 列与后 $3$ 列对调即可。

除了优化位姿，还希望优化特征点的空间位置。因此，需要讨论 $\boldsymbol e$ 关于空间点 $\boldsymbol P$ 的导数。

利用链式法则，有：

$$\frac{\partial \boldsymbol e}{\partial \boldsymbol P}=\frac{\partial \boldsymbol e}{\partial \boldsymbol P^{\prime}} \frac{\partial \boldsymbol P^{\prime}}{\partial \boldsymbol P}$$

第一项前面已推导，第二项，按照定义：

$$\boldsymbol{P}^{\prime}=(\boldsymbol{T} \boldsymbol{P})_{1: 3}=\boldsymbol{R} \boldsymbol{P}+\boldsymbol{t}$$

$\boldsymbol{P}^{\prime}$ 对 $\boldsymbol{P}$ 求导后只剩下 $\boldsymbol R$，所以：

$$\frac{\partial \boldsymbol{e}}{\partial \boldsymbol{P}}=-\left[\begin{array}{ccc} \frac{f_{x}}{Z^{\prime}} & 0 & -\frac{f_{x} X^{\prime}}{Z^{\prime 2}} \\ 0 & \frac{f_{y}}{Z^{\prime}} & -\frac{f_{y} Y^{\prime}}{Z^{\prime 2}} \end{array}\right] \boldsymbol{R} .$$

最后推导出了观测相机方程关于相机位姿与特征点的两个导数矩阵。能够在优化过程中提供重要的梯度方向，指导优化的迭代。

3D-3D：ICP

假设我们有一组配对好的 $3\mathrm D$ 点：

$$\boldsymbol{P}=\left\{\boldsymbol{p}_{1}, \cdots, \boldsymbol{p}_{n}\right\}, \quad \boldsymbol{P}^{\prime}=\left\{\boldsymbol{p}_{1}^{\prime}, \cdots, \boldsymbol{p}_{n}^{\prime}\right\}$$

想要找一个欧式变化 $\boldsymbol R,\boldsymbol t$，使得：

$$\forall i, \boldsymbol{p}_{i}=\boldsymbol{R} \boldsymbol{p}_{i}^{\prime}+\boldsymbol{t}$$

这个问题可以用迭代最近点（lterative Closest Point，ICP）求解。

$3\mathrm D$-$3\mathrm D$ 位姿估计问题中并没有出现相机模型，也就是说，仅考虑两组 $3\mathrm D$ 点之间的变换时，和相机并没有关系。

和PnP类似，ICP的求解也分为两种方式：

• 利用线性代数的求解（主要是SVD）
• 利用非线性优化方式的求解（类似于BA）

SVD方法

根据描述的ICP问题，定义第 $i$ 对点的误差项：

$$\boldsymbol{e}_{i}=\boldsymbol{p}_{i}-\left(\boldsymbol{R} \boldsymbol{p}_{i}^{\prime}+\boldsymbol{t}\right)$$

构建最小二乘问题，求使误差平方和达到极小的 $\boldsymbol R,\boldsymbol t$：

$$\min _{R, t} \frac{1}{2} \sum_{i=1}^{n}\left\|\left(\boldsymbol{p}_{i}-\left(\boldsymbol{R} \boldsymbol{p}_{i}{ }^{\prime}+\boldsymbol{t}\right)\right)\right\|_{2}^{2}$$

求解首先定义两组点的质心：

$$\boldsymbol{p}=\frac{1}{n} \sum_{i=1}^{n}\left(\boldsymbol{p}_{i}\right), \quad \boldsymbol{p}^{\prime}=\frac{1}{n} \sum_{i=1}^{n}\left(\boldsymbol{p}_{i}^{\prime}\right)$$

在误差函数中做如下处理：

$$\begin{aligned} \frac{1}{2} \sum_{i=1}^{n}\left\|p_{i}-\left(\boldsymbol{R} p_{i}{ }^{\prime}+t\right)\right\|^{2}=& \frac{1}{2} \sum_{i=1}^{n}\left\|p_{i}-\boldsymbol{R} p_{i}{ }^{\prime}-\boldsymbol{t}-\boldsymbol{p}+\boldsymbol{R} p^{\prime}+\boldsymbol{p}-\boldsymbol{R} \boldsymbol{p}^{\prime}\right\|^{2} \\ =& \frac{1}{2} \sum_{i=1}^{n}\left\|\left(p_{i}-\boldsymbol{p}-\boldsymbol{R}\left(\boldsymbol{p}_{i}{ }^{\prime}-\boldsymbol{p}^{\prime}\right)\right)+\left(\boldsymbol{p}-\boldsymbol{R} \boldsymbol{p}^{\prime}-\boldsymbol{t}\right)\right\|^{2} \\ =& \frac{1}{2} \sum_{i=1}^{n}\left(\left\|\boldsymbol{p}_{i}-\boldsymbol{p}-\boldsymbol{R}\left(\boldsymbol{p}_{i}{ }^{\prime}-\boldsymbol{p}^{\prime}\right)\right\|^{2}+\left\|\boldsymbol{p}-\boldsymbol{R} \boldsymbol{p}^{\prime}-\boldsymbol{t}\right\|^{2}+\right.\\ &\left.2\left(\boldsymbol{p}_{i}-\boldsymbol{p}-\boldsymbol{R}\left(\boldsymbol{p}_{i}{ }^{\prime}-\boldsymbol{p}^{\prime}\right)\right)^{\mathrm{T}}\left(\boldsymbol{p}-\boldsymbol{R} \boldsymbol{p}^{\prime}-\boldsymbol{t}\right)\right) . \end{aligned}$$

交叉项部分中 $\left(\boldsymbol{p}_{i}-\boldsymbol{p}-\boldsymbol{R}\left(\boldsymbol{p}_{i}{ }^{\prime}-\boldsymbol{p}^{\prime}\right)\right)$ 在求和之后为零，因此优化目标函数可以简化为：

$$\min _{\boldsymbol R, \boldsymbol t} J=\frac{1}{2} \sum_{i=1}^{n}\left\|\boldsymbol{p}_{i}-\boldsymbol{p}-\boldsymbol{R}\left(\boldsymbol{p}_{i}{ }^{\prime}-\boldsymbol{p}^{\prime}\right)\right\|^{2}+\left\|\boldsymbol{p}-\boldsymbol{R} \boldsymbol{p}^{\prime}-\boldsymbol{t}\right\|^{2} .$$

观察左右两项，左边只和旋转矩阵 $\boldsymbol R$ 相关，而右边既有 $\boldsymbol R$ 也有 $\boldsymbol t$，但只和质心相关。

只要获得了 $\boldsymbol R$，令第二项为零就能得到 $\boldsymbol t$。

于是，ICP可以分为以下三个步骤求解：

1. 计算两组点的质心位置 $p,p^\prime$，然后计算每个点的去质心坐标：

   $$\boldsymbol{q}_{i}=\boldsymbol{p}_{i}-\boldsymbol{p}, \quad \boldsymbol{q}_{i}^{\prime}=\boldsymbol{p}_{i}^{\prime}-\boldsymbol{p}^{\prime}$$

2. 根据以下优化问题计算旋转矩阵：

   $$\boldsymbol{R}^{*}=\arg \min _{\boldsymbol{R}} \frac{1}{2} \sum_{i=1}^{n}\left\|\boldsymbol{q}_{i}-\boldsymbol{R} \boldsymbol{q}_{i}^{\prime}\right\|^{2}$$

3. 根据第2步的 $\boldsymbol R$ 计算 $\boldsymbol t$：

   $$\boldsymbol t^{*}=\boldsymbol p-\boldsymbol R \boldsymbol p^{\prime}$$

只要求出了两组点之间的旋转，平移量就容易得到。

重点在于 $\boldsymbol R$ 的计算，展开关于 $\boldsymbol R$ 的误差项，得：

$$\frac{1}{2} \sum_{i=1}^{n}\left\|\boldsymbol{q}_{i}-\boldsymbol{R} \boldsymbol{q}_{i}^{\prime}\right\|^{2}=\frac{1}{2} \sum_{i=1}^{n}\left(\boldsymbol{q}_{i}^{\mathrm{T}} \boldsymbol{q}_{i}+\boldsymbol{q}_{i}^{\prime \mathrm{T}} \boldsymbol{R}^{\mathrm{T}} \boldsymbol{R} \boldsymbol{q}_{i}^{\prime}-2 \boldsymbol{q}_{i}^{\mathrm{T}} \boldsymbol{R} \boldsymbol{q}_{i}^{\prime}\right)$$

第一项和 $\boldsymbol R$ 无关，第二项由于 $\boldsymbol{R}^{\mathrm{T}} \boldsymbol{R}=\boldsymbol I$，亦与 $\boldsymbol R$ 无关。

因此，实际上优化目标函数变为：

$$\sum_{i=1}^{n}-\boldsymbol{q}_{i}^{\mathrm{T}} \boldsymbol{R} \boldsymbol{q}_{i}^{\prime}=\sum_{i=1}^{n}-\operatorname{tr}\left(\boldsymbol{R} \boldsymbol{q}_{i}^{\prime} \boldsymbol{q}_{i}^{\mathrm{T}}\right)=-\operatorname{tr}\left(\boldsymbol{R} \sum_{i=1}^{n} \boldsymbol{q}_{i}^{\prime} \boldsymbol{q}_{i}^{\mathrm{T}}\right)$$

为解出最优的 $\boldsymbol R$，先定义矩阵：

$$\boldsymbol{W}=\sum_{i=1}^{n} \boldsymbol{q}_{i} \boldsymbol{q}_{i}^{\prime \mathrm{T}}$$

$\boldsymbol{W}$ 是一个 $3×3$ 的矩阵，对 $\boldsymbol{W}$ 进行 SVD 分解，得：

$$\boldsymbol{W}=\boldsymbol{U} \boldsymbol{\Sigma} \boldsymbol{V}^\mathrm{T}$$

其中，$\boldsymbol{\Sigma}$ 为奇异值组成的对角矩阵，对角线元素从大到小排列，而 $\boldsymbol{U}$ 和 $\boldsymbol{V}$ 为对角矩阵。

当 $\boldsymbol{W}$ 满秩时，$\boldsymbol R$ 为：

$$\boldsymbol{R}=\boldsymbol{U} \boldsymbol{V}^\mathrm{T}$$

解得 $\boldsymbol R$ 后，按式 $\boldsymbol t^{*}=\boldsymbol p-\boldsymbol R \boldsymbol p^{\prime}$ 求解 $\boldsymbol t$ 即可。

如果此时 $\boldsymbol R$ 的行列式为负，则取 $-\boldsymbol R$ 作为最优值。

非线性优化方法

求解ICP的另一种方式是使用非线性优化，以迭代的方式去找最优值。和前面讲述的PnP非常相似。

以李代数表达位姿时，目标函数可以写成：

$$\min _{\boldsymbol{\xi}}=\frac{1}{2} \sum_{i=1}^{n}\left\|\left(\boldsymbol{p}_{i}-\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}_{i}^{\prime}\right)\right\|_{2}^{2}$$

单个误差项关于位姿的导数之前已推导，使用李代数扰动模型即可：

$$\frac{\partial \boldsymbol{e}}{\partial \delta \boldsymbol{\xi}}=-\left(\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}_{i}^{\prime}\right)^{\odot}$$

于是，在非线性优化中只需不断迭代，就能找到极小值。

而且，可以证明，ICP问题存在唯一解或无穷多解的情况。

在唯一解的情况下，只要能找到极小值解，这个极小值就是全局最优值——因此不会遇到局部极小而非全局最小的情况。这也意味着ICP求解可以任意选定初始值。这是已匹配点时求解ICP的一大好处。

某些场合下，例如在RGB-D SLAM中，一个像素的深度数据可能有，也可能测量不到，所以可以混合着使用PnP和 ICP优化：对于深度已知的特征点，建模它们的 $3\mathrm D$-$3\mathrm D$ 误差；对于深度未知的特征点，则建模 $3\mathrm D$-$2\mathrm D$ 的重投影误差。于是，可以将所有的误差放在同一个问题中考虑，使得求解更加方便。

小结：

1. 特征点是如何提取并匹配的。
2. 如何通过 $2\mathrm D$-$2\mathrm D$ 的特征点估计相机运动。
3. 如何从 $2\mathrm D$-$2\mathrm D$ 的匹配估计一个点的空间位置。
4. $3\mathrm D$-$2\mathrm D$ 的PnP问题，其线性解法和BA解法。
5. $3\mathrm D$-$3\mathrm D$ 的ICP问题，其线性解法和 BA解法。

第8讲 视觉里程计 2

直接法是视觉里程计的另一个主要分支，它与特征点法有很大不同。

直接法的引出

尽管特征点法在视觉里程计中占据主流地位，但研究者们还是认识到它至少有以下几个缺点：

1. 关键点的提取与描述子的计算非常耗时。实践中，SIFT目前在CPU上是无法实时计算 的，而ORB也需要近20毫秒的计算时长。如果整个SLAM以30毫秒/帧的速度运行,那么一大半时间都将花在计算特征点上。
2. 使用特征点时，忽略了除特征点以外的所有信息。一幅图像有几十万个像素，而特征点只有几百个。只使用特征点丢弃了大部分可能有用的图像信息。
3. 相机有时会运动到特征缺失的地方，这些地方往往没有明显的纹理信息。例如，有时我们会面对一堵白墙，或者一个空荡荡的走廊。这些场景下特征点数量会明显减少，我们可能找不到足够的匹配点来计算相机运动。

克服使用特征点出现的问题思路：

1. 保留特征点，但只计算关键点，不计算描述子。同时，使用光流法（Optical Flow） 跟踪特征点的运动。这样可以回避计算和匹配描述子带来的时间，而光流本身的计算时间要小于描述子的计算与匹配。
2. 只计算关键点，不计算描述子。同时，使用直接法（Direct Method） 计算特征点在下一时刻图像中的位置。这同样可以跳过描述子的计算过程，也省去了光流的计算时间。

第一种方法仍然使用特征点，只是把匹配描述子替换成了光流跟踪，估计相机运动时仍使用对极几何、PnP或ICP算法。这依然会要求提取到的关键点具有可区别性，即需要提到角点。而在直接法中，会根据图像的像素灰度信息同时估计相机运动和点的投影，不要求提取到的点必须为角点。甚至可以是随机的选点。

使用特征点法估计相机运动时，把特征点看作固定在三维空间的不动点。根据它们在相机中的投影位置，通过最小化重投影误差（Reprojection error）优化相机运动。在这个过程中，需要精确地知道空间点在两个相机中投影后的像素位置——这也就是要对特征进行匹配或跟踪的原因。同时，计算、匹配特征需要付出大量的计算量。

相对地，在直接法中，并不需要知道点与点之间的对应关系，而是通过最小化光度误差（Photometric error）来求得它们。

直接法根据像素的亮度信息估计相机的运动，可以完全不用计算关键点和描述子，于是,既避免了特征的计算时间,也避免了特征缺失的情况。只要场景中存在明暗变化（可以是渐变，不形成局部的图像梯度），直接法就能工作。

根据使用像素的数量，直接法分为稀疏、稠密和半稠密三种。与特征点法只能重构稀疏特征点（稀疏地图）相比，直接法还具有恢复稠密或半稠密结构的能力。

2D光流

直接法是从光流演变而来的。它们非常相似，具有相同的假设条件。

光流描述了像素在图像中的运动，而直接法则附带着一个相机运动模型。

光流是一种描述像素随时间在图像之间运动的方法。

随着时间的流逝,同一个像素会在图像中运动，而我们希望追踪它的运动过程。

其中，计算部分像素运动的称为稀疏光流，计算所有像素的称为稠密光流。

稀疏光流以Lucas-Kanade光流（LK光流）为代表，并可以在SLAM中用于跟踪特征点位置。

稠密光流以Horn-Schunck光流为代表。

Lucas-Kanade光流

在LK光流中，认为来自相机的图像是随时间变化的。

图像可以看作时间的函数：$\boldsymbol I(t)$

那么，一个在 $t$ 时刻，位于 $(x,y)$ 处的像素，它的灰度可以写成：

$$\boldsymbol I(x,y,t)$$

这种方式把图像看成了关于位置与时间的函数，它的值域就是图像中像素的灰度。

考虑某个固定的空间点，它在 $t$ 时刻的像素坐标为 $x,y$

由于相机的运动，它的图像坐标将发生变化。我们希望估计这个空间点在其他时刻图像中的位置。

引入光流法的基本假设：

灰度不变假设：同一个空间点的像素灰度值，在各个图像中是固定不变的。

对于 $t$ 时刻位于 $(x,y)$ 处的像素，设 $t+\mathrm{d}t$ 时刻运动到 $(x+\mathrm{d} x,y+\mathrm{d} y)$ 处。

由于灰度不变，有：

$$\boldsymbol{I}(x+\mathrm{d} x, y+\mathrm{d} y, t+\mathrm{d} t)=\boldsymbol{I}(x, y, t)$$

灰度不变假设是一个很强的假设，实际中很可能不成立。

事实上，由于物体的材质不同，像素会出现高光和阴影部分；有时，相机会自动调整曝光参数，使得图像整体变亮或变暗。这时灰度不变假设都是不成立的，因此光流的结果也不一定可靠。

对左边进行泰勒展开，保留一阶项，得：

$$\boldsymbol{I}(x+\mathrm{d} x, y+\mathrm{d} y, t+\mathrm{d} t) \approx \boldsymbol{I}(x, y, t)+\frac{\partial \boldsymbol{I}}{\partial x} \mathrm{~d} x+\frac{\partial \boldsymbol{I}}{\partial y} \mathrm{~d} y+\frac{\partial \boldsymbol{I}}{\partial t} \mathrm{~d} t .$$

因为假设灰度不变，于是下一时刻的灰度等于之前的灰度，从而：

$$\frac{\partial \boldsymbol{I}}{\partial x} \mathrm{~d} x+\frac{\partial \boldsymbol{I}}{\partial y} \mathrm{~d} y+\frac{\partial \boldsymbol{I}}{\partial t} \mathrm{~d} t = 0$$

两边除以 $\mathrm dt$，得：

$$\frac{\partial \boldsymbol{I}}{\partial x} \frac{\mathrm{d} x}{\mathrm{~d} t}+\frac{\partial \boldsymbol{I}}{\partial y} \frac{\mathrm{d} y}{\mathrm{~d} t}=-\frac{\partial \boldsymbol{I}}{\partial t}$$

其中 $\mathrm dx / \mathrm dt$ 为像素在 $x$ 轴上的运动速度，而 $\mathrm dy / \mathrm dt$ 为 $y$ 轴上的速度，把它们记为 $u,v$

同时，$\partial \boldsymbol{I}/\partial x$ 为图像在该点处 $x$ 方向的梯度，另一项则是在 $y$ 方向的梯度，记为 $\boldsymbol I_x,\boldsymbol I_y$

把图像灰度对时间的变化量记为 $\boldsymbol I_t$，写成矩阵形式，有：

$$\left[\begin{array}{ll} \boldsymbol{I}_{x} & \boldsymbol{I}_{y} \end{array}\right]\left[\begin{array}{l} u \\ v \end{array}\right]=-\boldsymbol{I}_{t} .$$

想计算的是像素的运动 $u,v$，但是该式是带有两个变量的一次方程，仅凭它无法计算出 $u,v$

因此，必须引入额外的约束来计算 $u,v$

在LK光流中，假设某一个窗口内的像素具有相同的运动。

考虑一个大小为 $w × w$ 的窗口，它含有 $w^2$ 数量的像素。

该窗口内像素具有同样的运动，因此共有 $w^2$ 个方程：

$$\left[\begin{array}{ll} \boldsymbol{I}_{x} & \boldsymbol{I}_{y} \end{array}\right]_{k}\left[\begin{array}{l} u \\ v \end{array}\right]=-\boldsymbol{I}_{t k}, \quad k=1, \ldots, w^{2}$$

记：

$$\boldsymbol{A}=\left[\begin{array}{c} {\left[\boldsymbol{I}_{x}, \boldsymbol{I}_{y}\right]_{1}} \\ \vdots \\ {\left[\boldsymbol{I}_{x}, \boldsymbol{I}_{y}\right]_{k}} \end{array}\right], \boldsymbol{b}=\left[\begin{array}{c} \boldsymbol{I}_{t 1} \\ \vdots \\ \boldsymbol{I}_{t k} \end{array}\right]$$

于是整个方程为：

$$\boldsymbol{A}\left[\begin{array}{l} u \\ v \end{array}\right]=-\boldsymbol{b}$$

这是一个关于 $u,v$ 的超定线性方程，传统解法是求最小二乘解。最小二乘经常被用到：

$$\left[\begin{array}{l} u \\ v \end{array}\right]^{*}=-\left(\boldsymbol{A}^{\mathrm{T}} \boldsymbol{A}\right)^{-1} \boldsymbol{A}^{\mathrm{T}} \boldsymbol{b} .$$

这样就得到了像素在图像间的运动速度 $u,v$。当 $t$ 取离散的时刻而不是连续时间时，可以估计某块像素在若干个图像中出现的位置。

由于像素梯度仅在局部有效，所以如果一次迭代不够好，会多迭代几次这个方程。

在SLAM 中，LK光流常被用来跟踪角点的运动。

LK光流跟踪能够直接得到特征点的对应关系。这个对应关系就像是描述子的匹配，只是光流对图像的连续性和光照稳定性要求更高一些。可以通过光流跟踪的特征点，用PnP、ICP或对极几何来估计相机运动。

总而言之，光流法可以加速基于特征点的视觉里程计算法，避免计算和匹配描述子的过程，但要求相机运动较平滑（或采集频率较高）。

直接法

直接法的推导

在光流中，会首先追踪特征点的位置，再根据这些位置确定相机的运动。

这样一种两步走的方案，很难保证全局的最优性。

能不能在后一步中，调整前一步的结果呢？

例如，如果认为相机右转了15°，那么光流能不能以这个15°运动作为初始值的假设，调整光流的计算结果呢？

直接法就是遵循这样的思路得到的结果。

如下图所示，考虑某个空间点 $P$ 和两个时刻的相机。

$P$ 的世界坐标为 $[X,Y,Z]$，在两个相机上成像，记像素坐标为 $\boldsymbol p_1,\boldsymbol p_2$

目标是求第一个相机到第二个相机的相对位姿变换。

以第一个相机为参照系，设第二个相机的旋转和平移为 $\boldsymbol R,\boldsymbol t$（对应李群为 $\boldsymbol T$）

同时，两相机的内参相同，记为 $\boldsymbol K$。

为清楚起见，列写完整的投影方程：

$$\begin{array}{l} \boldsymbol{p}_{1}=\left[\begin{array}{l} u \\ v \\ 1 \end{array}\right]_{1}=\frac{1}{Z_{1}} \boldsymbol{K} \boldsymbol{P}, \\ \boldsymbol{p}_{2}=\left[\begin{array}{l} u \\ v \\ 1 \end{array}\right]_{2}=\frac{1}{Z_{2}} \boldsymbol{K}(\boldsymbol{R} \boldsymbol{P}+\boldsymbol{t})=\frac{1}{Z_{2}} \boldsymbol{K}(\boldsymbol{T} \boldsymbol{P})_{1: 3} . \end{array}$$

其中 $Z_{1}$ 是 $P$ 的深度，$Z_{2}$ 是 $P$ 在第二个相机坐标系下的深度，也就是 $\boldsymbol{R} \boldsymbol{P}+\boldsymbol{t}$ 的第 $3$ 个坐标值。由于 $\boldsymbol T$ 只能和齐次坐标相乘，所以乘完之后要取出前3个元素。这和第5讲的内容是一致的。

特征点法中，由于通过匹配描述子知道了 $\boldsymbol p_1,\boldsymbol p_2$ 的像素位置，所以可以计算重投影的位置。

但在直接法中，由于没有特征匹配，无从知道哪一个 $\boldsymbol p_2$ 与 $\boldsymbol p_1$ 对应着同一个点。

直接法的思路是根据当前相机的位姿估计值寻找 $\boldsymbol p_2$ 的位置。

但若相机位姿不够好，$\boldsymbol p_2$ 的外观和 $\boldsymbol p_1$ 会有明显差别。于是，为了减小这个差别，优化相机的位姿，来寻找与 $\boldsymbol p_1$ 更相似的 $\boldsymbol p_2$

通过解一个优化问题完成，但此时最小化的不是重投影误差，而是光度误差，也就是 $P$ 的两个像素的亮度误差：

$$e=\boldsymbol{I}_{1}\left(\boldsymbol{p}_{1}\right)-\boldsymbol{I}_{2}\left(\boldsymbol{p}_{2}\right)$$

这里的 $e$ 是一个标量。同样地，优化目标为该误差的二范数，暂时取不加权的形式，为：

$$\min _{\boldsymbol T} J(\boldsymbol T)=\|e\|^{2}$$

能够做这种优化的理由，仍是基于灰度不变假设。

假设一个空间点在各个视角下成像的灰度是不变的。

有许多个（比如 $N$ 个）空间点 $P_i$，那么，整个相机位姿估计问题变为：

$$\min _{\boldsymbol{T}} J(\boldsymbol{T})=\sum_{i=1}^{N} e_{i}^{\mathrm{T}} e_{i}, \quad e_{i}=\boldsymbol{I}_{1}\left(\boldsymbol{p}_{1, i}\right)-\boldsymbol{I}_{2}\left(\boldsymbol{p}_{2, i}\right)$$

优化变量是相机位姿 $\boldsymbol T$，不像光流那样优化各个特征点的运动。

求解这个优化问题，误差 $e$ 是如何随着相机位姿 $\boldsymbol T$ 变化的，定义两个中间变量：

$$\begin{aligned} \boldsymbol{q} &=\boldsymbol{T} \boldsymbol{P} \\ \boldsymbol{u} &=\frac{1}{Z_{2}} \boldsymbol{K} \boldsymbol{q} \end{aligned}$$

$\boldsymbol{q}$ 为 $P$ 在第二个相机坐标系下的坐标，$\boldsymbol{u}$ 为它的像素坐标。

$\boldsymbol{q}$ 是 $\boldsymbol T$ 的函数，$\boldsymbol{u}$ 是 $\boldsymbol{q}$ 的函数，从而也是 $\boldsymbol T$ 的函数。

考虑李代数的左扰动模型，利用一阶泰勒展开，因为：

$$e(\boldsymbol{T})=\boldsymbol{I}_{1}\left(\boldsymbol{p}_{1}\right)-\boldsymbol{I}_{2}(\boldsymbol{u})$$

所以：

$$\frac{\partial e}{\partial \boldsymbol{T}}=\frac{\partial \boldsymbol{I}_{2}}{\partial \boldsymbol{u}} \frac{\partial \boldsymbol{u}}{\partial \boldsymbol{q}} \frac{\partial \boldsymbol{q}}{\partial \delta \boldsymbol{\xi}} \delta \boldsymbol{\xi}$$

其中 $\delta \boldsymbol{\xi}$ 为 $\boldsymbol T$ 的左扰动

一阶导数由于链式法则分成了 $3$ 项：

1. $\partial \boldsymbol{I}_{2}/\partial \boldsymbol{u}$ 为 $\boldsymbol{u}$ 处的像素梯度

2. $\partial \boldsymbol{u}/\partial \boldsymbol{q}$ 为投影方程关于相机坐标系下的三维点的导数。记 $\boldsymbol{q}=[X,Y,Z]^\mathrm T$，根据第 $7$ 讲的推导，导数为：

   $$\frac{\partial \boldsymbol{u}}{\partial \boldsymbol{q}}=\left[\begin{array}{ccc} \frac{\partial u}{\partial X} & \frac{\partial u}{\partial Y} & \frac{\partial u}{\partial Z} \\ \frac{\partial v}{\partial X} & \frac{\partial v}{\partial Y} & \frac{\partial v}{\partial Z} \end{array}\right]=\left[\begin{array}{ccc} \frac{f_{x}}{Z} & 0 & -\frac{f_{x} X}{Z^{2}} \\ 0 & \frac{f_{y}}{Z} & -\frac{f_{y} Y}{Z^{2}} \end{array}\right] .$$

3. $\partial \boldsymbol{q}/\partial \delta \boldsymbol{\xi}$ 为变换后的三维点对变换的导数，在李代数一讲介绍过：

$$\frac{\partial \boldsymbol{q}}{\partial \delta \boldsymbol{\xi}}=\left[\boldsymbol{I},-\boldsymbol{q}^{\wedge}\right]$$

在实践中，由于后两项只与三维点 $q$ 有关，而与图像无关，所以合并在一起：

$$\frac{\partial \boldsymbol{u}}{\partial \delta \boldsymbol{\xi}}=\left[\begin{array}{cccccc} \frac{f_{x}}{Z} & 0 & -\frac{f_{x} X}{Z^{2}} & -\frac{f_{x} X Y}{Z^{2}} & f_{x}+\frac{f_{x} X^{2}}{Z^{2}} & -\frac{f_{x} Y}{Z} \\ 0 & \frac{f_{y}}{Z} & -\frac{f_{y} Y}{Z^{2}} & -f_{y}-\frac{f_{y} Y^{2}}{Z^{2}} & \frac{f_{y} X Y}{Z^{2}} & \frac{f_{y} X}{Z} \end{array}\right] .$$

这是第7讲出现过的 $2×6$ 的矩阵，于是推导出误差相对于李代数的雅可比矩阵：

$$\boldsymbol{J}=-\frac{\partial \boldsymbol{I}_{2}}{\partial \boldsymbol{u}} \frac{\partial \boldsymbol{u}}{\partial \delta \boldsymbol{\xi}}$$

对于 $N$ 个点的问题，可以用这种方法计算优化问题的雅可比矩阵，然后使用高斯牛顿法或列文伯格—马夸尔特方法计算增量，迭代求解。

直接法的讨论

在上面的推导中，$P$ 是一个已知位置的空间点，它是怎么来的呢？

在RGB-D相机下，可以把任意像素反投影到三维空间，然后投影到下一幅图像中。

如果在双目相机中，那么同样可以根据视差来计算像素的深度。

如果在单目相机中，还须考虑由 $P$ 的深度带来的不确定性。

在 $P$ 深度已知的情况下，根据 $P$ 的来源，可以把直接法进行分类：

1. $P$ 来自于稀疏关键点，称之为稀疏直接法。通常，使用数百个至上千个关键点，并且像L-K光流那样，假设它周围像素也是不变的。这种稀疏直接法不必计算描述子，并且只使用数百个像素，因此速度最快，但只能计算稀疏的重构。
2. $P$ 来自部分像素。式 $\boldsymbol{J}=-\frac{\partial \boldsymbol{I}_{2}}{\partial \boldsymbol{u}} \frac{\partial \boldsymbol{u}}{\partial \delta \boldsymbol{\xi}}$ 中，如果像素梯度为零，那么整项雅可比矩阵就为零，不会对计算运动增量有任何贡献。因此，可以考虑只使用带有梯度的像素点，舍弃像素梯度不明显的地方。这称为半稠密（Semi-Dense）的直接法，可以重构一个半稠密结构。
3. $P$ 为所有像素，称为稠密直接法。稠密重构需要计算所有像素（一般几十万至几百万个）,因此多数不能在现有的CPU上实时计算，需要GPU的加速。但是，如前面讨论的，像素梯度不明显的点，在运动估计中不会有太大贡献，在重构时也会难以估计位置。

可以看到，从稀疏到稠密重构，都可以用直接法计算。它们的计算量是逐渐增长的。

稀疏方法可以快速地求解相机位姿，而稠密方法可以建立完整地图。

具体使用哪种方法，需要视机器人的应用环境而定。

特别地，在低端的计算平台上，稀疏直接法可以做到非常快速的效果，适用于实时性较高且计算资源有限的场合。

直接法优缺点总结

相比于特征点法，直接法完全依靠优化来求解相机位姿。

从式 $\boldsymbol{J}=-\frac{\partial \boldsymbol{I}_{2}}{\partial \boldsymbol{u}} \frac{\partial \boldsymbol{u}}{\partial \delta \boldsymbol{\xi}}$ 中可以看到，像素梯度引导着优化的方向。

如果想要得到正确的优化结果，就必须保证大部分像素梯度能够把优化引导到正确的方向。

大体上，优点如下：

1. 可以省去计算特征点、描述子的时间。
2. 只要求有像素梯度即可，不需要特征点。因此，直接法可以在特征缺失的场合下使用。比较极端的例子是只有渐变的一幅图像。它可能无法提取角点类特征，但可以用直接法估计它的运动。直接法对随机选取的点亦能正常工作。这一点在实用中非常关键，因为实用场景很有可能没有很多角点可供使用。
3. 可以构建半稠密乃至稠密的地图，这是特征点法无法做到的。

缺点也很明显：

1. 非凸性。直接法完全依靠梯度搜索，降低目标函数来计算相机位姿。其目标函数中需要取像素点的灰度值，而图像是强烈非凸的函数。这使得优化算法容易进入极小，只在运动很小时直接法才能成功。针对于此，金字塔的引入可以在一定程度上减小非凸性的影响。
2. 单个像素没有区分度。于是要么计算图像块，要么计算复杂的相关性。由于每个像素对改变相机运动的“意见”不一致，只能少数服从多数，以数量代替质量。所以，直接法在选点较少时的表现下降明显，通常建议用500个点以上。
3. 灰度值不变是很强的假设。如果相机是自动曝光的，当它调整曝光参数时，会使得图像整体变亮或变暗。光照变化时也会出现这种情况。特征点法对光照具有一定的容忍性，而直接法由于计算灰度间的差异，整体灰度变化会破坏灰度不变假设，使算法失败。针对这一点，实用的直接法会同时估计相机的曝光参数，以便在曝光时间变化时也能工作。