slam入门——十四讲笔记（六）

第7讲 视觉里程计1

【本节目标】

1. 理解图像特征点的意义，并掌握在单幅图像中提取特征点，及多幅图像中匹配特征点的方法。
2. 理解对极几何的原理，利用对极几何的约束，恢复出图像之间的摄像机的三维运动。
3. 理解PNP问题，及利用已知三维结构的图像的对应关系，求解摄像机的三维运动。
4. 理解ICP问题，及利用点云的匹配关系，求解摄像机的三维运动。
5. 理解如何通过三角化，获得二维图像上对应的三维结构。

本书之前的内容，介绍了运动方程和观测方程的具体形式，并讲解了以非线性优化为主的求解方法。从本讲开始，我们结束了基础知识的铺垫，开始步入正题：按照第二讲的内容，分别介绍视觉里程计、优化后端、回环检测和地图构建四个模块。本讲和下一讲主要介绍作为视觉里程计的主要理论，然后在第九章中进行一次实践。本讲关注基于特征点的方式的视觉里程计算法。我们将介绍什么是特征点，如何提取和匹配特征点，以及如何根据配对的特征点估计相机运动。

7.1 特征点法

回顾第二讲的内容，我们说过视觉 SLAM 主要分为视觉前端和优化后端。前端也称为视觉里程计（VO）。它根据相邻图像的信息，估计出粗略的相机运动，给后端提供较好的初始值。

VO 的实现方法，按是否需要提取特征，分为特征点法的前端以及不提特征的直 接法前端。基于特征点法的前端，长久以来（直到现在）被认为是视觉里程计的主流方法。 它运行稳定，对光照、动态物体不敏感，是目前比较成熟的解决方案。在本讲中，我们将 从特征点法入手，学习如何提取、匹配图像特征点，然后估计两帧之间的相机运动和场景 结构，从而实现一个基本的两帧间视觉里程计。

7.1.1 特征点

7.1.2 ORB特征

ORB 特征亦由关键点和描述子两部分组成。它的关键点称为“Oriented FAST”，是 一种改进的 FAST 角点，什么是 FAST 角点我们将在下文介绍。它的描述子称为 BRIEF （Binary Robust Independent Elementary Features）。因此，提取 ORB 特征分为两个步骤：

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

下面我们分别介绍 FAST 和 BRIEF。

FAST关键点
FAST 是一种角点，主要检测局部像素灰度变化明显的地方，以速度快著称。它的思想是：如果一个像素与它邻域的像素差别较大（过亮或过暗）, 那它更可能是角点。相比于 其他角点检测算法，FAST 只需比较像素亮度的大小，十分快捷。它的检测过程如下（见 图 7-3 ）：

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 个像 素的亮度。只有当这四个像素中有三个同时大于 $I_p+T$ 或小于 $I_p-T$ 时，当前像素才有可能是一个角点，否则应该直接排除。这样的预测试操作大大加速了角点检测。此外，原始的 FAST 角点经常出现“扎堆”的现象。所以在第一遍检测之后，还需要用非极大值抑制（Non-maximal suppression），在一定区域内仅保留响应极大值的角点，避免角点集中 的问题。

FAST 特征点的计算仅仅是比较像素间亮度的差异，速度非常快，但它也有一些问题。首先，FAST 特征点数量很大且不确定，而我们往往希望对图像提取固定数量的特征。因 此，在 ORB 中，对原始的 FAST 算法进行了改进。我们可以指定最终要提取的角点数量 N，对原始 FAST 角点分别计算 Harris 响应值，然后选取前 N 个具有最大响应值的角点， 作为最终的角点集合。

其次，FAST 角点不具有方向信息。而且，由于它固定取半径为 3 的圆，存在尺度问题： 远处看着像是角点的地方，接近后看可能就不是角点了。针对 FAST 角点不具有方向性和尺度的弱点，ORB 添加了尺度和旋转的描述。尺度不变性由构建图像金字塔，并在金字塔的每一层上检测角点来实现.。特征的旋转是由灰度质心法（Intensity Centroid）实现 的。我们稍微介绍一下。

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

1. 在一个小的图像块B中，定义图像块的矩为：
   $$m_{pq}=\sum _{x,y\in B}{x}^p{y}^{q}I(x,y),p,q=\left\{0,1\right\}.$$
2. 通过矩可以找到图像块的质心：
   $$C=(\frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}}).$$
3. 连接图像块的几何中心O与质心C，得到一个方向向量$OC$，于是特征点的方向可以定义为：
   $$\theta =\arctan (m_{01}/m_{10}).$$

通过以上方法，FAST角点便具有了尺度与旋转的描述，大大提升了它们在不同图像之间表述的鲁棒性。所以在ORB中，把这种改进后的FAST称为Oriented FAST。

BRIEF描述子
在提取 Oriented FAST 关键点后，我们对每个点计算其描述子。ORB 使用改进的BRIEF 特征描述。我们先来讲 BRIEF 是什么。

BRIEF 是一种二进制描述子，它的描述向量由许多个 0 和 1 组成，这里的 0 和 1 编 码了关键点附近两个像素（比如说 p 和 q）的大小关系：如果 p 比 q 大，则取 1，反之就 取 0。如果我们取了 128 个这样的 p, q，最后就得到 128 维由 0，1 组成的向量。那么，p 和 q 如何选取呢？在作者原始的论文中给出了若干种挑选方法，大体上都是按照某种概率 分布，随机地挑选 p 和 q 的位置，读者可以阅读 BRIEF 论文或 OpenCV 源码以查看它 的具体实现。

BRIEF 使用了随机选点的比较，速度非常快，而且由于使用了二进制表 达，存储起来也十分方便，适用于实时的图像匹配。原始的 BRIEF 描述子不具有旋转不 变性的，因此在图像发生旋转时容易丢失。而 ORB 在 FAST 特征点提取阶段计算了关键 点的方向，所以可以利用方向信息，计算了旋转之后的“Steer BRIEF”特征，使 ORB 的 描述子具有较好的旋转不变性。

由于考虑到了旋转和缩放，使得 ORB 在平移、旋转、缩放的变换下仍有良好的表现。同时，FAST 和 BRIEF 的组合也非常的高效，使得 ORB 特征在实时 SLAM 中非常受欢 迎。我们在图 7-2 中展示了一张图像提取 ORB 之后的结果，下面来介绍如何在不同的图 像之间进行特征匹配。

7.1.3 特征匹配

特征匹配是视觉 SLAM 中极为关键的一步，宽泛地说，特征匹配解决了 SLAM 中的 数据关联问题（data association），即确定当前看到的路标与之前看到的路标之间的对应关系。

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

然而，由于图像特征的局部特性，误匹配的情况广泛存在，而且长期以来一直没有得到有效解决，目前已经成为视觉 SLAM 中制约性能 提升的一大瓶颈。

部分原因是因为场景中经常存在大量的重复纹理，使得特征描述非常相 似。在这种情况下，仅利用局部特征解决误匹配是非常困难的。

不过，让我们先来看正确匹配的情况，等做完实验再回头去讨论误匹配问题。考虑两 个时刻的图像。如果在图像 $I_t$ 中提取到特征点$x_t^m,m=1,2,\cdots ,M$，在图像$I_{t+1}$中提取到特征点KaTeX parse error: Undefined control sequence: \codts at position 22: …}^n, n = 1, 2, \̲c̲o̲d̲t̲s̲, N，如何寻找这两个集合元素的对应关系呢？最简单的特征匹配方法就是暴力匹配（Brute-Force Matcher）。即对每一个特征点$x_t^m$，与所有的$x_{t+1}^m$测量描述子的距离，然后排序，取最近的一个作为匹配点。描述子距离表示了两个特征之间的相似程度，不过在实际运用中还可以取不同的距离度量范数。

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

然而，当特征点数量很大时，暴力匹配法的运算量将变得很大，特别是当我们想要匹配一个帧和一张地图的时候。这不符合我们在 SLAM 中的实时性需求。

此时**快速近似最近邻（FLANN）**算法更加适合于匹配点数量极多的情况。由于这些匹配算法理论已经成 熟，而且实现上也已集成到 OpenCV，所以我们这里就不再描述它的技术细节了。

7.2 实践：特征提取和匹配

目前主流的几种图像特征在 OpenCV 开源图像库中都已经集成完毕，我们可以很方便地进行调用。下面我们来实际练习一下 OpenCV 的图像特征提取、计算和匹配的过程。 我们为此实验准备了两张图像，位于 slambook/ch7/下的 1.png 和 2.png，如图 7-5 所示。

本节演示如何提取ORB特征，并进行匹配。下个程序将演示如何估计相机运动。

特征提取与匹配代码：

#include 
#include 
#include 
#include 

using namespace std;
using namespace cv;

int main(int argc, char** argv) {
    if ( argc != 3 )
    {
        cout<<"usage: feature_extraction img1 img2"<<endl;
        return 1;
    }
    //-- 读取图像
    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    //-- 初始化
    std::vector<KeyPoint> keypoints_1, keypoints_2;
    Mat descriptors_1, descriptors_2;
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // Ptr detector = FeatureDetector::create(detector_name);
    // Ptr descriptor = DescriptorExtractor::create(descriptor_name);
    Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create ( "BruteForce-Hamming" );

    //-- 第一步：检测Oriented FAST 角点位置
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);

    //-- 第二步：根据角点位置计算 BRIEF 描述子
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);

    Mat outimg1;
    drawKeypoints(img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT);
    imshow("ORB特征点", outimg1);

    //-- 第三步：对两幅图像中的BRIEF描述子进行匹配，使用Hamming距离
    vector<DMatch> matches;
    //BFMatcher matcher (NORM_HAMMING);
    matcher->match(descriptors_1, descriptors_2, matches);

    //-- 第四步：匹配点筛选
    double min_dist = 10000, max_dist = 0;

    // 找出所有匹配之间的最小距离和最大距离，即是最相似的和最不相似的两组点之间的距离
    for(int i=0; i<descriptors_1.rows; i++) {
        double dist = matches[i].distance;
        if(dist < min_dist) min_dist = dist;
        if(dist > max_dist) max_dist = dist;
    }
    // 仅供娱乐的写法
    min_dist = min_element( matches.begin(), matches.end(), [](const DMatch& m1, const DMatch& m2) {return m1.distance<m2.distance;} )->distance;
    max_dist = max_element( matches.begin(), matches.end(), [](const DMatch& m1, const DMatch& m2) {return m1.distance<m2.distance;} )->distance;

    printf ( "-- Max dist : %f \n", max_dist );
    printf ( "-- Min dist : %f \n", min_dist );

    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    std::vector< DMatch > good_matches;
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        if ( matches[i].distance <= max ( 2*min_dist, 30.0 ) )
        {
            good_matches.push_back ( matches[i] );
        }
    }

    //-- 第五步：绘制匹配结果
    Mat img_match;
    Mat img_goodmatch;
    drawMatches(img_1, keypoints_1, img_2, keypoints_2, matches, img_match);
    drawMatches(img_1, keypoints_1, img_2, keypoints_2, good_matches, img_goodmatch);
    imshow ( "所有匹配点对", img_match );
    imshow ( "优化后匹配点对", img_goodmatch );
    waitKey(0);

    return 0;
}

图 7-6 显示了例程的运行结果。我们看到未筛选的匹配中带有大量的误匹配。经过一次筛选之后，匹配数量减少了许多，但大多数匹配都是正确的。

这里，我们筛选的依据是汉明距离小于最小距离的两倍，这是一种工程上的经验方法，不一定有理论依据。不过，尽管在示例图像中能够筛选出正确的匹配，但我们仍然不能保证在所有其他图像中得到的匹配全是正确的。因此，在后面的运动估计中，还需要使用去除误匹配的算法。

接下来，我们希望根据匹配的点对，估计相机的运动。这里由于相机的原理不同，情况发生了变化：

1. 当相机为单目时，我们只知道2D的像素坐标，因而问题是根据两组2D点估计运动。该问题用对极几何来解决。
2. 当相机为双目、RGB-D时，或者我们通过某种按方法得到了距离信息，那问题就是根据两组3D点估计运动。该问题通常用ICP来解决。
3. 如果我们有3D点和它们在相机的投影位置，也能估计相机的运动。该问题通过PnP求解。

因此，下面几节的内容，我们就来介绍这三种情形下的相机运动估计。我们将从最基本的2D-2D情形出发，看看它如何求解，求解过程又具有哪些麻烦的问题。

7.3 2D-2D：对极几何

7.3.1 对极约束

现在，假设我们从两张图像中，得到了一对配对好的特征点，像图 7-7 里显示的那样。

如果我们有若干对这样的匹配点，就可以通过这些二维图像点的对应关系，恢复出在两帧 之间摄像机的运动。这里“若干对”具体是多少对呢？我们会在下文介绍。先来看看两个 图像当中的匹配点有什么几何关系吧。

以图 7-7 为例，我们希望求取两帧图像 $I_1,I_2$ 之间的运动，设第一帧到第二帧的运动为
$R,t$。两个相机中心分别为 $O_1,O_2$。现在，考虑 $I_1$ 中有一个特征点 $p_1$，它在 $I_2$ 中对应着特征点 $p_2$。我们晓得这俩是通过特征匹配得到的。如果匹配正确，说明它们确实是同一个 空间点在两个成像平面上的投影。这里我们需要一些术语来描述它们之间的几何关系。

首先，连线$O_1{p}_1$和连线$O_2{p}_2$在三维空间中会相交于点 $P$。这时候点 $O_1,O_2,P$ 三个点可以确定一个平面，称为极平面（Epipolar plane）。$O_1{O}_2$ 连线与像平面 $I_1,I_2$ 的交点分别 为 $e_1,e_2$。$e_1,e_2$，称为极点（Epipoles），$O_1{O}_2$ 被称为基线（Baseline）。称极平面与两个像平面$I_1,I_2$ 之间的相交线 $l_1,l_2$ 为极线（Epipolar line）。

直观上讲，从第一帧的角度上看，射线$O_1{p}_1$是某个像素可能出现的空间位置——因
为该射线上的所有点都会投影到同一个像素点。同时，如果不知道 $P$ 的位置，那么当我们在第二个图像上看时，连线$vec{e_2p_2}$（也就是第二个图像中的极线）就是 $P$ 可能出现的投影的位置，也就是射线$O_1{p}_1$在第二个相机中的投影。

现在，由于我们通过特征点匹配，确定了 $p_2$ 的像素位置，所以能够推断 $P$ 的空间位置，以及相机的运动。要提醒读者的是，这都是多亏了正确的特征匹配。如果没有特征匹配，我们就没法确定 $p_2$ 到底在极线的哪个位置了。那时，就必须在极线上搜索以获得正确的匹配，这将在第 13 讲中提到。

现在，我们从代数角度来看一下这里出现的几何关系。在第一帧的坐标系下，设$P$的空间位置为：
$$P=[X,Y,Z{]}^T.$$
根据第5讲介绍的针孔相机模型，我们知道两个像素点$p_1,p_2$的像素位置为：
$$s_1{p}_1=KP,s_2{p}_2=K(R{P}_t).(7.1)$$
这里$K$为相机内参矩阵，$R,t$为两个坐标系的相机运动（如果我们愿意，也可以写成李代数的形式）。如果使用齐次坐标，我们也可以把上式写成在乘以非零常数下成立的等式：
$$p_1=KP,p_2=K(RP+t).(7.2)$$
现在，取：
$$x_1=K^{-1}{p}_1,x_2=K^{-1}{p}_2.(7.3)$$
这里的$x_1,x_2$是两个像素点的归一化平面上的坐标。代入上式，得：
$$x_2=R{x}_1+t.(7.4)$$
两边同时左乘$t^{\wedge }$。回忆$\wedge$的定义，这里相当于两侧同时与$t$做外积：
$$t^{\wedge }{x}_2=t^{\wedge }R{x}_1.(7.5)$$
然后，两侧同时左乘$x_2^T$：

7.3.2 本质矩阵

根据定义，本质矩阵$E=t^{\wedge }R$。它是一个$3\times 3$的矩阵，内有9个未知数。那么，是不是任意一个$3\times 3$的矩阵都可以被当成本质矩阵呢？ 从$E$的构造方式上看，有以下值得注意的地方：

$E$ 具有五个自由度的事实，表明我们最少可以用五对点来求解 $E$。但是，$E$ 的内在性质是一种非线性性质，在求解线性方程时会带来麻烦，因此，也可以只考虑它的尺度等价性，使用八对点来估计 $E$——这就是经典的八点法（Eight-point-algorithm）。

八点法只利用了 $E$ 的线性性质，因此可以在线性代数框架下求解。下面我们来看八点法是 如何工作的。


这八个方程构成了一个线性方程组。它的系数矩阵由特征点位置构成，大小为 $8\times 9$。$e$ 位于该矩阵的零空间中。如果系数矩阵是满秩的（即秩为 $8$），那么它的零空间维数为 $1$， 也就是 $e$ 构成一条线。这与 $e$ 的尺度等价性是一致的。如果八对匹配点组成的矩阵满足秩为 $8$ 的条件，那么 $E$ 的各元素就可由上述方程解得。

接下来的问题是如何根据已经估得的本质矩阵$E$，恢复出相机的运动 $R,t$。

这个过程 是由奇异值分解（SVD）得到的。设 E 的 SVD 分解为：


图 7-8 形象地显示了分解本质矩阵得到的四个解。我们已知空间点在相机（蓝色线）上的投影（红点），想要求解相机的运动。在保持红点不变的情况下，可以画出四种可能的 情况，不过幸运的是，只有第一种解中，$P$ 在两个相机中都具有正的深度。因此，只要把 任意一点代入四种解中，检测该点在两个相机下的深度，就可以确定哪个解是正确的了。

7.3.3 单应矩阵

除了基本矩阵和本质矩阵，我们还有一种称为单应矩阵（Homography）$H$ 的东西，它 描述了两个平面之间的映射关系。若场景中的特征点都落在同一平面上（比如墙，地面等），则可以通过单应性来进行运动估计。

这种情况在无人机携带的俯视相机，或扫地机携带的 顶视相机中比较常见。由于之前没提到过单应，我们稍微介绍一下。

单应矩阵通常描述处于共同平面上的一些点，在两张图像之间的变换关系。考虑在图像$I_1和I_2$ 有一对匹配好的特征点 $p_1$ 和 $p_2$。这些特征点落在某平面上。设这个平面满足方程：

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

请注意这里的等号是在非零因子下成立的。我们在实际处理中，通常乘以一个非零因子使得 $h_9=1$（在它取非零值时）。然后根据第三行，去掉这个非零因子，于是有：

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

与本质矩阵相似，求出单应矩阵以后需要对其进行分解，才可以得到相应的旋转矩阵 $R$ 和平移向量 $t$。分解的方法包括数值法与解析法。

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

如果已知成像的地图点的深度全为正值（即在相机前方），则又可以排除两组解。最后仅剩两组解，这时需要通过更多 的先验信息进行判断。

通常我们可以通过假设已知场景平面的法向量来解决，如场景平面 与相机平面平行，那么法向量 $\mathbf{n}$ 的理论值为 $1^T$。

单应性在 SLAM 中具重要意义。当特征点共面，或者相机发生纯旋转的时候，基础矩阵的自由度下降，这就出现了所谓的退化（degenerate）。现实中的数据总包含一些噪声，这时候如果我们继续使用八点法求解基础矩阵，基础矩阵多余出来的自由度将会主要由噪声决定。

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

7.4 实践：对极约束求解相机运动

下面，我们来练习一下如何通过 Essential 矩阵求解相机运动。上一节实践部分的程序提供了特征匹配，而这次我们就使用匹配好的特征点来计算$E,F$ 和 $H$，进而分解 $E$ 得到 $R,t$。整个程序使用 OpenCV 提供的算法进行求解。我们把上一节的特征提取封装成函数，以供后面使用。本节只展示位姿估计部分的代码。

略去

7.4.1 讨论

7.5 三角测量

之前两节，我们使用对极几何约束估计了相机运动，也讨论这种方法的局限性。在得到运动之后，下一步我们需要用相机的运动估计特征点的空间位置。在单目 SLAM 中，仅通过单张图像无法获得像素的深度信息，我们需要通过三角测量（Triangulation）（或三 角化）的方法来估计地图点的深度。

三角测量是指，通过在两处观察同一个点的夹角，确定该点的距离。三角测量最早由高斯提出并应用于测量学中，它在天文学、地理学的测量中都有应用。例如，我们可以通过不同季节观察到星星的角度，估计它离我们的距离。在 SLAM 中，我们主要用三角化来估计像素点的距离。

7.6 实践：三角测量

7.6.1 三角测量代码

7.6.2 讨论

关于三角测量，还有一个必须注意的地方。

三角测量是由平移得到的，有平移才会有对极几何中的三角形，才谈的上三角测量。因 此，纯旋转是无法使用三角测量的，因为对极约束将永远满足。在平移存在的情况下，我 们还要关心三角测量的不确定性，这会引出一个三角测量的矛盾。

如图 7-10 所示。当平移很小时，像素上的不确定性将导致较大的深度不确定性。也就是说，如果特征点运动一个像素 $\delta x$，使得视线角变化了一个角度 $\delta \theta$，那么测量到深度值 将有 $\delta d$ 的变化。从几何关系可以看到，当 $t$ 较大时，$\delta d$ 将明显变小，这说明平移较大时， 在同样的相机分辨率下，三角化测量将更精确。对该过程的定量分析可以使用正弦定理得 到，但我们这里先考虑定性分析。

因此，要增加三角化的精度，其一是提高特征点的提取精度，也就是提高图像分辨率 ——但这会导致图像变大，提高计算成本。另一方式是使平移量增大。但是，平移量增大，会导致图像的外观发生明显的变化，比如箱子原先被挡住的侧面显示出来了，比如反射光 发生变化了，等等。外观变化会使得特征提取与匹配变得困难。

总而言之，在增大平移，会 导致匹配失效；而平移太小，则三角化精度不够——这就是三角化的矛盾。

虽然本节只介绍了三角化的深度估计，但只要我们愿意，也能够定量地计算每个特征点的位置及不确定性。

所以，如果假设特征点服从高斯分布，并且对它不断地进行观测，在信息正确的情况下，我们就能够期望它的方差会不断减小乃至收敛。这就得到了一个滤波器，称为深度滤波器（Depth Filter）。

不过，由于它的原理较复杂，我们留到第 13 讲 再详细讨论它。下面，我们来讨论从 3D-2D 的匹配点来估计相机运动，以及 3D-3D 的估计方法。

7.7 3D-2D：PnP

PnP（Perspective-n-Point）是求解 3D 到 2D 点对运动的方法。它描述了当我们知道$n$个 3D 空间点以及它们的投影位置时，如何估计相机所在的位姿。前面已经说了，2D-2D 的对极几何方法需要八个或八个以上的点对（以八点法为例），且存在着初始化、纯旋转和尺度的问题。然而，如果两张图像中，其中一张特征点的 3D 位置已知，那么最少只需 三个点对（需要至少一个额外点验证结果）就可以估计相机运动。

特征点的 3D 位置可以 由三角化，或者由 RGB-D 相机的深度图确定。因此，在双目或 RGB-D 的视觉里程计中， 我们可以直接使用 PnP 估计相机运动。而在单目视觉里程计中，必须先进行初始化，然后才能使用 PnP。

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

PnP问题有很多种求解方法，例如用三对点估计位姿的P3P，直接线性变换（DLT），EPnP（Efficient PnP）[46]，UPnP[47] 等等）。此外，还能用非线性优化的方式，构建最小二乘问题并迭代求解，也就是万金油式的 Bundle Adjustment。我们先来看 DLT，然后再讲 Bundle Adjustment。

7.7.1 直接线性变换

考虑某个空间点 $P$，它的齐次坐标为 $P=(X,Y,Z,1{)}^T$。在图像 $I_1$ 中，投影到特征
点 $x_1=(u_1,v_1,1{)}^T$（以归一化平面齐次坐标表示）。此时相机的位姿 $R,t$ 是未知的。与单 应矩阵的求解类似，我们定义增广矩阵 $[R|t]$ 为一个 $3\times 4$ 的矩阵，包含了旋转与平移信息$x$。我们把它的展开形式列写如下：

由于 $t$ 一共有 $12$ 维，因此最少通过六对匹配点，即可实现矩阵 $T$ 的线性求解，这种方
法（也）称为直接线性变换（Direct Linear Transform，DLT）。当匹配点大于六对时，（又） 可以使用 SVD 等方法对超定方程求最小二乘解。

在 DLT 求解中，我们直接将 $T$ 矩阵看成了 $12$ 个未知数，忽略了它们之间的联系。因为旋转矩阵 R ∈ SO(3)，用 DLT 求出的解不一定满足该约束，它是一个一般矩阵。平移向量比较好办，它属于向量空间。对于旋转矩阵 $R$，我们必须针对 DLT 估计的 $T$ 的左边$3\times 3$ 的矩阵块，寻找一个最好的旋转矩阵对它进行近似。

这可以由 $QR$ 分解完成， 相当于把结果从矩阵空间重新投影到 $SE(3)$ 流形上，转换成旋转和平移两部分。

需要解释的是，我们这里的 $x_1$ 使用了归一化平面坐标，去掉了内参矩阵 $K$ 的影响——这是因为内参 $K$ 在 SLAM 中通常假设为已知。如果内参未知，那么我们也能用 PnP 去估计 $K,R,t$ 三个量。然而由于未知量的增多，效果会差一些。

7.7.2 P3P

下面讲的 P3P 是另一种解 PnP 的方法。它仅使用三对匹配点，对数据要求较少，因 此这里也简单介绍一下。

P3P 需要利用给定的三个点的几何关系。它的输入数据为三对 3D-2D 匹配点。记 3D点为 $A,B,C$，2D 点为 $a,b,c$，其中小写字母代表的点为大写字母在相机成像平面上的投 影，如图 7-11 所示。此外，P3P 还需要使用一对验证点，以从可能的解出选出正确的那一 个（类似于对极几何情形）。记验证点对为 $D-d$，相机光心为 $O$。

请注意，我们知道的是 $A,B,C$ 在世界坐标系中的坐标，而不是在相机坐标系中的坐标。

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

首先，显然，三角形之间存在对应关系：


注意这些方程中的已知量和未知量。由于我们知道 2D 点的图像位置，三个余弦角$cos⟨a,b⟩,cos⟨b,c⟩,cos⟨a,c⟩$ 是已知的。同时，$u=B{C}^2/A{B}^2,w=A{C}^2/A{B}^2$ 可以通过 $A,B,C$ 在世界坐标系下的坐标算出，变换到相机坐标系下之后，并不改变这个比值。

该式 中的 $x,y$ 是未知的，随着相机移动会发生变化。因此，该方程组是关于 $x,y$ 的一个二元二 次方程（多项式方程）。解析地求解该方程组是一个复杂的过程，需要用吴消元法。这里不 展开对该方程解法的介绍，感兴趣的读者请参照 。

类似于分解 E 的情况，该方程最多 可能得到四个解，但我们可以用验证点来计算最可能的解，得到 $A,B,C$ 在相机坐标系下的 3D 坐标。然后，根据 3D-3D 的点对，计算相机的运动 $R,t$。这部分将在 7.9 小结介绍。

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

后文将看到，带有匹配信息的 3D-3D 位姿求解非常容易，所以这种思路是非常有效的。其他的一些方法，例如 EPnP，亦采用了这种思路。然而，P3P 也存在着一些问题：

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

所以后续人们还提出了许多别的方法，如 EPnP、UPnP 等。它们利用更多的信息，而且用迭代的方式对相机位姿进行优化，以尽可能地消除噪声的影响。不过，相对于 P3P 来 说，原理会更加复杂一些，所以我们建议读者阅读原始的论文，或通过实践来理解 PnP 过程。

在 SLAM 当中，通常的做法是先使用 P3P/EPnP 等方法估计相机位姿，然后构建最小二乘优化问题对估计值进行调整（Bundle Adjustment）。接下来我们从非线性优化角度来看一下 PnP 问题。

7.7.3 Bundle Adjustment

除了使用线性方法之外，我们可以把 PnP 问题构建成一个定义于李代数上的非线性最小二乘问题。这将用到本书第四章和第五章的知识。前面说的线性方法，往往是先求相机位姿，再求空间点位置，而非线性优化则是把它们都看成优化变量，放在一起优化。

这是 一种非常通用的求解方式，我们可以用它对 PnP 或 ICP 给出的结果进行优化。在 PnP 中， 这个 Bundle Adjustment 问题，是一个最小化**重投影误差（Reprojection error）**的问题。

我们在本节给出此问题在两个视图下的基本形式，然后在第十讲讨论较大规模的 BA 问题。

该问题的误差项，是将像素坐标（观测到的投影位置）与 3D 点按照当前估计的位姿进行投影得到的位置相比较得到的误差，所以称之为重投影误差。

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

如图 7-12 所示，我们通过特征匹配，知道了 $p_1$ 和 $p_2$ 是同一个空间点 $P$ 的投影，但是我们不知道相机的位姿。在初始值中，$P$ 的投影 ${\hat{p}}_2$ 与实际的 $p2$ 之间有一定的距离。于是我们调整相机的位姿，使得这个距离变小。

不过，由于这个调整需要考虑很多个点，所以最后每个点的误差通常都不会精确为零。

最小二乘优化问题已经在第六讲介绍过了。使用李代数，可以构建无约束的优化问题，很方便地通过 G-N, L-M 等优化算法进行求解。不过，在使用 G-N 和 L-M 之前，我们需要知道每个误差项关于优化变量的导数，也就是线性化：

这里的 $J$ 的形式是值得讨论的，甚至可以说是关键所在。我们固然可以使用数值导数，但如果能够推导解析形式时，我们会优先考虑解析导数。现在，当 $e$ 为像素坐标误差（$2$ 维），$x$ 为相机位姿（$6$ 维）时，$J$ 将是一个$2\times 6$的矩阵。我们来推导 $J$ 的形式。

回忆李代数的内容，我们介绍了如何使用扰动模型来求李代数的导数。首先，记变换到相机坐标系下的空间点坐标为 $P\prime$，并且把它前三维取出来：

这里的 $\oplus$ 指李代数上的左乘扰动。第一项是误差关于投影点的导数，在式（7.40）已 经列出了变量之间的关系，易得：

这个雅可比矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系。我们保留了前面的负号，因为这是由于误差是由观测值减预测值定义的。它当然也可反过来，定义成 “预测减观测”的形式。在那种情况下，只要去掉前面的负号即可。

此外，如果 $\mathfrak{s}\mathfrak{e}(3)$ 的定义方式是旋转在前，平移在后时，只要把这个矩阵的前三列与后三列对调即可。 另一方面，除了优化位姿，我们还希望优化特征点的空间位置。因此，需要讨论 $e$ 关于空间点 $P$ 的导数。所幸这个导数矩阵相对来说容易一些。仍利用链式法则，有：

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

7.8 实践：求解PnP

7.8.1 使用EPnP求解位姿

7.8.2 使用BA优化

7.9 3D-2D：ICP

最后，我们来介绍 3D-3D 的位姿估计问题。假设我们有一组配对好的 3D 点（比如我们对两个 RGB-D 图像进行了匹配）：

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

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

因此，在激光 SLAM 中也会碰到 ICP，不过由于激光数据特征不够丰富，我们无从知道两个点集之间的匹配关系，只能认为距离最近的两个点为同一个， 所以这个方法称为迭代最近点。

而在视觉中，特征点为我们提供了较好的匹配关系，所以 整个问题就变得更简单了。在 RGB-D SLAM 中，可以用这种方式估计相机位姿。下文我们用 ICP 指代匹配好的两组点间运动估计问题。

和 PnP 类似，ICP 的求解也分为两种方式：利用线性代数的求解（主要是 SVD），以及利用非线性优化方式的求解（类似于 Bundle Adjustment）。下面分别来介绍它们。

7.9.1 SVD方法

仔细观察左右两项，我们发现左边只和旋转矩阵 $R$ 相关，而右边既有 $R$ 也有 $t$，但 只和质心相关。只要我们获得了 $R$，令第二项为零就能得到 $t$。于是，ICP 可以分为以下 三个步骤求解：

我们看到，只要求出了两组点之间的旋转，平移量是非常容易得到的。所以我们重点关注 $R$ 的计算。展开关于 $R$ 的误差项，得：

7.9.2 非线性优化方法

求解 ICP 的另一种方式是使用非线性优化，以迭代的方式去找最优值。该方法和我们前面讲述的 PnP 非常相似。以李代数表达位姿时，目标函数可以写成：

于是，在非线性优化中只需不断迭代，我们就能找到极小值。而且，可以证明，ICP 问题存在唯一解或无穷多解的情况。

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

需要说明的是，我们这里讲的 ICP，是指已经由图像特征给定了匹配的情况下，进行位姿估计的问题。在匹配已知的情况下，这个最小二乘问题实际上具有解析解， 所以并没有必要进行迭代优化。ICP 的研究者们往往更加关心匹配未知的情况。

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

7.10 实践：求解ICP

7.10.1 SVD方法

7.10.2 非线性优化方法

7.11 小结

本节介绍了基于特征点的视觉里程计中的几个重要的问题。包括：

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

本章内容较为丰富，且结合应用了前几章的基本知识。读者若觉得理解有困难，可以对前面知识稍加回顾。最好亲自做一遍实验，以理解整个运动估计的内容。

需要解释的是，为保证行文流畅，我们省略了大量的，关于某些特殊情况的讨论。例如，如果在对极几何求解过程中，给定的特征点共面，会发生什么情况（这在单应矩阵 H 中提到）？共线又会发生什么情况？在 PnP 和 ICP 中若给定这样的解，又会导致什么情 况？求解算法能否识别这些特殊的情况，并报告所得的解可能不可靠？——尽管它们都是 值得研究和探索的，然而对它们的讨论势必让本书变得特别繁琐。而且在工程实现中，这些情况甚少出现，所以本书介绍的方法，是指在实际工程中能够有效运行的方法，我们假定了那些少见的情况并不发生。