在无人驾驶中，感知、定位、规划决策、控制是四个基本的系统模块。由于当前算法还无法实现绝对的智能，因此依然需要大量的先验知识来提高模块性能、鲁棒性，以实现安全的自动驾驶。其中，高精地图是对道路及周边环境先验知识的集成。而建立在地图之上的准确定位，是判断行车状况的重要依据，为后续的感知、规划决策提供有力支撑。
用于定位的主要数据源目前主要有 GPS、激光雷达、视觉、毫米波雷达。对于视觉而言，虽然目前还没有一套产业内公认的足够可靠的定位方案，但是在这方面探索从未停止过，主要原因如下:
安全性是无人驾驶系统最重要的指标，因此大部分功能的实现，都是多源数据、不同算法结果的耦合。没有哪种传感器方案是完美的，比如 GPS RTK 作为广泛使用的方案，容易受天气状况、 数据链传输状况影响，在隧道内、室内和高楼密集区无法使用。再者，激光雷达虽然具有运算量小，提供深度信息，不受光照影响等优点，但信息稀疏，造价目前还十分昂贵，还不具备大批量车辆装配能力。相比较而言，摄像头提供的视觉信息，虽然会受到光照、天气影响，但是成本低，内容丰富，是目前辅助驾驶方案主要数据源，在地图定位方面也具有很大潜力。
由于主流基于视觉定位算法的核心思想一脉相承，所以本文仅从一系列重要算法框架组件角度，介绍了目前实践中最常用的、基于特征点的全局定位算法，即在地图坐标系下进行定位。本文省略了其中涉及到的优化、几何约束公式推导，旨在给同学们一个定位算法的宏观介绍，具体细节可以参考相关文献和书籍。
1 视觉全局定位概念
视觉全局定位，指的是根据当前图像，求出相机在地图坐标系中的 6 个自由度 (Degree of freedom, DoF) 位姿 (Pose) , 即 (x, y, z) 坐标，以及环绕三个坐标轴的角度偏转 (yaw, pitch, roll) 。目前主要可以分类为基于 3D 结构的方法、基于 2D 图像的方法、基于序列图像的方法、基于深度学习的方法。其中，基于深度学习的方法属于端到端 (End-to-end) 的方法，而其它多阶段 (Multi-stage) 非端到端方法虽然流程有所差别，但算法思路大都如 Fig. 1 所示:
Figure 1: 根据查询图像，计算 2D-3D 转换矩阵，求解相机位姿
基于已建的地图，匹配历史中最相似的地图子集（图像/点云/特征点），根据匹配到的地图子集所提供的历史位姿真值、特征点坐标真值，计算点对间的变换矩阵，求解当前相机位姿。
所以，其核心包含图像描述、建图查询、特征匹配，位姿计算四个方面。这里仅仅是技术层面的宏观分类，实际算法框架不一定按照此顺序执行，而学者在研究中主要针对这些技术进行改进。整体而言，基于特征点的图像描述基本成熟，发展较少。而位姿计算由于是基于几何约束的优化问题，所以方法也较为固定。相对地，建图查询和特征匹配中改进技术较多。根据数据源不同，建图查询、匹配可以是2D-2D，2D-3D，3D-3D。2D 图像由相机得到，3D 点云可以由提供深度的双目相机、RGB-D 相机产生。
2 特征点提取
2D 图像本身是一个由亮度、色彩组成的矩阵，对视角、光照、色调变化等很敏感，直接使用十分困难。所以，一般会使用具有代表性的点进行相关计算。人们希望这样的点具有旋转、平移、尺度、光照不变性等优点。这些点称为图像的特征 (Feature) 点，包含关键点(Key-points) 和描述子 (Descriptor) 两部分。关键点表达了特征点的位置，而描述子则是对于特征点视觉特性的描述，大多为向量形式。一般而言，描述子主要是以某种模式，统计关键点周围的灰度/色彩梯度变化。一种鲁棒的描述子，在不同图像 的不同情况下，同一特征点的描述子的距离 (Distance) 应当较小。
描述子一般是人为手工设计的 (Hand-crafted features) 。经典的描述如 HOG(Histogram of oriented gradients)[1]，SIFT(Scale-invariant feature transform)[2]，SURF(Speeded up robust features)[3]，AKAZE(Accelerated KAZE)[4] 等。
为了实时性的要求，一些计算速度更快的二值模式描述子被设计出来，如 LBP(Local binary patterns)[5]，BRIEF(Binary robust independent elementary features)，ORB(Oriented FAST and rotated BRIEF)[6]，BRISK(Binary robust invariant scalable key-point)[7]，FREAK(Fast retina key-point)[8] 等。
在深度学习流行之前，这些手工特征一直引领着整个计算视觉产业，直到今天，这些特征在那些缺少标注数据、约束较多的场景下，依然被广泛应用。下面简单介绍两类常用的描述子。
SIFT
SIFT 描述子可以算是 CV 界最具影响力的技术之一。从关键点检测层面，主要使用高斯差分 (Difference of Gaussian, DoG) 方法检测多尺度空间上的极值点，作为关键点。而 Babaud 等人 [9] 证明了高斯平滑是唯一的能用多尺度空间平滑滤波核，为相关方法提供了充足的理论支持。
那么为什么这样的方法可以找到特征关键点呢?
由于高斯核可以通过模糊的方式把图像缩放到不同尺度空间，而梯度变化较小的平滑区域在不同尺度空间的值差距较小。相反，边缘、点、角、纹理等区域则差距较大。这样通过对相邻尺度的图像做差分，最终可以算得多尺度空间的极值点。但是，不同的图像细节本身就处于不同的尺度中。比如一副人物画像中，人脸可能经过较小的模糊就会被平滑为一片，而画框的角则可能需要更大尺度的平滑才会体现出局部“极值”。
因此，如 Fig. 2 所示，首先利用图像金字塔将图像先分组 (Octave) ，每组中再使用不同尺度的高斯核，形成一系列的层。这种方式比单纯地使用更多尺度的高斯核效果更好，可以检测到更多的特征点。需要注意的是，虽然 SIFT 使用了 DoG 进行关键点检测，但是其它检测方法也是可行的，并不影响 SIFT 描述子的建立。
Figure 2: 高斯差分方法
SIFT 特征点的描述子，可以理解为一种简单统计版的 HOG。如 Fig. 3所示，以检测到的关键点为中心，选取周围 16 × 16 的区域，将区域再组织为 4 个 4 × 4 的块(Patch)。对每一个块，使用 8-bins 的直方图对梯度进行统计，梯度方向决定落入哪个 bin，而梯度的模决定值的大小。为了保证尺度一致性，梯度大小需要进行归一化。为了保证旋转不变性，会根据 16 × 16 的区域内的所有梯度计算出一个主方向， 所有梯度按照主方向进行旋转。最终形成 4 × 4 × 8 的 128 维向量。
Figure 3: 基于梯度分块统计的 SIFT 描述子
二值描述子
虽然在 SIFT 提出后，又产生了一些改进算法如 SURF、AKAZE 等，但是即使放在 2019 年的今天， 依然难以保证一些场景对算法实时性的要求。例如，手持设备一般算力有限。而无人驾驶中，CPU、GPU资源需要被多个计算密集型模块同时调度。因此，效率是考察算法实用性的重要指标。
为了提高效率，一些二值描述子被学者们提出。一般地，这些方法都是在特征关键点周围进行点采 样。然后比较一对点的灰度大小，结果以 0/1 表示，形成 N 维的二进制描述向量，构成特征点的二值模式。而不同二值描述子最大的差别，主要在于特征采样模式不同、点对选取方法不同。
Figure 4: LBP 描述子采样模式
如 Fig. 4所示，LBP 描述子采用对关键点周围，进行环形采样，并与中心关键点的灰度进行比较的方案。圆环上展示了灰度比较结果，黑色的点是 0，白色的点是 1。LBP 是二值描述子最简单的形式，而 ORB 改进了 BRIEF 特征，是目前比较常用的二值描述子。如 Fig. 5所示，在点对选取上，与单纯使用中心点不同，ORB 采用了随机的方式，更全面地描述局部细节。但点对的相关性会比较大，从而降低描述子的判别性(Discriminative)。ORB 直接采用了贪婪法、穷举法解决这一问题，寻找相关性低的随机点对。
Figure 5: ORB 描述子点对选取模式
以上二值描述子的采样方式和点对选取方式符合人们一般直觉，而 BRISK、FREAK 等描述子则提供了更加规则化、自带尺度信息的二值模式构建方法。例如，FREAK 描述子模仿了人眼的视觉采样模式。如 Fig. 6所示，每个采样点的值是红色圆圈范围内的灰度均值，蓝线则表示点对选取方案。
Figure 6: FREAK 描述子采样、点对选取摸式
二值描述子的高效率，主要体现在三个方面。
(1)二值描述子使用二进制向量作为特征描述，只需要 比较点对大小而不需要计算具体梯度。
(2)两个描述子之间比较可以使用计算更快，更容易优化的汉明距离 (Hamming distance)。
(3)由于每个二进制向量都对应一个十进制数，所以其本身也代了表一种模 式，而不需要像 SIFT 一样使用直方图进行表示。
二值描述子一般判别性不如 SIFT 家族描述子，但在特定场景下，配合并行化编程，可以在保证相似判别能力的同时，效率高出几十甚至百倍。
3 数据库建立与查询
数据库可以理解为于地图 + 索引的集成。地图可以是由单纯的 2D 图像组成，也可以是由 3D 点云地图组成，也可以是 2D 图像和 3D 点云的结合。3D 点云地图生成主要使用三维重建的方法 SfM(Structure from motion)，从时间序列的 2D 图像中推算 3D 信息。如果有双目、RGB-D 相机提供深度，可以获得 更准确的 3D 点信息。其中也包含了一些诸如关键帧(Key-frame)的选取策略，具体方法超出了本文的讨论范围，有兴趣的同学可以自行查阅相关资料。数据库的作用在于:
对于一张输入的观测图像，通过数据库，查询建图历史（图像/点云/特征点），得到当前图像最可能观测到的地图子集（图像/点云/特征点），将地图与观测信息进行匹配，计算变换矩阵，得到观测相机的位姿。
索引则是加速这一过程的关键。数据库本身往往是巨大的。以美团的小袋机器人在北京朝阳大悦城二层试运营为例，安装有 3 个深度相机，即使经过筛选，也使用了将近 8 万张 900 × 600 的图片。考虑到定位所需要的实时性，查询时不可能每次都和 8 万张图片一一对比，所以要使用索引技术加速整个算法。这方面技术与 SLAM 中的回环测试，视觉中的图像检索、位置识别等高度重合，以下仅介绍一般方法。
一张图像内有若干特征点，需要先对特征点进行编码，如 VLAD(Vector of locally aggregated descriptors) 编码，用局部描述子形成图像的全局描述。再使用索引，如 kd-tree，进行图像级查询。当然，编码和索引也可以同时进行，如层次化词袋模型(Bag-of-words，BoW)+ 正向索引 + 逆向索引的方法。
VLAD 编码
VLAD(Vector of locally aggregated descriptors)[10]，如 Fig. 7所示，是一种通过聚合局部描述子形成码本 (Codebook) ，通过累加计算描述子与码词 (Word) 的距离，进行全局编码的简单方法。一个 
维描述子 
通过 
个码词的码本进行编码，可以形成一个 
维的描述向量，向量中的值是描述子与第 
个码词在第 
维的差。之后进行 
归一化，形成最后的 VLAD 向量。
Figure 7: VLAD 通过描述子与码词的距离进行编码
这里要特别提介绍一下 DenseVLAD[11] 和 NetVLAD[12] 。Torii 等人证明，DenseSIFT 在查询、匹配上都优于标准 SIFT。DenseVLAD 在四个尺度，以 2 个像素间隔的网格状采样模式，提取 SIFT 点。在全局随机采样 25M 个描述子，用 k-means 算法生成 128 个码词的码本。VLAD 向量在归一化后使用 PCA(Principal component analysis) 降维，形成最后 4096 维的 DenseVLAD 向量。如 Fig. 8所示，使用DenseSIFT 匹配后的内点（绿）数量更多。
Figure 8: DenseSIFT 和标准 SIFT 特征点，匹配后内点(绿)对比
而 NetVLAD，将 VLAD 中加入了监督信息，加强 VLAD 编码的判别性。如 Fig. 9所示，假设红、绿两个描述子来源于不应匹配到一起的两张图片。由于它们都离 VLAD 中心(×)半径较大且距离相似，经过 L2 归一化，它们编码后值也会很相似。而加入了红、绿描述子所对应图片不匹配的监督信息后，NetVLAD 生成的中心点(★)则可以更好地区分两个描述子，增加他们编码后的距离（半径）差。
Figure 9: NetVLAD 聚类中心(×)与 VLAD 聚类中心(★)对比
BoW 编码 + 索引
基于词袋模型 BoW[13, 14] 的特征编码及其设计思想在计算机视觉发展中具有举足轻重的地位，这里不再展开介绍。本文以 2D 查询图像匹配 2D 图像数据库为例，介绍一种常见的 BoW 编码、索引一体化的模型。如 Fig. 10所示，词典 (Vocabulary) 生成采用层次化方法，对于数据集中的所有描述子，按树状结构进行空间划分，每一层都是由 k-means 聚类计算。最终叶子节点就相当于码词(Fig. 10中有 9个码词)。
Figure 10: 带正向索引、逆向索引的层次化 BoW 模型
树的构造过程，实际上就是将原始图像编码的过程。但是编码本身并不能加快搜索过程，与 VLAD 相似，还是需要与数据库中的图像逐一比较。因此，这里设计了一种逆向索引(Inverse index) ，不需要比较编码后的向量。其原理如 Fig. 11所示，对于一张查询图像 (Query image) ，将提取的描述子输入到 BoW 中，最终会落入码词叶子结点 (Visual word) k 中。
而每个码词对应一个索引，记录码词 
对于数据库中第 
张图的权重 
(Fig.10)。这里权重使用 TF-IDF(Term frequency–inverse document frequency) 计算。即如果一个词 
在某个图像 
中出现频率高，在其它图像出现频率低，则这个词对于图像判别性较好，权重值 
较高。最终通过投票 (Voting) 机制，选出匹配图像。同样需要注意的是，逆向索引不一定建立在树形结构的 BoW 上，它仅仅是提供一种快速查询的方法。
Figure 11: 通过逆向索引 + 投票机制，直接查询图像
而正向索引 (Direct Index) 的作用主要是记录构造 BoW 时，数据库图片的特征点都落入了哪些结点中，这样当查询到图像后，不需要计算特征点，可以直接通过索引提取特征点。
3D 点云查询
2D 图像查询中，是先从语意层面查询图像，因此可以通过图像对特征点的空间范围进行约束。3D 点云查询没有这样的约束，所以具诸多难点。如需要考虑空间连续性，查询到的点是否都在可观测范围内等。这里仅介绍 Sattler 在 TPAMI 2016 上发表的方法 [15]，经过多年的打磨，这套方法框架相对简洁、完善。由于其中的词典编码搜索步骤与上节内容有所重叠，这里仅介绍 Active Search 和 Visbility Filtering 两种机制。
Active Search 主要是为了使得匹配到的 3D 点尽可能空间中临近、有几何意义。如 Fig. 12所示，红 色的点通过一系列编码、精化过程(红线)，匹配到了点云中一个点。根据所提出优先排序(Prioritization) 框架，从点云中找到一个概率最大的 3D 点，并反向(蓝线)匹配查询图像中的一个对应的 2D 点。
Figure 12: Active Search
Figure 13: Visbility Filtering
Visbility Filtering 主要是为了让匹配到的点尽可能可以被相机观测到（定位是无监督的，并不能知道所匹配到的点是否正确）。这里采用的方法是在使用 SfM 建立 3D 点云地图时，同时建立一个双向可见图 (Bipartite visibility graph) 。如 Fig. 13（左）所示，当一个点可以同时被两个相机观测时，则建立拓扑关系。Fig. 13（中）里，蓝色的点为匹配到的点，它们从观测视角上存在冲突。通过在已有拓扑上进 行图聚类，将相机两两分组，如 Fig. 13（右）。这样就可以生成新的图拓扑关系。之后通过判断每个子图(Sub-graph)间的重合情况，过滤掉那些那大概率不可见的点。
需要说明的是，虽然双目相机和 RGB-D 相机可以获取深度，查询 2D 图像也可以获得限定范围内的 3D 特征点坐标，但是由于目前技术限制，在室内材质复杂，室外大尺度场景下，深度并不可靠。所以 2D图像点和 3D 点云地图的匹配依然是一种重要的方法。
4 特征点匹配
特征点匹配过程可以是在数据库查询中自适应完成的，这多见于基于 3D 结构的查询。匹配也可以是在查询后单独进行，多见于基于 2D 图像查询。特征匹配的目的是，为后续的变换矩阵计算提供匹配的点对集，实现位姿的解算。
经典 RANSAC
随机抽样一致算法 (Random sample consensus，RANSAC)[16] 是一种经典的数据过滤、参数拟合算法。它假设数据（内点，Inliers）分布符合一定的数学模型，通过迭代计算，去除外点 (Outliers) 、噪声点， 同时获取概率上最佳的模型参数。在全局定位中，内点指正确的匹配，外点指错误的匹配，参数模型指匹配点对的空间变换矩阵。如 Fig. 14所示，经过 RANSAC 算法优化后，匹配更加合理。RANSAC 所期望找到的匹配子集需要满足两个指标：内点重投影误差尽可能小；内点数量尽可能多。所以基本流程如下：
- ①采样初始子集。
- ②计算变换矩阵。
- ③ 根据变换矩阵计算匹配点的重投影误差。
- ④ 去除误差较大的点
- ⑤ 循环①-④，保留最满足指标的匹配方案。
Figure 14: （上）原始特征匹配；（下）经过 RANSAC 算法优化后的匹配
其中，初始候选匹配是根据描述子之间的距离产生的，但重投影误差则只和关键点的空间位置有关， 与描述子本身无关。具体投影矩阵方法请参考“2.4 位姿计算”。需要指出的是，RANSAC 算法受到原始匹 配误差和参数选择的影响，只能保证算法有足够高的概率合理，不一定得到最优的结果。算法参数主要包括阈值和迭代次数。RANSAC 得到可信模型的概率与迭代次数成正比，所得到的匹配数量和阈值成反比。因此实际使用时，可能需要反复尝试不同的参数设置才能得到较优的结果。
学者们对经典 RANSAC 算法进行了很多改进，如 Fig. 15所示，提出了全局 RANSAC(Universal- RANSAC)[17] 的结构图，形成了具有普适性的 RANSAC 架构，涵盖了几乎所有的 RANSAC 的改进方 面，如预滤波、最小子集采样、由最小子集生成可靠模型、参数校验、模型精化。
Figure 15: Universal-RANSAC 通用算法框架
可微分 RANSAC
由于手工描述子在定位领域依然表现出较高的性能，所以一些学者开始探索使用深度学习代替算法框架中的某些部分，而不是直接使用端到端的位姿估计模型完全代替传统方法。可微分 RANSAC(Differentiable RANSAC，DSAC)[18] 旨在用概率假说选择代替确定性假说选择，使得 RANSAC 过程可以被求导，流程如 Fig. 16所示，其中“Scoring”步骤依然采用重投影误差作为指标，所不同的是，误差是基于整张图像而不是特征点，而原先筛选特征点匹配的过程被换为了直接以概率筛选相机位姿假设 h 的过程。虽然目 前方法局限性比较大，但 DSAC 为如何在当前无监督为主的定位算法框架中加入先验知识，提供了一种可行的思路。
Figure 16: 差分 RANSAC 算法框架
5 位姿计算
对于已获得的正确匹配点对，需要通过几何约束计算相应的变换矩阵 (Transformation matrix) 。由于数据库中的点坐标，采样时的相机位姿已知，所以可以通过匹配点对于地图点的变换矩阵，得到当前相机的位姿。此处定义一些基本符号。相机内参为 
，变换矩 
的齐次形式为:
其中， 为旋转矩阵， 为平移矩阵。
2.4.1 2D-2D 变换矩阵计算
Figure 17: 2D-2D 变换矩阵计算中的对极几何
对于两张二维图像中的已匹配好的特征点对 () ，它们在归一化平面上的坐标为 ()，需要通过对极约束计算出相应的变换矩阵。如 Fig. 17所示，其几何意义是 三者共面，这个面也被称为极平面，
称为基线，
称为极线。对极约束中同时包含了平移和旋转，定义为:
其中，
是 
在归一化平面上的坐标，∧ 是外积运算符。将公式中间部分计为基础矩阵 
和本质矩阵 
，则有:
由于本质矩阵 
不具有尺度信息，所以 E 乘以任意非零常数后对极约束依然成立。
可以通过经典的 8 点法求解(Eight-point-algorithm)，然后分解得到 
, 
。因此可以看出 2D-2D 的变换矩阵求解方式有两个缺点，首先单目视觉具有尺度不确定性，尺度信息需要在初始化中由 
提供。相应地，单目初始化不能只有纯旋转，必须要有足够程度的平移，否则会导致 
为零。
2D-3D 变换矩阵计算
2D-3D 匹配是位姿估计中重要的一种方法。一般使用 PnP 法，即已知 
对 2D-3D 匹配点，求解变换矩阵，以获得相机位姿。我们将 3D 点 P(X, Y, Z) 投影到相机成像平面 (
) 上：
其中，
为尺度，
。这个等式的求解可以化为一个线性方程问题，每个特征可以提供两个线性约束:
这样，最少可以通过 6 对匹配点进行求解，而匹配数大于 6 时可以使用 SVD 等方法通过构造最小二乘 求解。P3P 法可以看作是 PnP 法的特殊解法，如 Fig. 18所示，利用三角形相似性质增加更多约束，只需要 3 对点就可以求解。其它解法还有直接线性变换法 (Direct linear transformation，DLT)，EPnP(Efficient PnP) 法，和 UPnP(Uncalibrated PnP)等。相对于以上线性优化方法，非线性优化方法如Bundle Adjustment(BA) 也有着广泛的应用。BA 方法在视觉 SLAM 中是一种“万金油”的存在，可以同时优化多个变量，这样可以一定程度缓解局部误差带来的系统不鲁棒，感兴趣的同学可以翻阅相关资料更深入地进行了解。
Figure 18: 2D-3D 变换矩阵计算中的 P3P 方法
3D-3D 变换矩阵计算
3D 点之间的变换矩阵可以用迭代最近点(Iterative closet point, ICP)算法求解。假设点对匹配 (
) 结果正确，则求得的转换矩阵应当尽量减少重投影误差 
。可以使用 SVD 求解最小二乘问题:
或者使用建立在李代数上的非线性优化方法 Bundle Adjustment 求解
其中，
表示相机位姿。这里的优化目标与 2D-3D 匹配中的 Bundle Adjustment 的类似，但是不需要考虑相机内参 
，因为通过双目相机或者 RGB-D 深度相机，已经把原本图像上的 2D 点从相机成像平面投影到 3D 世界中。
ICP 问题已经被证明存在唯一解和无穷多解的情况。因此，在存在唯一解的情况下，优化函数相当于是一个凸函数，极小值即是全局最优解，无论采取何种初始化，都可以求得这一唯解。这是 ICP 方法的一大优点。
本文从图像描述、建图查询、特征匹配，位姿计算四个方面介绍了基于特征点的位姿估计算法。虽然传统视觉全局定位方法目前依然是实际应用中的首选，但是，传统方法是建立在特征点被正确定义、正确提取、正确匹配、正确观测的前提下进行的，这一前提对于视觉本身而言就是巨大的挑战。其次，由于传统方法是 multi-stage 框架，而非 end-to-end，所以中间每个环节，环节之间的交互，都需要众多参数调整，每个环节的技术都可以作为一个单独的研究方向。实际应用时，也需要加入对应具体场景的大量tricks，工程上比较复杂。
而人们对 end-to-end 方法的期望催生出了如 PoseNet，VLocNet，HourglassNet 等网络，在 benchmark上取得了不错的成绩。笔者认为目前 end-to-end 的方法还存在很多问题，主要有 loss function 缺少几何 约束，建图时位姿的 6 自由度空间并不连续，与输入空间难以形成良好映射，而且缺少相应的位姿回归、 精化机制等。不能否认，作为非线性空间最有力的建模工具，深度学习在未来会更多地出现在定位领域中。
回归到视觉定位本身，由于视觉最重要的优势就是成本低、语意丰富、使用场景限制少。因此，以视觉为主，其它低成本传感器为辅的定位融合方案在未来也将会是一个重要的课题。
