视觉SLAM⑦---视觉里程计Ⅰ(特征点法、2D-2D对极几何)

APS2023

已于 2022-05-19 19:44:37 修改

阅读量3.5k

点赞数 5

分类专栏： SLAM 十四讲读书笔记文章标签：计算机视觉人工智能

于 2022-05-13 10:39:30 首次发布

本文链接：https://blog.csdn.net/qq_41694024/article/details/124710172

版权

SLAM 十四讲读书笔记专栏收录该内容

12 篇文章

订阅专栏

7.5 讨论：2D-2D对极几何会出现什么问题

7.8.1 对极几何推导：如何通过已知的像素点对应信息及世界坐标点求R,t及存在的问题

7.8.2 如何通过三角测量得到目标的深度及存在的问题

7.0 本章主要目标

主要目标
Ⅰ.理解图像特征点的意义，并掌握在单幅图像中提取特征点及多幅图像中匹配特征点的方法。
Ⅱ.理解对极几何的原理，利用对极几何的约束，恢复图像之间的摄像机的三维运动。

Ⅲ.理解PNP问题，以及利用已知三维结构与图像的对应关系求解摄像机的三维运动。

Ⅳ.理解ICP问题，以及利用点云的匹配关系求解摄像机的三维运动
Ⅴ.理解如何通过三角化获得二维图像上对应点的三维结构。

Ⅵ.将书中代码跑起来

图7-1 本章主要目标

7.1 特征点法

        在第2讲中，我们介绍过，一个SLAM系统分为前端和后端，其中前端也称为视觉里程计。

        视觉里程计根据相邻图像的信息估计出粗略的相机运动，给后端提供较好的初始值。

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

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

        在本讲中，我们将从特征点法入手，学习如何提取、匹配图像特征点，然后估计两帧之间的相机运动和场景结构，从而实现一个两帧间视觉里程计。这类算法有时也称为两视图几何(Two-view geometry)。

7.1.1 特征点

1.视觉里程计要解决的问题及为什么需要特征点

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

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

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

图7-2 路标的特点及SLAM中的图像特征

图7-3 一图浅理解特征点提取与匹配

        数字图像在计算机中以灰度值矩阵的方式存储，所以最简单的，单个图像像素（也就是说，图像的每个像素都是特征，不过特征好坏而已）也是一种“特征”。

        但是，在视觉里程计中，我们希望特征点在相机运动之后保持稳定，而灰度值受光照、形变、物体材质的影响严重，在不同图像间变化非常大，不够稳定。理想的情况是，当场景和相机视角发生少量改变时，算法还能从图像中判断哪些地方是同一个点。所以，仅凭灰度值是不够的，我们需要对图像提取特征点。

2.什么是特征点？角点、区块、边缘哪个更适合做特征点？局部图像特征？

  特征点是图像里一些特别的地方。以图7-4为例，我们可以把图像中的角点、边缘和区块都当成图像中有代表性的地方。

        我们更容易精确地指出，某两幅图像中出现了同一个角点；指出某两幅图像中出现同一个边缘则稍微困难一些，因为沿着该边缘前进，图像局部是相似的；指出某两幅图像中出现同一个区块则是最困难的。

        我们发现，图像中的角点、边缘相比于像素区块而言更加“特别”，在不同图像之间的辨识度更强。所以，一种直观的提取特征的方式就是在不同图像间辨认角点，确定它们的对应关系。在这种做法中，角点就是所谓的特征。角点的提取算法有很多，例如Harris角点、FAST角点、GFTT角点，等等。
        然而，在大多数应用中，单纯的角点依然不能满足我们的很多需求。例如，从远处看上去是角点的地方，当相机离近之后，可能就不显示为角点了。或者，当旋转相机时，角点的外观会发生变化，我们也就不容易辨认出那是同一个角点了。为此，计算机视觉领域的研究者们在长年的研究中设计了许多更加稳定的局部图像特征，如著名的SIFT、SURF、ORB，等等。相比于朴素的角点，这些人工设计的特征点能够拥有如下性质：

1．可重复性(Repeatability)：相同的特征可以在不同的图像中找到(在A图中看见B图仍然能看见)

2．可区别性(Distinctiveness)：不同的特征有不同的表达(A里面看见的点和B里面看到的点要是有差别的不能完全一样的)
3．高效率(Efficiency)：同一图像中，特征点的数量应远小于像素的数量。

4．本地性(Locality)：特征仅与一小片图像区域相关。

图7-4-1 可以作为图像特征的部分:角点、边缘、区块

图7-4-2 特征点及特征点信息

3.特征点的组成及一些特征点提取技术

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

        例如，当我们说“在一张图像中计算SIFT特征点”时，是指“提取SIFT关键点，并计算SIFT描述子”两件事情。

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

        描述子通常是一个向量，按照某种人为设计的方式，描述了该关键点周围像素的信息。描述子是按照“外观相似的特征应该有相似的描述子”的原则设计的。因此，只要两个特征点的描述子在向量空间上的距离相近，就可以认为它们是同样的特征点。
        历史上，研究者们提出过许多图像特征。它们有些很精确，在相机的运动和光照变化下仍具有相似的表达，但相应地计算量较大。其中，SIFT(尺度不变特征变换，Scale-Invariant FeatureTransform)当属最为经典的一种。它充分考虑了在图像变换过程中出现的光照、尺度、旋转等变化，但随之而来的是极大的计算量。
        另一些特征，则考虑适当降低精度和鲁棒性，以提升计算的速度，例如，FAST关键点属于计算特别快的一种特征点(注意，这里“关键点”的表述，说明它没有描述子)，而ORB (OrientedFAST and Rotated BRIEF)特征则是目前看来非常具有代表性的实时图像特征。它改进了FAST检测子不具有方向性的问题，并采用速度极快的二进制描述子BRIEF(Binary Robust IndependentElementary Feature)，使整个图像特征提取的环节大大加速。

        大部分特征提取都具有较好的并行性，可以通过GPU等设备加速计算。经过GPU加速后的SIFT，就可以满足实时计算的要求。但是，引入GPU将带来整个SLAM成本的提升。由此带来的性能提升是否足以抵去付出的计算成本,需要系统的设计人员仔细考量。

7.1.2 ORB特征

关键点：Oriented FAST（连续N个点的灰度有明显差异）

描述子：BRIEF（在FAST基础上计算旋转）

1.总体概括

ORB特征由关键点和描述子两部分组成。它的关键点称为“Oriented FAST”，是一种改进的FAST角点，关于什么是FAST角点我们将在下文介绍。它的描述子称为BRIEF。因此，提取ORB特征分为如下两个步骤：
1. FAST角点提取：找出图像中的“角点”。相较于原版的FAST，ORB中计算了特征点的
主方向，为后续的BRIEF描述子增加了旋转不变特性。
2. BRIEF描述子：对前一步提取出特征点的周围图像区域进行描述。ORB对BRIEF进行了
一些改进，主要是指在BRIEF中使用了先前计算的方向信息。

2.FAST关键点：

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

图7-5 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)，在一定区域内仅保留响应极大值的角点，避免角点集中的问题。

3.FAST关键点的问题

         FAST特征点的计算仅仅是比较像素间亮度的差异，所以速度非常快，但它也有重复性不强(两图同一角点不容易匹配)、分布不均匀的缺点。

        此外，FAST角点不具有方向信息。同时，由于它固定取半径为3的圆，存在尺度问题：远处看着像是角点的地方，接近后看可能就不是角点了。

        FAST提取的角点无朝向信息，比如下图：那两个点本身是可以匹配的（事实上是同一个点，不过相机稍微旋转了一下），导致截取区域差异过大，不好进行匹配，于是要引入旋转。

图7-6-1 FAST没有朝向信息的问题（可能无法匹配）

图7-6-2 FAST没有朝向信息的问题（得到旋转角）

4.改进

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

        尺度不变性由构建图像金字塔，并在金字塔的每一层上检测角点来实现。

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

图7-7 使用金字塔可以匹配不同缩放倍率下的图像

金字塔是计算图视觉中常用的一种处理方法，示意图如图7-7所示。金字塔底层是原始图像。每往上一层，就对图像进行一个固定倍率的缩放，这样我们就有了不同分辨率的图像。较小的图像可以看成是远处看过来的场景。在特征匹配算法中，我们可以匹配不同层上的图像，从而实现尺度不变性。例如，如果相机在后退，那么我们应该能够在上一个图像金字塔的上层和下一个图像金字塔的下层中找到匹配。

         在旋转方面，我们计算特征点附近的图像灰度质心。所谓质心是指以图像块灰度值作为权重的中心。其具体操作步骤如下：
1．在一个小的图像块 $B$ 中，定义图像块的矩为

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

2．通过矩可以找到图像块的质心：

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

3．连接图像块的几何中心 $O$ 与质心 $C$ ，得到一个方向向量 $\underset{oc}{\rightarrow}$ ，于是特征点的方向可以定义为
                                                     $\theta =arctan(m_{01}/m_{10})$

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

5.BRIEF描述子

        在提取Oriented FAST关键点后，我们对每个点计算其描述子。ORB使用改进的 BRIEF特征描述。我们先来介绍 BRIEF。
        BRIEF是一种二进制描述子，其描述向量由许多个0和1组成，这里的0和1编码了关键点对近两个随机像素(比如p和q)的大小关系：如果p比q大，则取1，反之就取0。

图7-8 BRIEF示例

解释7-8，取一些序对 $(p_{i},q_{i})$ ，如果 $p_{i}$ 亮度 $q_{i}$ 记为1，否则记为0，最后得到向量 $[0,1,1......]$ 后，携带了图像的编码信息，即使图像光照发生变化使灰度值发生了改变，但图像编码信息（相对亮度）却不怎么改变。

？那么问题来了？如果是在图7-6-1的情况下进行BRIEF呢？那当然是先旋转在进行BRIEF啦！

        如果我们取了128个这样的 $p,q$ ，则最后得到128维由0、1组成的向量。BRIEF使用了随机选点的比较，速度非常快，而且由于使用了二进制表达，存储起来也十分方便，适用于实时的图像匹配。原始的BRIEF描述子不具有旋转不变性，因此在图像发生旋转时容易丢失。
        而ORB在FAST特征点是取阶段计算了关键点的方向，所以可以利用方向信息，计算旋转之后的“Steer BRIEF”特征使ORB的描述子具有较好的旋转不变性。
        由于考虑到了旋转和缩放，ORB在平移、旋转和缩放的变换下仍有良好的表现。同时，FAST和BREIF的组合也非常高效，使得ORB特征在实时SLAM 中非常受欢迎。下面介绍如何在不同的图像之间进行特征匹配。

图7-9 OpenCV提供的ORB特征点检测结果

7.1.3 特征匹配

1.特征匹配及其要解决的问题

特征匹配（如图7-11所示）是视觉SLAM 中极为关键的一步，宽泛地说，特征匹配解决了SLAM中的数据关联问题(data association)，即确定当前看到的路标与之前看到的路标之间的对应关系。通过对图像与图像或者图像与地图之间的描述子进行准确匹配，我们可以为后续的姿态估计、优化等操作减轻大量负担。

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

2.匹配方法：暴力匹配&&快速近似最近邻(FLANN)算法

先看正确匹配的情况，考虑两个时刻的情况，如果在图像 $I_{t}$ 中提取出的特征点 $x_{t}^{m},m = 1,2,3...,M$ ，在图像 $I_{t+1}$ 中提取出的特征点 $x_{t+1}^{n},n = 1,2,3...,N$ ，如何寻找这两个集合元素的对应关系呢？

        最简单的特征匹配方法就是暴力匹配(Brute-ForceMatcher)。即对每一个特征点 $x_{t}^{m}$ 与所有的 $x_{t+1}^{n}$ 测量描述子的距离，然后排序，取最近的一个作为匹配点。描述子距离表示了两个特征之间的相似程度，不过在实际运用中还可以取不同的距离度量范数。对于浮点类型的描述子，使用欧氏距离进行度量即可。而对于二进制的描述子（比如BRIEF 这样的)，我们往往使用汉明距离(Hamming distance)作为度量——两个二进制串之间的汉明距离，指的是其不同位数的个数。

图7-10 暴力匹配与汉明距离

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

        此时，快速近似最近邻(FLANN)算法更加适合于匹配点数量极多的情况。由于这些匹配算法理论已经成熟，而且实现上也已集成到OpenCV。

图7-11 两帧图像间的特征匹配

7.2 实践：特征提取和匹配

OpenCV已经集成了多数主流的图像特征，我们可以很方便地进行调用。下面我们来完成两个实验：

第一个实验中，我们演示使用OpenCV进行ORB的特征匹配；

第二个实验中，我们演示如何根据前面介绍的原理，手写一个简单的ORB特征。

7.2.1 OpenCV的ORB特征

首先，我们调用OpenCV来提取和匹配ORB。

code：

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <chrono>

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);
  assert(img_1.data != nullptr && img_2.data != nullptr);

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

  //-- 第一步:检测 Oriented FAST 角点位置
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  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);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "extract ORB cost = " << time_used.count() << " seconds. " << endl;

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

  //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
  vector<DMatch> matches;
  t1 = chrono::steady_clock::now();
  matcher->match(descriptors_1, descriptors_2, matches);
  t2 = chrono::steady_clock::now();
  time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "match ORB cost = " << time_used.count() << " seconds. " << endl;

  //-- 第四步:匹配点对筛选
  // 计算最小距离和最大距离
  auto min_max = minmax_element(matches.begin(), matches.end(),
                                [](const DMatch &m1, const DMatch &m2) { return m1.distance < m2.distance; });
  double min_dist = min_max.first->distance;
  double max_dist = min_max.second->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("all matches", img_match);
  imshow("good matches", img_goodmatch);
  waitKey(0);

  return 0;
}

运行：

liuhongwei@liuhongwei-virtual-machine:~/桌面/slambook2/ch7$ build/orb_cv 1.png 2.png 
extract ORB cost = 0.0135651 seconds. 
match ORB cost = 0.00158659 seconds. 
-- Max dist : 95.000000 
-- Min dist : 4.000000

结果：

图7-7显示了例程的运行结果。我们看到未筛选的匹配中带有大量的误匹配。经过一次筛选之后，匹配数量减少了许多，但大多数匹配都是正确的。这里，筛选的依据是汉明距离小于最小距离的两倍，这是一种工程上的经验方法，不一定有理论依据。不过尽管在示例图像中能够筛选出正确的匹配，但我们仍然不能保证在所有其他图像中得到的匹配都是正确的。因此，在后面的运动估计中,还需要使用去除误匹配的算法。

7.2.2 手写ORB特征

1.code


#include <opencv2/opencv.hpp>
#include <string>
#include <nmmintrin.h>
#include <chrono>

using namespace std;

// global variables
string first_file = "./1.png";
string second_file = "./2.png";

// 32 bit unsigned int, will have 8, 8x32=256
typedef vector<uint32_t> DescType; // Descriptor type

/**
 * compute descriptor of orb keypoints
 * @param img input image
 * @param keypoints detected fast keypoints
 * @param descriptors descriptors
 *
 * NOTE: if a keypoint goes outside the image boundary (8 pixels), descriptors will not be computed and will be left as
 * empty
 */
void ComputeORB(const cv::Mat &img, vector<cv::KeyPoint> &keypoints, vector<DescType> &descriptors);

/**
 * brute-force match two sets of descriptors
 * @param desc1 the first descriptor
 * @param desc2 the second descriptor
 * @param matches matches of two images
 */
void BfMatch(const vector<DescType> &desc1, const vector<DescType> &desc2, vector<cv::DMatch> &matches);

int main(int argc, char **argv) {

  // load image
  cv::Mat first_image = cv::imread(first_file, 0);
  cv::Mat second_image = cv::imread(second_file, 0);
  assert(first_image.data != nullptr && second_image.data != nullptr);

  // detect FAST keypoints1 using threshold=40
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  vector<cv::KeyPoint> keypoints1;
  cv::FAST(first_image, keypoints1, 40);
  vector<DescType> descriptor1;
  ComputeORB(first_image, keypoints1, descriptor1);

  // same for the second
  vector<cv::KeyPoint> keypoints2;
  vector<DescType> descriptor2;
  cv::FAST(second_image, keypoints2, 40);
  ComputeORB(second_image, keypoints2, descriptor2);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "extract ORB cost = " << time_used.count() << " seconds. " << endl;

  // find matches
  vector<cv::DMatch> matches;
  t1 = chrono::steady_clock::now();
  BfMatch(descriptor1, descriptor2, matches);
  t2 = chrono::steady_clock::now();
  time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "match ORB cost = " << time_used.count() << " seconds. " << endl;
  cout << "matches: " << matches.size() << endl;

  // plot the matches
  cv::Mat image_show;
  cv::drawMatches(first_image, keypoints1, second_image, keypoints2, matches, image_show);
  cv::imshow("matches", image_show);
  cv::imwrite("matches.png", image_show);
  cv::waitKey(0);

  cout << "done." << endl;
  return 0;
}

// -------------------------------------------------------------------------------------------------- //
// ORB pattern
int ORB_pattern[256 * 4] = {
  8, -3, 9, 5/*mean (0), correlation (0)*/,
  4, 2, 7, -12/*mean (1.12461e-05), correlation (0.0437584)*/,
  -11, 9, -8, 2/*mean (3.37382e-05), correlation (0.0617409)*/,
  7, -12, 12, -13/*mean (5.62303e-05), correlation (0.0636977)*/,
  2, -13, 2, 12/*mean (0.000134953), correlation (0.085099)*/,
  1, -7, 1, 6/*mean (0.000528565), correlation (0.0857175)*/,
  -2, -10, -2, -4/*mean (0.0188821), correlation (0.0985774)*/,
  -13, -13, -11, -8/*mean (0.0363135), correlation (0.0899616)*/,
  -13, -3, -12, -9/*mean (0.121806), correlation (0.099849)*/,
  10, 4, 11, 9/*mean (0.122065), correlation (0.093285)*/,
  -13, -8, -8, -9/*mean (0.162787), correlation (0.0942748)*/,
  -11, 7, -9, 12/*mean (0.21561), correlation (0.0974438)*/,
  7, 7, 12, 6/*mean (0.160583), correlation (0.130064)*/,
  -4, -5, -3, 0/*mean (0.228171), correlation (0.132998)*/,
  -13, 2, -12, -3/*mean (0.00997526), correlation (0.145926)*/,
  -9, 0, -7, 5/*mean (0.198234), correlation (0.143636)*/,
  12, -6, 12, -1/*mean (0.0676226), correlation (0.16689)*/,
  -3, 6, -2, 12/*mean (0.166847), correlation (0.171682)*/,
  -6, -13, -4, -8/*mean (0.101215), correlation (0.179716)*/,
  11, -13, 12, -8/*mean (0.200641), correlation (0.192279)*/,
  4, 7, 5, 1/*mean (0.205106), correlation (0.186848)*/,
  5, -3, 10, -3/*mean (0.234908), correlation (0.192319)*/,
  3, -7, 6, 12/*mean (0.0709964), correlation (0.210872)*/,
  -8, -7, -6, -2/*mean (0.0939834), correlation (0.212589)*/,
  -2, 11, -1, -10/*mean (0.127778), correlation (0.20866)*/,
  -13, 12, -8, 10/*mean (0.14783), correlation (0.206356)*/,
  -7, 3, -5, -3/*mean (0.182141), correlation (0.198942)*/,
  -4, 2, -3, 7/*mean (0.188237), correlation (0.21384)*/,
  -10, -12, -6, 11/*mean (0.14865), correlation (0.23571)*/,
  5, -12, 6, -7/*mean (0.222312), correlation (0.23324)*/,
  5, -6, 7, -1/*mean (0.229082), correlation (0.23389)*/,
  1, 0, 4, -5/*mean (0.241577), correlation (0.215286)*/,
  9, 11, 11, -13/*mean (0.00338507), correlation (0.251373)*/,
  4, 7, 4, 12/*mean (0.131005), correlation (0.257622)*/,
  2, -1, 4, 4/*mean (0.152755), correlation (0.255205)*/,
  -4, -12, -2, 7/*mean (0.182771), correlation (0.244867)*/,
  -8, -5, -7, -10/*mean (0.186898), correlation (0.23901)*/,
  4, 11, 9, 12/*mean (0.226226), correlation (0.258255)*/,
  0, -8, 1, -13/*mean (0.0897886), correlation (0.274827)*/,
  -13, -2, -8, 2/*mean (0.148774), correlation (0.28065)*/,
  -3, -2, -2, 3/*mean (0.153048), correlation (0.283063)*/,
  -6, 9, -4, -9/*mean (0.169523), correlation (0.278248)*/,
  8, 12, 10, 7/*mean (0.225337), correlation (0.282851)*/,
  0, 9, 1, 3/*mean (0.226687), correlation (0.278734)*/,
  7, -5, 11, -10/*mean (0.00693882), correlation (0.305161)*/,
  -13, -6, -11, 0/*mean (0.0227283), correlation (0.300181)*/,
  10, 7, 12, 1/*mean (0.125517), correlation (0.31089)*/,
  -6, -3, -6, 12/*mean (0.131748), correlation (0.312779)*/,
  10, -9, 12, -4/*mean (0.144827), correlation (0.292797)*/,
  -13, 8, -8, -12/*mean (0.149202), correlation (0.308918)*/,
  -13, 0, -8, -4/*mean (0.160909), correlation (0.310013)*/,
  3, 3, 7, 8/*mean (0.177755), correlation (0.309394)*/,
  5, 7, 10, -7/*mean (0.212337), correlation (0.310315)*/,
  -1, 7, 1, -12/*mean (0.214429), correlation (0.311933)*/,
  3, -10, 5, 6/*mean (0.235807), correlation (0.313104)*/,
  2, -4, 3, -10/*mean (0.00494827), correlation (0.344948)*/,
  -13, 0, -13, 5/*mean (0.0549145), correlation (0.344675)*/,
  -13, -7, -12, 12/*mean (0.103385), correlation (0.342715)*/,
  -13, 3, -11, 8/*mean (0.134222), correlation (0.322922)*/,
  -7, 12, -4, 7/*mean (0.153284), correlation (0.337061)*/,
  6, -10, 12, 8/*mean (0.154881), correlation (0.329257)*/,
  -9, -1, -7, -6/*mean (0.200967), correlation (0.33312)*/,
  -2, -5, 0, 12/*mean (0.201518), correlation (0.340635)*/,
  -12, 5, -7, 5/*mean (0.207805), correlation (0.335631)*/,
  3, -10, 8, -13/*mean (0.224438), correlation (0.34504)*/,
  -7, -7, -4, 5/*mean (0.239361), correlation (0.338053)*/,
  -3, -2, -1, -7/*mean (0.240744), correlation (0.344322)*/,
  2, 9, 5, -11/*mean (0.242949), correlation (0.34145)*/,
  -11, -13, -5, -13/*mean (0.244028), correlation (0.336861)*/,
  -1, 6, 0, -1/*mean (0.247571), correlation (0.343684)*/,
  5, -3, 5, 2/*mean (0.000697256), correlation (0.357265)*/,
  -4, -13, -4, 12/*mean (0.00213675), correlation (0.373827)*/,
  -9, -6, -9, 6/*mean (0.0126856), correlation (0.373938)*/,
  -12, -10, -8, -4/*mean (0.0152497), correlation (0.364237)*/,
  10, 2, 12, -3/*mean (0.0299933), correlation (0.345292)*/,
  7, 12, 12, 12/*mean (0.0307242), correlation (0.366299)*/,
  -7, -13, -6, 5/*mean (0.0534975), correlation (0.368357)*/,
  -4, 9, -3, 4/*mean (0.099865), correlation (0.372276)*/,
  7, -1, 12, 2/*mean (0.117083), correlation (0.364529)*/,
  -7, 6, -5, 1/*mean (0.126125), correlation (0.369606)*/,
  -13, 11, -12, 5/*mean (0.130364), correlation (0.358502)*/,
  -3, 7, -2, -6/*mean (0.131691), correlation (0.375531)*/,
  7, -8, 12, -7/*mean (0.160166), correlation (0.379508)*/,
  -13, -7, -11, -12/*mean (0.167848), correlation (0.353343)*/,
  1, -3, 12, 12/*mean (0.183378), correlation (0.371916)*/,
  2, -6, 3, 0/*mean (0.228711), correlation (0.371761)*/,
  -4, 3, -2, -13/*mean (0.247211), correlation (0.364063)*/,
  -1, -13, 1, 9/*mean (0.249325), correlation (0.378139)*/,
  7, 1, 8, -6/*mean (0.000652272), correlation (0.411682)*/,
  1, -1, 3, 12/*mean (0.00248538), correlation (0.392988)*/,
  9, 1, 12, 6/*mean (0.0206815), correlation (0.386106)*/,
  -1, -9, -1, 3/*mean (0.0364485), correlation (0.410752)*/,
  -13, -13, -10, 5/*mean (0.0376068), correlation (0.398374)*/,
  7, 7, 10, 12/*mean (0.0424202), correlation (0.405663)*/,
  12, -5, 12, 9/*mean (0.0942645), correlation (0.410422)*/,
  6, 3, 7, 11/*mean (0.1074), correlation (0.413224)*/,
  5, -13, 6, 10/*mean (0.109256), correlation (0.408646)*/,
  2, -12, 2, 3/*mean (0.131691), correlation (0.416076)*/,
  3, 8, 4, -6/*mean (0.165081), correlation (0.417569)*/,
  2, 6, 12, -13/*mean (0.171874), correlation (0.408471)*/,
  9, -12, 10, 3/*mean (0.175146), correlation (0.41296)*/,
  -8, 4, -7, 9/*mean (0.183682), correlation (0.402956)*/,
  -11, 12, -4, -6/*mean (0.184672), correlation (0.416125)*/,
  1, 12, 2, -8/*mean (0.191487), correlation (0.386696)*/,
  6, -9, 7, -4/*mean (0.192668), correlation (0.394771)*/,
  2, 3, 3, -2/*mean (0.200157), correlation (0.408303)*/,
  6, 3, 11, 0/*mean (0.204588), correlation (0.411762)*/,
  3, -3, 8, -8/*mean (0.205904), correlation (0.416294)*/,
  7, 8, 9, 3/*mean (0.213237), correlation (0.409306)*/,
  -11, -5, -6, -4/*mean (0.243444), correlation (0.395069)*/,
  -10, 11, -5, 10/*mean (0.247672), correlation (0.413392)*/,
  -5, -8, -3, 12/*mean (0.24774), correlation (0.411416)*/,
  -10, 5, -9, 0/*mean (0.00213675), correlation (0.454003)*/,
  8, -1, 12, -6/*mean (0.0293635), correlation (0.455368)*/,
  4, -6, 6, -11/*mean (0.0404971), correlation (0.457393)*/,
  -10, 12, -8, 7/*mean (0.0481107), correlation (0.448364)*/,
  4, -2, 6, 7/*mean (0.050641), correlation (0.455019)*/,
  -2, 0, -2, 12/*mean (0.0525978), correlation (0.44338)*/,
  -5, -8, -5, 2/*mean (0.0629667), correlation (0.457096)*/,
  7, -6, 10, 12/*mean (0.0653846), correlation (0.445623)*/,
  -9, -13, -8, -8/*mean (0.0858749), correlation (0.449789)*/,
  -5, -13, -5, -2/*mean (0.122402), correlation (0.450201)*/,
  8, -8, 9, -13/*mean (0.125416), correlation (0.453224)*/,
  -9, -11, -9, 0/*mean (0.130128), correlation (0.458724)*/,
  1, -8, 1, -2/*mean (0.132467), correlation (0.440133)*/,
  7, -4, 9, 1/*mean (0.132692), correlation (0.454)*/,
  -2, 1, -1, -4/*mean (0.135695), correlation (0.455739)*/,
  11, -6, 12, -11/*mean (0.142904), correlation (0.446114)*/,
  -12, -9, -6, 4/*mean (0.146165), correlation (0.451473)*/,
  3, 7, 7, 12/*mean (0.147627), correlation (0.456643)*/,
  5, 5, 10, 8/*mean (0.152901), correlation (0.455036)*/,
  0, -4, 2, 8/*mean (0.167083), correlation (0.459315)*/,
  -9, 12, -5, -13/*mean (0.173234), correlation (0.454706)*/,
  0, 7, 2, 12/*mean (0.18312), correlation (0.433855)*/,
  -1, 2, 1, 7/*mean (0.185504), correlation (0.443838)*/,
  5, 11, 7, -9/*mean (0.185706), correlation (0.451123)*/,
  3, 5, 6, -8/*mean (0.188968), correlation (0.455808)*/,
  -13, -4, -8, 9/*mean (0.191667), correlation (0.459128)*/,
  -5, 9, -3, -3/*mean (0.193196), correlation (0.458364)*/,
  -4, -7, -3, -12/*mean (0.196536), correlation (0.455782)*/,
  6, 5, 8, 0/*mean (0.1972), correlation (0.450481)*/,
  -7, 6, -6, 12/*mean (0.199438), correlation (0.458156)*/,
  -13, 6, -5, -2/*mean (0.211224), correlation (0.449548)*/,
  1, -10, 3, 10/*mean (0.211718), correlation (0.440606)*/,
  4, 1, 8, -4/*mean (0.213034), correlation (0.443177)*/,
  -2, -2, 2, -13/*mean (0.234334), correlation (0.455304)*/,
  2, -12, 12, 12/*mean (0.235684), correlation (0.443436)*/,
  -2, -13, 0, -6/*mean (0.237674), correlation (0.452525)*/,
  4, 1, 9, 3/*mean (0.23962), correlation (0.444824)*/,
  -6, -10, -3, -5/*mean (0.248459), correlation (0.439621)*/,
  -3, -13, -1, 1/*mean (0.249505), correlation (0.456666)*/,
  7, 5, 12, -11/*mean (0.00119208), correlation (0.495466)*/,
  4, -2, 5, -7/*mean (0.00372245), correlation (0.484214)*/,
  -13, 9, -9, -5/*mean (0.00741116), correlation (0.499854)*/,
  7, 1, 8, 6/*mean (0.0208952), correlation (0.499773)*/,
  7, -8, 7, 6/*mean (0.0220085), correlation (0.501609)*/,
  -7, -4, -7, 1/*mean (0.0233806), correlation (0.496568)*/,
  -8, 11, -7, -8/*mean (0.0236505), correlation (0.489719)*/,
  -13, 6, -12, -8/*mean (0.0268781), correlation (0.503487)*/,
  2, 4, 3, 9/*mean (0.0323324), correlation (0.501938)*/,
  10, -5, 12, 3/*mean (0.0399235), correlation (0.494029)*/,
  -6, -5, -6, 7/*mean (0.0420153), correlation (0.486579)*/,
  8, -3, 9, -8/*mean (0.0548021), correlation (0.484237)*/,
  2, -12, 2, 8/*mean (0.0616622), correlation (0.496642)*/,
  -11, -2, -10, 3/*mean (0.0627755), correlation (0.498563)*/,
  -12, -13, -7, -9/*mean (0.0829622), correlation (0.495491)*/,
  -11, 0, -10, -5/*mean (0.0843342), correlation (0.487146)*/,
  5, -3, 11, 8/*mean (0.0929937), correlation (0.502315)*/,
  -2, -13, -1, 12/*mean (0.113327), correlation (0.48941)*/,
  -1, -8, 0, 9/*mean (0.132119), correlation (0.467268)*/,
  -13, -11, -12, -5/*mean (0.136269), correlation (0.498771)*/,
  -10, -2, -10, 11/*mean (0.142173), correlation (0.498714)*/,
  -3, 9, -2, -13/*mean (0.144141), correlation (0.491973)*/,
  2, -3, 3, 2/*mean (0.14892), correlation (0.500782)*/,
  -9, -13, -4, 0/*mean (0.150371), correlation (0.498211)*/,
  -4, 6, -3, -10/*mean (0.152159), correlation (0.495547)*/,
  -4, 12, -2, -7/*mean (0.156152), correlation (0.496925)*/,
  -6, -11, -4, 9/*mean (0.15749), correlation (0.499222)*/,
  6, -3, 6, 11/*mean (0.159211), correlation (0.503821)*/,
  -13, 11, -5, 5/*mean (0.162427), correlation (0.501907)*/,
  11, 11, 12, 6/*mean (0.16652), correlation (0.497632)*/,
  7, -5, 12, -2/*mean (0.169141), correlation (0.484474)*/,
  -1, 12, 0, 7/*mean (0.169456), correlation (0.495339)*/,
  -4, -8, -3, -2/*mean (0.171457), correlation (0.487251)*/,
  -7, 1, -6, 7/*mean (0.175), correlation (0.500024)*/,
  -13, -12, -8, -13/*mean (0.175866), correlation (0.497523)*/,
  -7, -2, -6, -8/*mean (0.178273), correlation (0.501854)*/,
  -8, 5, -6, -9/*mean (0.181107), correlation (0.494888)*/,
  -5, -1, -4, 5/*mean (0.190227), correlation (0.482557)*/,
  -13, 7, -8, 10/*mean (0.196739), correlation (0.496503)*/,
  1, 5, 5, -13/*mean (0.19973), correlation (0.499759)*/,
  1, 0, 10, -13/*mean (0.204465), correlation (0.49873)*/,
  9, 12, 10, -1/*mean (0.209334), correlation (0.49063)*/,
  5, -8, 10, -9/*mean (0.211134), correlation (0.503011)*/,
  -1, 11, 1, -13/*mean (0.212), correlation (0.499414)*/,
  -9, -3, -6, 2/*mean (0.212168), correlation (0.480739)*/,
  -1, -10, 1, 12/*mean (0.212731), correlation (0.502523)*/,
  -13, 1, -8, -10/*mean (0.21327), correlation (0.489786)*/,
  8, -11, 10, -6/*mean (0.214159), correlation (0.488246)*/,
  2, -13, 3, -6/*mean (0.216993), correlation (0.50287)*/,
  7, -13, 12, -9/*mean (0.223639), correlation (0.470502)*/,
  -10, -10, -5, -7/*mean (0.224089), correlation (0.500852)*/,
  -10, -8, -8, -13/*mean (0.228666), correlation (0.502629)*/,
  4, -6, 8, 5/*mean (0.22906), correlation (0.498305)*/,
  3, 12, 8, -13/*mean (0.233378), correlation (0.503825)*/,
  -4, 2, -3, -3/*mean (0.234323), correlation (0.476692)*/,
  5, -13, 10, -12/*mean (0.236392), correlation (0.475462)*/,
  4, -13, 5, -1/*mean (0.236842), correlation (0.504132)*/,
  -9, 9, -4, 3/*mean (0.236977), correlation (0.497739)*/,
  0, 3, 3, -9/*mean (0.24314), correlation (0.499398)*/,
  -12, 1, -6, 1/*mean (0.243297), correlation (0.489447)*/,
  3, 2, 4, -8/*mean (0.00155196), correlation (0.553496)*/,
  -10, -10, -10, 9/*mean (0.00239541), correlation (0.54297)*/,
  8, -13, 12, 12/*mean (0.0034413), correlation (0.544361)*/,
  -8, -12, -6, -5/*mean (0.003565), correlation (0.551225)*/,
  2, 2, 3, 7/*mean (0.00835583), correlation (0.55285)*/,
  10, 6, 11, -8/*mean (0.00885065), correlation (0.540913)*/,
  6, 8, 8, -12/*mean (0.0101552), correlation (0.551085)*/,
  -7, 10, -6, 5/*mean (0.0102227), correlation (0.533635)*/,
  -3, -9, -3, 9/*mean (0.0110211), correlation (0.543121)*/,
  -1, -13, -1, 5/*mean (0.0113473), correlation (0.550173)*/,
  -3, -7, -3, 4/*mean (0.0140913), correlation (0.554774)*/,
  -8, -2, -8, 3/*mean (0.017049), correlation (0.55461)*/,
  4, 2, 12, 12/*mean (0.01778), correlation (0.546921)*/,
  2, -5, 3, 11/*mean (0.0224022), correlation (0.549667)*/,
  6, -9, 11, -13/*mean (0.029161), correlation (0.546295)*/,
  3, -1, 7, 12/*mean (0.0303081), correlation (0.548599)*/,
  11, -1, 12, 4/*mean (0.0355151), correlation (0.523943)*/,
  -3, 0, -3, 6/*mean (0.0417904), correlation (0.543395)*/,
  4, -11, 4, 12/*mean (0.0487292), correlation (0.542818)*/,
  2, -4, 2, 1/*mean (0.0575124), correlation (0.554888)*/,
  -10, -6, -8, 1/*mean (0.0594242), correlation (0.544026)*/,
  -13, 7, -11, 1/*mean (0.0597391), correlation (0.550524)*/,
  -13, 12, -11, -13/*mean (0.0608974), correlation (0.55383)*/,
  6, 0, 11, -13/*mean (0.065126), correlation (0.552006)*/,
  0, -1, 1, 4/*mean (0.074224), correlation (0.546372)*/,
  -13, 3, -9, -2/*mean (0.0808592), correlation (0.554875)*/,
  -9, 8, -6, -3/*mean (0.0883378), correlation (0.551178)*/,
  -13, -6, -8, -2/*mean (0.0901035), correlation (0.548446)*/,
  5, -9, 8, 10/*mean (0.0949843), correlation (0.554694)*/,
  2, 7, 3, -9/*mean (0.0994152), correlation (0.550979)*/,
  -1, -6, -1, -1/*mean (0.10045), correlation (0.552714)*/,
  9, 5, 11, -2/*mean (0.100686), correlation (0.552594)*/,
  11, -3, 12, -8/*mean (0.101091), correlation (0.532394)*/,
  3, 0, 3, 5/*mean (0.101147), correlation (0.525576)*/,
  -1, 4, 0, 10/*mean (0.105263), correlation (0.531498)*/,
  3, -6, 4, 5/*mean (0.110785), correlation (0.540491)*/,
  -13, 0, -10, 5/*mean (0.112798), correlation (0.536582)*/,
  5, 8, 12, 11/*mean (0.114181), correlation (0.555793)*/,
  8, 9, 9, -6/*mean (0.117431), correlation (0.553763)*/,
  7, -4, 8, -12/*mean (0.118522), correlation (0.553452)*/,
  -10, 4, -10, 9/*mean (0.12094), correlation (0.554785)*/,
  7, 3, 12, 4/*mean (0.122582), correlation (0.555825)*/,
  9, -7, 10, -2/*mean (0.124978), correlation (0.549846)*/,
  7, 0, 12, -2/*mean (0.127002), correlation (0.537452)*/,
  -1, -6, 0, -11/*mean (0.127148), correlation (0.547401)*/
};

// compute the descriptor
void ComputeORB(const cv::Mat &img, vector<cv::KeyPoint> &keypoints, vector<DescType> &descriptors) {
  const int half_patch_size = 8;
  const int half_boundary = 16;
  int bad_points = 0;
  for (auto &kp: keypoints) {
    if (kp.pt.x < half_boundary || kp.pt.y < half_boundary ||
        kp.pt.x >= img.cols - half_boundary || kp.pt.y >= img.rows - half_boundary) {
      // outside
      bad_points++;
      descriptors.push_back({});
      continue;
    }

    float m01 = 0, m10 = 0;
    for (int dx = -half_patch_size; dx < half_patch_size; ++dx) {
      for (int dy = -half_patch_size; dy < half_patch_size; ++dy) {
        uchar pixel = img.at<uchar>(kp.pt.y + dy, kp.pt.x + dx);
        m10 += dx * pixel;
        m01 += dy * pixel;
      }
    }

    // angle should be arc tan(m01/m10);
    float m_sqrt = sqrt(m01 * m01 + m10 * m10) + 1e-18; // avoid divide by zero
    float sin_theta = m01 / m_sqrt;
    float cos_theta = m10 / m_sqrt;

    // compute the angle of this point
    DescType desc(8, 0);
    for (int i = 0; i < 8; i++) {
      uint32_t d = 0;
      for (int k = 0; k < 32; k++) {
        int idx_pq = i * 32 + k;
        cv::Point2f p(ORB_pattern[idx_pq * 4], ORB_pattern[idx_pq * 4 + 1]);
        cv::Point2f q(ORB_pattern[idx_pq * 4 + 2], ORB_pattern[idx_pq * 4 + 3]);

        // rotate with theta
        cv::Point2f pp = cv::Point2f(cos_theta * p.x - sin_theta * p.y, sin_theta * p.x + cos_theta * p.y)
                         + kp.pt;
        cv::Point2f qq = cv::Point2f(cos_theta * q.x - sin_theta * q.y, sin_theta * q.x + cos_theta * q.y)
                         + kp.pt;
        if (img.at<uchar>(pp.y, pp.x) < img.at<uchar>(qq.y, qq.x)) {
          d |= 1 << k;
        }
      }
      desc[i] = d;
    }
    descriptors.push_back(desc);
  }

  cout << "bad/total: " << bad_points << "/" << keypoints.size() << endl;
}

// brute-force matching
void BfMatch(const vector<DescType> &desc1, const vector<DescType> &desc2, vector<cv::DMatch> &matches) {
  const int d_max = 40;

  for (size_t i1 = 0; i1 < desc1.size(); ++i1) {
    if (desc1[i1].empty()) continue;
    cv::DMatch m{i1, 0, 256};
    for (size_t i2 = 0; i2 < desc2.size(); ++i2) {
      if (desc2[i2].empty()) continue;
      int distance = 0;
      for (int k = 0; k < 8; k++) {
        distance += _mm_popcnt_u32(desc1[i1][k] ^ desc2[i2][k]);
      }
      if (distance < d_max && distance < m.distance) {
        m.distance = distance;
        m.trainIdx = i2;
      }
    }
    if (m.distance < d_max) {
      matches.push_back(m);
    }
  }
}

执行：

这个演示中我们只展示ORB的计算代码和匹配代码。在计算中，我们用256位的二进制描述，对应到8个32位的unsigned int数据，用typedef将它表示成DescType。
然后，我们根据的面介绍的原理计算FAST特征点的角度，再使用该角度计算描述子。此代码中通过三角函数的原理回避了复杂的arctan及sin、cos 计算，从而达到加速的效果。

在BfMatch 函数中，我们还使用了SSE指令集中的_mm_popent_u32函数计算一个unsigned int变量中1的个数，从而达到计算汉明距离的效果。该段程序的运行结果如下，匹配结果如上图所示。

可见，我们通过一些简单的算法修改，使对ORB的提取。请注意，编译这个程序需要你的CPU支持SSE指令集。如果我们能够对提取特征部分进一步并行化处理，则算法还可以有加速的空间。

7.2.3 计算相机运动

我们已经有了匹配好的点对，接下来，我们要根据点对估计相机的运动。这里由于相机的原理不同，情况发生了变化:
①当相机为单目时，我们只知道2D的像素坐标，因而问题是根据两组2D点估计运动。该
问题用对极几何解决。
②当相机为双目、RGB-D时，或者通过某种方法得到了距离信息，那么问题就是根据两组
3D点估计运动。该问题通常用ICP解决。
③如果一组为3D，一组为2D，即，我们得到了一些3D点和它们在相机的投影位置，也能
估计相机的运动。该问题通过PnP求解。
因此，下面几节介绍这三种情形下的相机运动估计。我们将从信息最少的2D-2D情形出发，看看它如何求解，求解过程中又遇到哪些麻烦的问题。

7.3 2D-2D对极几何

7.3.1 对级约束

1.概念

        现在，假设我们从两张图像中得到了一对配对好的特征点，如下图。

        如果有若干对这样的匹配点，就可以通过这些二维图像点的对应关系，恢复出在两帧之间摄像机的运动。这里“若干对”具体是多少对呢？我们会在下文介绍。下面先来介绍两个图像中的匹配点有什么几何关系。
        以下图为例，我们希望求取两帧图像 $I_{1},I_{2}$ 之间的运动，设第一帧到第二帧的运动为 $R,t$ 。两个相机中心分别为 $O_1,O_2$ 。现在，考虑 $I_{1}$ 中有一个特征点 $p_{1}$ ，它在 $I_{2}$ 中对应着特征点 $p_{2}$ 。我们知道两者是通过特征匹配得到的。

        如果匹配正确，说明它们确实是同一个空间点在两个成像平面上的投影。这里需要一些术语来描述它们之间的几何关系。

        首先，连线 $\overrightarrow{o_{1}p_{1}}$ 和连线 $\overrightarrow{o_{2}p_{2}}$ 在三维空间中会相交于点 $P$ 。这时 $O_{1},O_{2},P$ 三个点可以确定一个平面，称为极平面（Epipolarplane)。

         $O_{1}O_{2}$ 连线与像平面 $I_{1},I_{2}$ 的交点分别为 $e_{1},e_{2}$ 。 $e_{1},e_{2}$ 称为极点（Epipoles)。

         $O_{1}O_{2}$ 被称为基线。

        极平面与两个像平面 $I_{1},I_{2}$ 之间的相交线 $l_{1},l_{2}$ 为极线(Epipolar line)。

图7-15-1 对级几何约束



图7-15-2 对级几何约束解释

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

        同时，如果不知道P的位置，那么当我们在第二幅图像上看时，连线 $\overrightarrow{e_{2}p_{2}}$ (也就是第二幅图像中的极线)就是Р可能出现的投影的位置，也就是射线 $\overrightarrow{o_{1}p_{1}}$ 在第二个相机中的投影。

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

  解释一下：书上说的有点乱，我也是看了好久才看懂，如果不知道P的位置，连接极线 $\overrightarrow{e_{2}p_{2}}$ ，那么就是Р可能出现的投影的位置，如图7-5-12，A1B1完全遮挡住A2B2的成像。

2.几何关系演算

        在第一帧的坐标系下，设 $P$ 的空间位置为 $P=[X,Y,Z]^{T}$

        根据针孔相机模型，两个像素点 $p_{1},p_{2}$ 的像素位置为： $s_{1}p_{1}=KP \ ,s_{2}p_{2}=K(R_{21}P+t_{21})$ ，这里 $K$ 为相机内参矩阵， $R,t$ 为两个坐标系的相机运动。

        (注意：为什么是 $R_{21}$ ，因为 $P$ 是世界坐标即在 $O_{1}$ 参考系下看见的坐标，但不知道第二个坐标系看到的 $P$ 是什么样子，于是乘以 $R_{21}$ ，转化为第二个坐标系即 $O_{2}$ 看到的坐标值，那 $s_{2}$ 是什么， $s_{2}$ 是 $O_{2}p$ ， $p_{2}$ 是在 $O_{2}$ 参考系下的像素矩阵)

        具体来说，这里计算的是 $R_{21}$ 和 $t_{21}$ ，因为它们把第一个坐标系下的坐标转换到第二个坐标系下。也可以把它们写成李代数形式。
        有时，我们会使用齐次坐标表示像素点。在使用齐次坐标时，一个向量将等于它自身乘上任意的非零常数。这通常用于表达一个投影关系。例如， $s_{1}p_{1}$ 和 $p_{1}$ 成投影关系，它们在齐次坐标的意义下是相等的。我们称这种相等关系为尺度意义下相等(equal up to a scale)，记作： $sp\simeq p$

        那么，上述两个投影关系可写为：   $p_{1}\simeq KP \ ,p_{2}\simeq K(RP+t)$

现在取 $x_{1}=K^{-1}p_{1},x_{2}=K^{-1}p_{2}$

        这里的 $x_{1},x_{2}$ 是两个像素点的归一化平面上的坐标。代入上式，得 $x_{2}\simeq Rx_{1}+t$

        两边同时左乘 $\hat{t}$ 。回忆^的定义，这相当于两侧同时与 $t$ 做外积： $t^{\wedge }x_{2}\simeq t^{\wedge }Rx_{1}$

        两侧同时左乘 $x_{2}^{T}$ ： $x_{2}^{T}t^{\wedge }x_{2}\simeq x_{2}^{T}t^{\wedge }Rx_{1}$

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

        由于等式左侧严格为零，乘以任意非零常数之后也为零，于是我们可以把 $\simeq$ 写成通常的等号。因此，我们就得到： $x_{2}^{T}t^{\wedge }Rx_{1} = 0$

重新带入 $p_{1},p_{2}$ 有 $p_{2}^{T}K^{-T}t^{\wedge }RK^{-1}p_{1} = 0$

        这两个式子都称为对极约束。它的几何意义是 $O_{1},P,O_{2}$ 三者共面。

        对极约束中同时包含了平移和旋转。我们把中间部分记作两个矩阵：基础矩阵(Fundamental Matrix)F和本质矩阵(Essential Matrix)E，于是可以进一步简化对极约束：



$E=t^{\wedge }R,\ F=K^{-T}EK^{-1},\ x_{2}Ex_{1}=p_{2}^{T}Fp_{1}=0$
式7-1 基础矩阵与本质矩阵

3.总结

对极约束简洁地给出了两个匹配点的空间位置关系。于是，相机位姿估计问题变为以下两步：

1.根据配对点的像素位置求出 $E$ 或者 $F$ 。
2.根据 $E$ 或者 $F$ 求出 $R,t$ 。
由于 $E$ 和 $F$ 只相差了相机内参，而内参在SLAM中通常是已知的，所以实践中往往使用形式更简单的 $E$ 。我们以 $E$ 为例，介绍上面两个问题如何求解。

7.3.2 本质矩阵

1.本质矩阵的性质

        根据定义，本质矩阵 $E=t^{\wedge }R$ 。它是一个3×3的矩阵，内有9个未知数。那么，是不是任意一个3×3的矩阵都可以被当成本质矩阵呢？
        本质矩阵是由对极约束定义的。由于对极约束是等式为零的约束，所以对E乘以任意非零常数后，对极约束依然满足。我们把这件事情称为E在不同尺度下是等价的。
        根据 $E=t^{\wedge }R$ ，可以证明，本质矩阵 $E$ 的奇异值必定是 $[\sigma ,\sigma ,0]^{T}$ 的形式。这称为本质矩阵的内在性质。
        另外，由于平移和旋转各有3个自由度，故 $t^{\wedge }R$ 共有6个自由度。但由于尺度等价性，故 $E$ 实际上有5个自由度。

2.八点法求本质矩阵

   $E$ 具有5个自由度的事实，表明我们最少可以用5对点来求解 $E$ 。但是， $E$ 的内在性质是一种非线性性质，在估计时会带来麻烦，因此，也可以只考虑它的尺度等价性，使用8对点来估计 $E$ ——这就是经典的八点法（Eight-point-algorithm ) 。八点法只利用了 $E$ 的线性性质，因此可以在线性代数框架下求解。下面我们来看八点法是如何工作的：
        考虑一对匹配点，它们的归一化坐标为 $x_{1}=[u_{1},v_{1},1]^{T},x_{2}=[u_{2},v_{2},1]^{T}$ 。根据对极约束，有

                                         $(u_{2},v_{2},1)\begin{pmatrix} e_{1} & e_{2}&e_{3} \\ e_{4} &e_{5} & e_{6}\\ e_{7} & e_{8}& e_{9} \end{pmatrix} \begin{pmatrix} u_{1}\\v_{1} \\ 1 \end{pmatrix}=0$

        我们把矩阵 $E$ 展开，写成向量的形式： $e=(e_{1},e_{2},e_{3},e_{4},e_{5},e_{6},e_{7},e_{8},e_{9})^{T}$

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

$[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]\cdot e=0$

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

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

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

3.根据本质矩阵估计相机运用参数R,t

         接下来的问题是如何根据已经估得的本质矩阵E,恢复出相机的运动 $R,t$ 。这个过程是由奇异值分解（SVD）得到的。设E的SVD为： $E=U\Sigma V^{T}$

         其中 $U,V$ 为正交阵， $\Sigma$ 为奇异值矩阵。根据 $E$ 的内在性质，我们知道 $\Sigma =diag(\sigma ,\sigma ,0)$ 。在SVD分解中，对于任意一个 $E$ ，存在两个可能的 $t,R$ 与它对应：



         其中， $R_{Z}(\frac{\pi }{2})$ 表示沿 $Z$ 轴旋转90°得到旋转矩阵。同时，由于 $-E$ 和 $E$ 等价，所以对任意一个 $t$ 取负号，也会得到同样的结果。因此，从 $E$ 分解到 $t,R$ 时，一共存在4个可能的解。
        下图展示了分解本质矩阵得到的4个解。我们已知空间点在相机(蓝色线)上的投影(红色点)，想要求解相机的运动。在保持红色点不变的情况下，可以画出4种可能的情况:

图7-16分解本质矩阵得到的4个解。在保持投影点（红色点)不变的情况下，两个相机及空间
点一共有4种可能的情况

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

         如果利用E的内在性质，那么它只有5个自由度。所以最少可以通过5对点来求解相机运动。然而这种做法形式复杂，从工程实现角度考虑，由于平时通常会有几十对乃至上百对的匹配点，从8对减至5对意义并不明显。为保持简单，我们这里就只介绍基本的八点法。
        剩下的一个问题是：根据线性方程解出的 $E$ ，可能不满足 $E$ 的内在性质——它的奇异值不一定为 $\sigma ,\sigma ,0$ 的形式。这时，我们会刻意地把矩阵调整成上面的样子。通常的做法是，对八点法求得的 $E$ 进行SVD，会得到奇异值矩阵 $\Sigma =diag(\sigma_{1} ,\sigma_{2} ,\sigma _{3})$ ，不妨设 $\sigma_{1}\geqslant \sigma_{2}\geqslant \sigma _{3}$ 。取:



         这相当于是把求出来的矩阵投影到了 $E$ 所在的流形上。当然，更简单的做法是将奇异值矩阵取成 $diag(1,1,0)$ ，因为 $E$ 具有尺度等价性，所以这样做也是合理的。

7.3.3 单应矩阵

1.单应矩阵定义及其应用场景

        除了基本矩阵和本质矩阵，二视图几何中还存在另一种常见的矩阵：单应矩阵(Homography)H，它描述了两个平面之间的映射关系。

        若场景中的特征点都落在同一平面上（比如墙、地面等)，则可以通过单应性进行运动估计。这种情况在无人机携带的俯视相机或扫地机携带的顶视相机中比较常见。
        单应矩阵通常描述处于共同平面上的一些点在两张图像之间的变换关系。设图像 $I_{1}$ 和 $I_{2}$ 有一对匹配好的特征点 $p_{1}$ 和 $p_{2}$ 。这个特征点落在平面 $P$ ，设这个平面满足方程：

                                                       $n^{T}P+d=0$

其中， $n$ 是法线，截距是 $d$ 。

整理，有 $-\frac{n^{T}P}{d}=1$ ，然后，回顾本节开头的式(7.1)，得
$p_{2}\simeq K(RP+t)\simeq K(RP+t\cdot (\frac{n^{T}P}{d})) \simeq K(R-\frac{tn^{T}}{d})P\simeq K(R-\frac{tn^{T}}{d})K^{-1}p_{1}$

        于是,我们得到了一个直接描述图像坐标 $p_{1}$ 和 $p_{2}$ 之间的变换，把中间这部分记为 $H$ ，于是： $p_{2}\simeq Hp_{1}$

定义单应矩阵H为：

$K(R-\frac{tn^{T}}{d})K^{-1}$
式7-2 单应矩阵

          它的定义与旋转(t)、平移(R)及平面的参数(n)有关。

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

         请注意,这里的等号依然是 $\simeq$ 而不是普通的等号，所以H矩阵也可以乘以任意非零常数。我们在实际处理中可以令 $h_{9}$ =1(在它取非零值时)。然后根据第3行，去掉这个非零因子，于是有：

整理有：

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

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

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

        分解的方法包括数值法与解析法。与本质矩阵的分解类似，单应矩阵的分解同样会返回4组旋转矩阵与平移向量，同时，可以计算出它们分别对应的场景点所在平面的法向量。如果已知成像的地图点的深度全为正值(即在相机前方)，则又可以排除两组解。最后仅剩两组解，这时需要通过更多的先验信息进行判断。通常，我们可以通过假设已知场景平面的法向量来解决，如场景平面与相机平面平行，那么法向量 $n$ 的理论值为 $1^{T}$ 。
        单应性在SLAM 中具有重要意义。当特征点共面或者相机发生纯旋转时，基础矩阵的自由度下降，这就出现了所谓的退化( degenerate )。现实中的数据总包含一些噪声，这时如果继续使用八点法求解基础矩阵，基础矩阵多余出来的自由度将会主要由噪声决定。为了能够避免退化现象造成的影响，通常我们会同时估计基础矩阵F和单应矩阵H，选择重投影误差比较小的那个作为最终的运动估计矩阵。

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

1.实践目标

使用匹配好的特征点来计算E、F和H，进而分解E得到R, t。

2.代码Ⅰ：该函数提供了从特征点求解相机运动的部分，然后，我们在主函数中调用它,就能得到相机的运动。

void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1,
                          std::vector<KeyPoint> keypoints_2,
                          std::vector<DMatch> matches,
                          Mat &R, Mat &t) {
  // 相机内参,TUM Freiburg2
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

  //-- 把匹配点转换为vector<Point2f>的形式
  vector<Point2f> points1;
  vector<Point2f> points2;

  for (int i = 0; i < (int) matches.size(); i++) {
    points1.push_back(keypoints_1[matches[i].queryIdx].pt);
    points2.push_back(keypoints_2[matches[i].trainIdx].pt);
  }

  //-- 计算基础矩阵
  Mat fundamental_matrix;
  fundamental_matrix = findFundamentalMat(points1, points2, CV_FM_8POINT);
  cout << "fundamental_matrix is " << endl << fundamental_matrix << endl;

  //-- 计算本质矩阵
  Point2d principal_point(325.1, 249.7);  //相机光心, TUM dataset标定值
  double focal_length = 521;      //相机焦距, TUM dataset标定值
  Mat essential_matrix;
  essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
  cout << "essential_matrix is " << endl << essential_matrix << endl;

  //-- 计算单应矩阵
  //-- 但是本例中场景不是平面，单应矩阵意义不大
  Mat homography_matrix;
  homography_matrix = findHomography(points1, points2, RANSAC, 3);
  cout << "homography_matrix is " << endl << homography_matrix << endl;

  //-- 从本质矩阵中恢复旋转和平移信息.
  // 此函数仅在Opencv3中提供
  recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
  cout << "R is " << endl << R << endl;
  cout << "t is " << endl << t << endl;

}

3.代码Ⅱ：在函数中输出了E,F和H的数值，然后验证了对极约束是否成立，以及t^R和E在非零数乘下等价的事实。

int main(int argc, char **argv) {
  if (argc != 3) {
    cout << "usage: pose_estimation_2d2d 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);
  assert(img_1.data && img_2.data && "Can not load images!");

  vector<KeyPoint> keypoints_1, keypoints_2;
  vector<DMatch> matches;
  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
  cout << "一共找到了" << matches.size() << "组匹配点" << endl;

  //-- 估计两张图像间运动
  Mat R, t;
  pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);

  //-- 验证E=t^R*scale
  Mat t_x =
    (Mat_<double>(3, 3) << 0, -t.at<double>(2, 0), t.at<double>(1, 0),
      t.at<double>(2, 0), 0, -t.at<double>(0, 0),
      -t.at<double>(1, 0), t.at<double>(0, 0), 0);

  cout << "t^R=" << endl << t_x * R << endl;

  //-- 验证对极约束
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
  for (DMatch m: matches) {
    Point2d pt1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
    Mat y1 = (Mat_<double>(3, 1) << pt1.x, pt1.y, 1);
    Point2d pt2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
    Mat y2 = (Mat_<double>(3, 1) << pt2.x, pt2.y, 1);
    Mat d = y2.t() * t_x * R * y1;
    cout << "epipolar constraint = " << d << endl;
  }
  return 0;
}

4.现在，调用此程序即可看到输出结果：

liuhongwei@liuhongwei-virtual-machine:~/桌面/slambook2/ch7/build$ ./pose_estimation_2d2d ../1.png ../2.png 
-- Max dist : 95.000000 
-- Min dist : 4.000000 
一共找到了79组匹配点
fundamental_matrix is 
[4.844484382466111e-06, 0.0001222601840188731, -0.01786737827487386;
 -0.0001174326832719333, 2.122888800459598e-05, -0.01775877156212593;
 0.01799658210895528, 0.008143605989020664, 1]
essential_matrix is 
[-0.02036185505234771, -0.4007110038118444, -0.033240742498241;
 0.3939270778216368, -0.03506401846698084, 0.5857110303721015;
 -0.006788487241438231, -0.5815434272915687, -0.01438258684486259]
homography_matrix is 
[0.9497129583105288, -0.143556453147626, 31.20121878625771;
 0.04154536627445031, 0.9715568969832015, 5.306887618807696;
 -2.81813676978796e-05, 4.353702039810921e-05, 1]
R is 
[0.9985961798781877, -0.05169917220143662, 0.01152671359827862;
 0.05139607508976053, 0.9983603445075083, 0.02520051547522452;
 -0.01281065954813537, -0.02457271064688494, 0.9996159607036126]
t is 
[-0.8220841067933339;
 -0.0326974270640541;
 0.5684264241053518]
t^R=
[-0.02879601157010514, -0.5666909361828475, -0.0470095088643642;
 0.5570970160413605, -0.04958801046730488, 0.8283204827837457;
 -0.009600370724838811, -0.8224266019846685, -0.02034004937801358]

7.5 讨论：2D-2D对极几何会出现什么问题

7.4.1 尺度不确定性

        对 $t$ 长度的归一化，直接导致了单目视觉的尺度不确定性。

        例如，程序中输出的 $t$ 第一维约为0.822。这个0.822究竟是指0.822米还是0.822厘米，我们是没法确定的。因为对 $t$ 乘以任意比例常数后，对极约束依然是成立的。

        换言之，在单目SLAM中，对轨迹和地图同时缩放任意倍数，我们得到的图像依然是一样的。
        在单目视觉中，我们对两张图像的 $t$ 归一化相当于固定了尺度。虽然我们不知道它的实际长度是多少，但我们以这时的 $t$ 为单位1，计算相机运动和特征点的3D位置。这被称为单目SLAM的初始化。在初始化之后，就可以用3D-2D计算相机运动了。初始化之后的轨迹和地图的单位，就是初始化时固定的尺度。因此，单目SLAM有一步不可避免的初始化。初始化的两张图像必须有一定程度的平移，而后的轨迹和地图都将以此步的平移为单位。
        除了对 $t$ 进行归一化，另一种方法是令初始化时所有的特征点平均深度为1，也可以固定一个尺度。相比于令 $t$ 长度为1的做法，把特征点深度归一化可以控制场景的规模大小，使计算在数值上更稳定。

但相对尺度是确定的！比如1：5对应2：10。

7.4.2 初始化的纯旋转问题

        从 $E$ 分解到 $R,t$ 的过程中，如果相机发生的是纯旋转，导致 $t$ 为零，那么，得到的E也将为零，这将导致我们无从求解R。（ $E=t^{\wedge }R$ ，如果 $t$ 为0，那么 $E$ 就近似为0）

        不过，此时我们可以依靠 $H$ 求取旋转，但仅有旋转时，我们无法用三角测量估计特征点的空间位置(这将在下文提到)。

        于是，另一个结论是，单目初始化不能只有纯旋转，必须要有一定程度的平移。如果没有平移，单目将无法初始化。在实践中，如果初始化时平移太小，会使得位姿求解与三角化结果不稳定，从而导致失败。

        相对地，如果把相机左右移动而不是原地旋转，就容易让单目SLAM初始化。因而，有经验的SLAM研究人员，在单目SLAM情况下经常选择让相机进行左右平移以顺利地进行初始化。

7.4.3 多于八对点的情况

        当给定的点数多于8对时(例如，例程找到了79对匹配)，我们可以计算一个最小二乘解。回忆下式

中线性化后的对极约束，我们把左侧的系数矩阵记为 $A$ 有 $Ae=0$ 。
        对于八点法， $A$ 的大小为8×9。如果给定的匹配点多于8，则该方程构成一个超定方程
即不一定存在 $e$ 使得上式成立。

        因此，可以通过最小化一个二次型来求：

$\underset{e}{min}\begin{Vmatrix} Ae \end{Vmatrix}_{2}^{2} =\underset{e}{min}e^{T}A^{T}Ae$

        于是就求出了在最小二乘意义下的E矩阵。不过，当可能存在误匹配的情况时，我们会更倾向于使用随机采样一致性(Random Sample Concensus，RANSAC)来求，而不是最小二乘。RANSAC是一种通用的做法，适用于很多带错误数据的情况，可以处理带有错误匹配的数据。

7.6 三角测量

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

图7-17 三角测量获得像素深度信息

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

        三角测量最早由高斯提出并应用于测量学中，它在天文学、地理学的测量中都有应用。例如，我们可以通过不同季节观察到的星星的角度，估计它与我们的距离。在SLAM中，我们主要用三角化来估计像素点的距离。
        和上一节类似，考虑图像 $I_{1}$ 和 $I_{2}$ ，以左图为参考，右图的变换矩阵为 $T$ 。相机光心为 $O_{1}$ 和 $O_{2}$ 。在 $I_{1}$ 中有特征点 $p_{1}$ ，对应 $I_{2}$ 中有特征点 $p_{2}$ 。理论上，直线 $O_{1}p_{1}$ 与 $O_{2}p_{2}$ 在场景中会相交于一点 $P$ ，该点即两个特征点所对应的地图点在三维场景中的位置。然而由于噪声的影响，这两条直线往往无法相交。因此，可以通过最小二乘法求解。

         按照对极几何中的定义，设 $x_{1},x_{2}$ 为两个特征点的归一化坐标，那么它们满足：

$s_{2}x_{2}=s_{1}Rx_{1}+t$

        现在已知 $R,t$ ，我们想求解两个特征点的深度 $s_{1},s_{2}$ 。从几何上看，可以在射线 $O_{1}p_{1}$ 上寻找3D点，使其投影位置接近 $p_{2}$ 。同理，也可以在 $O_{2}p_{2}$ 上找，或者在两条线的中间找。不同的策略对应着不同的计算方式，当然它们大同小异。例如，我们希望计算 $s_{1}$ ，那么先对上式两侧左乘一个 $x_{2}^{\wedge }$ ，得

$s_{2}x_{2}^{\wedge }x_{2}=s_{1}x_{2}^{\wedge }Rx_{1}+x_{2}^{\wedge }t$

        该式左侧为零，右侧可看成 $s_{2}$ 的一个方程，可以根据它直接求得 $s_{2}$ 。有了 $s_{2},s_{1}$ 也非常容易求出。于是，我们就得到了两帧下的点的深度，确定了它们的空间坐标。当然，由于噪声的存在，我们估得的 $R,t$ 不一定精确使上式为零，所以更常见的做法是求最小二乘解而不是直接的解。

7.7 实践：三角测量

7.7.1 三角测量代码

下面，我们演示如何根据之前利用对极几何求解的相机位姿，通过三角化求出上一节特征点的空间位置。我们调用OpenCV提供的triangulation函数进行三角化。

#include <iostream>
#include <opencv2/opencv.hpp>
// #include "extra.h" // used in opencv2
using namespace std;
using namespace cv;

void find_feature_matches(
  const Mat &img_1, const Mat &img_2,
  std::vector<KeyPoint> &keypoints_1,
  std::vector<KeyPoint> &keypoints_2,
  std::vector<DMatch> &matches);

void pose_estimation_2d2d(
  const std::vector<KeyPoint> &keypoints_1,
  const std::vector<KeyPoint> &keypoints_2,
  const std::vector<DMatch> &matches,
  Mat &R, Mat &t);

void triangulation(
  const vector<KeyPoint> &keypoint_1,
  const vector<KeyPoint> &keypoint_2,
  const std::vector<DMatch> &matches,
  const Mat &R, const Mat &t,
  vector<Point3d> &points
);

/// 作图用
inline cv::Scalar get_color(float depth) {
  float up_th = 50, low_th = 10, th_range = up_th - low_th;
  if (depth > up_th) depth = up_th;
  if (depth < low_th) depth = low_th;
  return cv::Scalar(255 * depth / th_range, 0, 255 * (1 - depth / th_range));
}

// 像素坐标转相机归一化坐标
Point2f pixel2cam(const Point2d &p, const Mat &K);

int main(int argc, char **argv) {
  if (argc != 3) {
    cout << "usage: triangulation 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);

  vector<KeyPoint> keypoints_1, keypoints_2;
  vector<DMatch> matches;
  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
  cout << "一共找到了" << matches.size() << "组匹配点" << endl;

  //-- 估计两张图像间运动
  Mat R, t;
  pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);

  //-- 三角化
  vector<Point3d> points;
  triangulation(keypoints_1, keypoints_2, matches, R, t, points);

  //-- 验证三角化点与特征点的重投影关系
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
  Mat img1_plot = img_1.clone();
  Mat img2_plot = img_2.clone();
  for (int i = 0; i < matches.size(); i++) {
    // 第一个图
    float depth1 = points[i].z;
    cout << "depth: " << depth1 << endl;
    Point2d pt1_cam = pixel2cam(keypoints_1[matches[i].queryIdx].pt, K);
    cv::circle(img1_plot, keypoints_1[matches[i].queryIdx].pt, 2, get_color(depth1), 2);

    // 第二个图
    Mat pt2_trans = R * (Mat_<double>(3, 1) << points[i].x, points[i].y, points[i].z) + t;
    float depth2 = pt2_trans.at<double>(2, 0);
    cv::circle(img2_plot, keypoints_2[matches[i].trainIdx].pt, 2, get_color(depth2), 2);
  }
  cv::imshow("img 1", img1_plot);
  cv::imshow("img 2", img2_plot);
  cv::waitKey();

  return 0;
}

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
  //-- 初始化
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  // use this if you are in OpenCV2
  // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
  // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
  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);

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

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

  //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }

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

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

void pose_estimation_2d2d(
  const std::vector<KeyPoint> &keypoints_1,
  const std::vector<KeyPoint> &keypoints_2,
  const std::vector<DMatch> &matches,
  Mat &R, Mat &t) {
  // 相机内参,TUM Freiburg2
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

  //-- 把匹配点转换为vector<Point2f>的形式
  vector<Point2f> points1;
  vector<Point2f> points2;

  for (int i = 0; i < (int) matches.size(); i++) {
    points1.push_back(keypoints_1[matches[i].queryIdx].pt);
    points2.push_back(keypoints_2[matches[i].trainIdx].pt);
  }

  //-- 计算本质矩阵
  Point2d principal_point(325.1, 249.7);        //相机主点, TUM dataset标定值
  int focal_length = 521;            //相机焦距, TUM dataset标定值
  Mat essential_matrix;
  essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);

  //-- 从本质矩阵中恢复旋转和平移信息.
  recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
}

void triangulation(
  const vector<KeyPoint> &keypoint_1,
  const vector<KeyPoint> &keypoint_2,
  const std::vector<DMatch> &matches,
  const Mat &R, const Mat &t,
  vector<Point3d> &points) {
  Mat T1 = (Mat_<float>(3, 4) <<
    1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 1, 0);
  Mat T2 = (Mat_<float>(3, 4) <<
    R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),
    R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),
    R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0)
  );

  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
  vector<Point2f> pts_1, pts_2;
  for (DMatch m:matches) {
    // 将像素坐标转换至相机坐标
    pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt, K));
    pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt, K));
  }

  Mat pts_4d;
  cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);

  // 转换成非齐次坐标
  for (int i = 0; i < pts_4d.cols; i++) {
    Mat x = pts_4d.col(i);
    x /= x.at<float>(3, 0); // 归一化
    Point3d p(
      x.at<float>(0, 0),
      x.at<float>(1, 0),
      x.at<float>(2, 0)
    );
    points.push_back(p);
  }
}

Point2f pixel2cam(const Point2d &p, const Mat &K) {
  return Point2f
    (
      (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
      (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}

同时，在main 函数中加入三角测量部分，然后画出各点的深度示意图。读者可以自行运行此程序查看三角化结果。

7.7.2 讨论

        关于三角测量，还有一个必须注意的地方。三角测量是由平移得到的，有平移才会有对极几何中的三角形，才谈得上三角测量。因此，纯旋转是无法使用三角测量的，因为在平移为零时，对极约束一直为零。当然，实际数据往往不会完全等于零。在平移存在的情况下，我们还要关心三角测量的不确定性，这会引出一个三角测量的矛盾。
        如图7-18所示，当平移很小时，像素上的不确定性将导致较大的深度不确定性。也就是说，如果特征点运动一个像素 $\delta x$ ，使得视线角变化了一个角度 $\delta \theta$ ，那么将测量到深度值有 $\delta d$ 的变化。从几何关系可以看到，当 $t$ 较大时， $\delta d$ 将明显变小，这说明平移较大时，在同样的相机分辨率下，三角化测量将更精确。对该过程的定量分析可以使用正弦定理得到。
        因此，要提高三角化的精度，一种方式是提高特征点的提取精度，也就是提高图像分辨率——但这会导致图像变大，增加计算成本。另一种方式是使平移量增大。但是，这会导致图像的外观发生明显的变化，比如箱子原先被挡住的侧面显示出来，或者物体的光照发生变化，等等。外观变化会使得特征提取与匹配变得困难。总而言之，增大平移，可能导致匹配失效；而平移太小，则三角化精度不够——这就是三角化的矛盾。我们把这个问题称为“视差”( parallax)。
        在单目视觉中，由于单目图像没有深度信息，我们要等待特征点被追踪几帧之后，产生了足够的视角，再用三角化来确定新增特征点的深度值。这有时也被称为延迟三角化。但是，如果相机发生了原地旋转，导致视差很小，就不好估计新观测到的特征点的深度。这种情况在机器人场合下更常见，因为原地旋转往往是一个机器人常见的指令。在这种情况下，单目视觉就可能出现追踪失败、尺度不正确等情况。

图7-18 三角测量遇到的问题

         虽然本节只介绍了三角化的深度估计，但只要我们愿意，也能够定量地计算每个特征点的位置及不确定性。所以，如果假设特征点服从高斯分布，并且不断地对它进行观测，在信息正确的情况下，我们就能够期望它的方差不断减小乃至收敛。这就得到了一个滤波器，称为深度滤波器(Depth Filter)。