特征点法前端

前端又称为视觉里程计 (VO)，它根据相邻图像间的信息来估计出相机的运动。估计值既可作为结果输出，也可以作为初始值提供给后端来进行优化。VO 的实现，按照是否提取图像特征，分为特征点法前端和直接法前端。本讲介绍的是特征法前端。

特征点与特征点匹配

如前所述，VO 的主要问题是根据图像信息来估计相机的运动。一般来说，我们首先从图像中选取出比较有代表性的点，然后根据这些点来估计相机的位姿（和点的定位）。在SLAM 中，这些点也称为路标。

特征点

影像在计算机中是以数值矩阵的方式来进行存储的。因此，单个像素也是一种特征。但我们希望所提取的特征能够在相机运动后保持稳定，即有一定程度的不变性。而单个像素往往收到光照、视角、形变等等因素的影响而变得不稳定。因此，在计算机视觉中，常常通过人工设计的特征提取器来获取具有鲁棒性的图像特征。

常见的图像特征就是角点。角点有易辨认易提取的特点。但它也存在一些问题。比如距离的影响：从远处看是角点的地方，相机移近之后却不是了。还有旋转的影响：相机旋转后，不同影像上的同一角点可能就具有不同的外观。

为此，研究者们设计了许多能够提取具有足够鲁棒性特征提取算法，如SIFT, SURE, ORB 等等。这些人工设计的特征点一般都具备如下性质：

1. 可重复性：相同的特征点可以在不同的影像上找到；
2. 可区别性：不同的特征点具有不同的表达；
3. 高效率：同一图像中，特征点的数量远小于像素的数量；
4. 本地性：提取的特征点仅仅和一小片图像区域相关。

一个特征点由关键点 (key-point) 和描述子 (descriptor) 两部分组成。关键点指的是该特征点在图像上的位置信息，有些还具有方向、大小等其他信息。描述子通常是一个向量，它按照某种人为设计的方式，描述了特征点周围像素点信息。描述子点一个重要设计原则就是外观相似的特征具有相似的描述子。

由于SLAM 是一种实时应用，因此除了鲁棒性之外，算法的实时性也应该被考虑。实际上，特征点提取和匹配占据了SLAM 主要的时间消耗。因此选用合适的特征提取算法至关重要。如SIFT 算子虽好，但计算量太大，时间消耗过多。虽然大部分的特征提取都具有良好的并行性，可以使用GPU 来加速运算，但由此带来的成本提升也要纳入考量。而ORB算子是质量和效率之间比较好的折中方案，常被用在目前的视觉SLAM 方案中。

ORB

网上关于ORB 算子的资料很多，相关论文也可以直接获取。这里仅仅进行一个简要的叙述。

ORB 特征一样由关键点和描述子两部分组成。它的关键点为Oriented FAST，是FAST 算子的一种改进；描述子则是BRIEF。

FAST 算子很高效，但不具备尺度和旋转不变性。通过构建影像金字塔，在不同尺度的影像上提取特征来增加尺度不变性。然后引入像素重心来确定特征点的方向，引入旋转不变性。中间还可以使用Harris 角点滤波来提取出N 个最有可能的角点。

传统的BRIEF 描述子一样不具备旋转不变性，同样通过像素重心所确定的特征点方向来作为描述子点方向，得到steer BRIEF。最后，为了得到更好的两两比较模式，利用一个角点数据集和贪心算法得到一个具有高方差、低相关的模式，用以构建合适的描述子，称为rBRIEF。

原论文

特征匹配

完成特征提取后，就可以进行特征匹配了。特征匹配解决了SLAM 中的数据关联问题，即确定了当前看到的路标和之前看到的路标之间的对应关系。

最简单的特征匹配方法就是暴力匹配 (brute-force matching)：计算待匹配特征点与其他特征点之间的距离，然后按距离排序，选取距离最近的特征点作为匹配点。在这里，描述子间的距离表示了两个特征点之间的相似程度，有欧式距离，汉明距离等等。对于特征点数量巨大的情况，快速近似最邻近 (FLANN) 算法会更为高效。

接下来，我们希望根据匹配的特征点对来估计相机的运动。根据相机原理或所有的数据等不同，有三种情况：

1. 当使用单目相机时，我们只知道二维的像素坐标，因此问题是根据两组匹配的2D 点来估计相机运动。该问题用对极几何来解决。
2. 当相机为双目或为RGB-D 相机时，由于我们可以获得深度信息，问题就是估计两组3D 点间的运动。该问题用ICP 来解决。
3. 如果有3D 点云及其对应像素点的2D 坐标，也能顾及相机运动。该问题通过PnP 求解。

对极几何

对极约束

上图展示了一对匹配好的特征点。我们希望求取这两帧之间的运动。设两个相机关心分别为O1 和O2，第一帧到第二帧到运动为R，t。点p1 (x1) 和点p2 (x2) 是同一个空间点在两个成像平面上的投影。连线O1p1 和O2p2 在三维空间中相交于点P。这时，O1, O2 和P 三点确定一个平面，称为极平面 (epipolar plane)。O1O2 连线与像平面I1, I2 的交点分别为e1, e2。点e1, e2 称为极点(epipoles)，是相机光心在另一幅影像上的投影。注意到这里e1, e2 都位于像平面内。有时候它们有可能会落在成像平面之外。O1O2 称为基线 (baseline)。而极平面与两个像平面之间的交线l1, l2 为极线 (epipolar line)，它们分别是射线O2p2 和O1p2 在对方影像上的投影。

从几何上来看，射线O1p1 是像素点p1 所对应的物方点可能出现的位置：该射线上的所有点都有可能投影到点p1 上。射线O1p2 是像素点p2 所对应的物方点可能出现的位置。如果匹配正确的话，像素点p1, p2 对应于同一个物方点。这两条射线的交点就是就是点P 的空间位置。如果没有特征匹配，我们就必须在极线l2 上搜索p1 的匹配点。

现在我们从代数的角度上看，在第一帧的相机坐标系下，点P 的空间位置为：

根据针孔相机模型，不考虑畸变，两个像素点p1, p2 点像素坐标分别为：

这里，K 为相机内参矩阵。如果使用齐次坐标，则前面的系数s1, s2 可以省略。设：

这里，x1 和x2 分别为两个像素点在各自相机坐标系下的归一化平面坐标。将之代入式(2) 可得：

将上式两边同时左乘  ，这相当于两侧同时和t 做外积：

再将两侧同时左乘  ：

注意到  是一个垂直于二者的向量，因此它和x2 的内积为0。由此可得：

如果我们代入p1, p2 则可得：

这两个式子（式(7) 和式(8)）称为对极约束。它的几何意义为O1, O2 和P 三点共面。这两个式子的中间部分分别称为本质矩阵 (essential matrix) E 和基础矩阵 (fundamental matrix) F。

对极约束给出了两个匹配点的空间位置关系。E 和F 之间只差了相机内参。在SLAM 中，相机内参一般都是已知的（也可以通过相机标定获得），所以实践中常常使用形式更简单的E。注意到E 完全有旋转矩阵R 和平移向量t 组成，由此我们也希望能够通过E 来求得R, t。因此，相机位姿的估计就可以描述为：

1. 通过匹配点求出E；
2. 根据E 求取R, t。

实际情况自然会比这个复杂。下面我们就来了解下这个求解过程。

本质矩阵

关于本大节（对极几何）的更详细的讲解和推导推荐看书 An Invitation to 3-D vision 的第五章。这章也正好是sample chapters 之一，可以免费阅读。地址在此

.

本质矩阵  是一个3 * 3 大小的矩阵，共9个未知数。它包含了一个相对位置信息t 和一个旋转矩阵R。所有本质矩阵也构成一个集合，具有以下的性质：

• 本质矩阵是由对极约束定义的。由上可知对极约束是一个等式为零的约束，所以对E 乘以任意非零常数后，对极约束仍然满足。说明E 在不同尺度下是等价的。
• 根据  ，本质矩阵E 的奇异值必定是  的形式。这称为本质矩阵的内在性质。想要详细的证明还请看上面的章样。
• 平易和旋转各自有3 个自由度，所以 一共只有6个自由度。考虑到尺度等价性，本质矩阵E 实际上只有5个自由度。

既然E 只有5个自由度，说明我们可以只用5对点来对其进行求解。但E 的内在性质是非线性的，只用5对点求解会更麻烦些。考虑E 的尺度等价性，可以使用8对点来求解E。这就是八点法。

考虑一对匹配点，它们的归一化坐标为  和  。根据对极约束则有：

把矩阵E 展开，写成向量的形式 (stacked version)：

对极约束就可以写成与e 有关的线形形式：

8个特征点对就构成了一个线性方程组。设系数矩阵为A，它是一个8 * 9 大小的矩阵，e 位于该矩阵的零空间 (Null Space) 中。如果矩阵A 满足秩为8（满秩）的条件（8个点不共面），那么其零空间维度为1，即e 构成一条线，这与e 的尺度等价性是一致的。

求解出E 后，问题就变成了如何从E 中恢复出相机的运动R，t。该过程可以由奇异值分解 (SVD) 得到。且对于任意一个本质矩阵E，有两组相对运动 (R, t) 与之对应。同样地，详细证明请看上面的书籍章样。假设E 的SVD 分解为：

其中，U, V 是正交阵，  是奇异值矩阵。与E 对应的两组 (R, t) 分别为：

其中， 表示沿z 轴旋转90度的旋转矩阵。对比上面两个式子可以发现，这两组解其实是以参考帧为中心，绕z 轴呈180度旋转对称的两组解，如下图所示（来自上面推荐的书籍）：

同时，由于E 可以取任意符号，即 -E 和 E 是等价的，所以对任意一个 t 取负号又取得一个符合条件的解，所以一共有四组符合条件的解。

我们可以将任意一对特征点代入所取得的4组解中，检测该点在两个相机下的深度值。显然物方特征点应该位于两个相机的前方，取两个深度值都为正的解即是正确的解。

最后，使用带有噪声的数据利用线形方程组求解得到的E 可能并不是一个”正确“的解，即奇异值矩阵并不满足E 的内在性质  而是  ，为从大到小的排序。通常的做法是取  或者直接取 (1, 1, 0)。

单应矩阵

前面我们提到，利用八点法来求解本质矩阵的一个前提是系数矩阵满秩，这也意味着八组特征点不能（近似）落在同一个平面上。但在一些情况中，如无人机俯拍影像，这个假设就不成立了。此时，可以利用单应矩阵 (Homography) H 来求解相机运动。

考虑图像I1 和I2 有匹配好的特征点对p1 和p2，这些特征点所对应的物方点P 落在同一平面上。以第一张影像的相机坐标系为惯性系，该平面的法向量为n，到惯性系的原点的距离为d，则该平面可以表示为：

整理得：

影像I2 相对于影像I1 的运动为 (R, t)，则有：

这样，我们就得到了描述两个相机坐标系下同一物方点的转换关系，把中间括号内的部分抽取出来就得到了单应矩阵H。当然，也可以在括号两端各加上相机矩阵K，得到 ，这是高博书中的表示方式，描述了两个图像坐标之间的转换关系。

单应矩阵包含了相机运动信息 (R, t) 和对应平面的参数 (n, d)，同样是一个3 * 3 大小的矩阵，同样可以先通过匹配的特征点对计算H 然后将之分解得到平移和旋转。值得注意的是，若相机运动为纯旋转，情形仍和单应矩阵相同，因为此时  或  。可见，旋转矩阵也是单应矩阵的一种。

由上可得：

需要注意的是这里的等号是在一个非零因子下成立的（齐次坐标）。实际处理中常常乘以一个非零因子使得h9 = 1。然后去掉这个非零因子可得：

整理得：

如此，一组匹配点可以构造出两个约束（三组约束中只有两组线性独立）。于是，自由度为8 的单应矩阵可以由4对特征点算出（不存在三点共线的情况）。这种将H 转化为向量形式来直接求解的方式称为直接线性变换 (Direct Linear Transform, DLT)。

和本质矩阵的分解类似，分解单应矩阵H 也会得到4组解（如下所示）。这里的推导比较复杂（我也没看得很明白），还请参看前面的书籍章样。利用物方点的深度值为正（位于相机前方）的特性，可以排除两组解，剩下的两组解则需通过其他先验信息进行验证。

补充

尺度不确定性

前面提到，E 具有尺度等价性，由它分解得到的 (R, t) 也具有一个尺度等价性，但由于旋转矩阵R 自身带有约束（行列式为1等），所以只有t 具有一个尺度。换言之，分解E 得到的其实是qt, q 为一个分零因子。而在通过分解H 得到 (R, t) 的时候，由于平面到坐标原点的距离未知，得到的t 同样具有一个尺度等价性。在这种情况下，通常是将t 进行归一化处理，即令其模长为1.

对t 长度的归一化直接导致了单目视觉的尺度不确定性。如果对轨迹和地图同时缩放任意倍数，我们得到的图像仍然是一样的。而对两张图像间的平移t 进行归一化相当于固定尺度。以t 的长度作为为单位长度，计算相机轨迹和特征点的三维位置。这被称为单目slam 的初始化。初始化后，就可以利用3D - 2D 来计算相机运动了。进行初始化的两张图像必须有一定程度的平移，而后都将以此步的平移为单位。

纯旋转

在只有纯旋转的情况下，我们可以通过H 来秋去旋转。此时由于t = 0，E 也为0。但此时我们无法利用三角测量来计算特征点的空间位置（不构成对极几何）。所以，单目初始化不能只有纯旋转，必须有一定程度的平移。

多余匹配

求解E 和H 都只需要用到少量的特征点对。而通过特征提取和匹配，往往能获得远超需要的特征点对。拥有这么多匹配点，在求解E 或H 的过程中当然可以构造一个最小二乘问题。但是，在可能存在误匹配的情况下，随机采样一致性 (RANSAC) 更受青睐。

三角测量

在使用对极约束估计了相机运动之后，利用估计而得的相机位姿和匹配点对，可以通过三角测量来计算特征点的深度信息。

考虑图像I1 和I2，以I1 为参考，图像I2 的位姿为T12， 表示第二幅影像到第一幅影像的变换关系。相机光心为O1 和O2。现在有特征点对p1 和p2。理论上射线O1p1 和射线O2p2 会在物方空间中交会于一点P。但由于噪声的影像，这两条射线往往无法相交，需要利用最小二乘法来进行求解。

设x1 和x2 分别为p1 和p2 的归一化坐标，则有：

现在，我们已经通过对极约束的到了R 和t，想要求解两个特征点的深度s1 和s2。先来求s2，对上式两侧坐乘  ，有

上式可以看成是s2 的一个方程。但由于噪声的存在，之前求得的R 和t 并不一定使其精确为0。所以更常见的做法是求最小二乘解。

补充

三角测量是由平移得到的，只有平移才会有对极几何中的三角形 (即三角形O1 O2 P)。因此，纯旋转是无法进行三角测量的，因为此时对极约束永远满足。

而当平移存在时，也会出现三角测量的矛盾。回顾 相机与图像 一讲，深度值  ，这里b 为基线长度，也就是平移t，而b 为视差，反映了特征点的测量精度。假设特征点测量有一个小误差  时，视线角度会变化一个角度  ，使得深度测量值发生  的变化。当平移很小时，  的变化较大；而当平移较大时，  将明显变小。如下图所示。

所以，要提高三角测量的精度，一是提高特征点的提取精度，亦即提高视差的精度，这也就提高图像的分辨率，但这就会使得图像变大，增加计算成本；二是增大平移量。但这也会使得图像外观发生明显变化，可能导致匹配失败。

总而言之，增大平移，可能导致匹配失败；平移太小，则导致三角测量精度不够。这就是三角化的矛盾。