SLAM第二篇：视觉里程计
- 转载自：SLAM第二篇：视觉里程计 | ExBot易科机器人实验室 http://blog.exbot.net/archives/2546
上一讲中，我们介绍了组成SLAM系统的各个模块。以后，我们会详细介绍各模块的主要算法步骤，以及各种方法的特点和优势。
本讲主要关注视觉里程计。为了讲清楚它的来龙去脉，我们会进行一些公式的推导。视觉里程计算法需要大量的几何知识，我们将在必要的时候，以Tips的形式介绍一些背景知识。
里程计
在里程计问题中，我们希望测量一个运动物体的轨迹。这可以通过许多不同的手段来实现。例如，我们在汽车轮胎上安装计数码盘，就可以得到轮胎转动的距离，从而得到汽车的估计。或者，也可以测量汽车的速度、加速度，通过时间积分来计算它的位移。完成这种运动估计的装置（包括硬件和算法）叫做里程计（Odometry）。里程计一个很重要的特性，是它只关心局部时间上的运动，多数时候是指两个时刻间的运动。当我们以某种间隔对时间进行采样时，就可估计运动物体在各时间间隔之内的运动。由于这个估计受噪声影响，先前时刻的估计误差，会累加到后面时间的运动之上，这种现象称为漂移（Drift）。
漂移是我们不希望看到的，它们扰乱全局轨迹的估计。但是，如果没有其他校正机制，而只有局部运动的情况下，这也是所有里程计都不可避免的现象之一。视觉里程计如果一个里程计主要依靠视觉传感器，比如单目、双目相机，我们就叫它视觉里程计。和传统里程计一样，视觉里程计最主要的问题是如何从几个相邻图像中，估计相机的运动。
相邻图像间的相似性，为我们估计相机运动提供了依据。目前，视觉里程计的主要方法分为基于特征点的方法和不使用特征点的直接法两种。特征点方法也叫稀疏方法，而使用特征点描述的也叫稠密方法。
Tips
我们知道，相机能够把三维空间中的信息变成一张二维的照片。这件事情是怎么做到的呢？我们需要一个数学模型来清楚地描述它，也就是相机模型。其中，针孔模型是最常用，也是最简单的相机模型。在这个模型中，我们考虑一个空间点X=[x,y,z]，它投影在相机平面，并产生了一个像素p，位于照片中的[u,v]位置。假设相机光圈中心对准z轴，成像平面位于z=1处，那么根据下图描述的关系，投影方程即为：
写成惯用的矩阵形式，则有：
把z写在左边，写成zp的形式，表示z为p的深度值。我们可以看到，投影公式的形式是相当简单的。实际当中，每个相机的焦距、光圈中心都有所不同，这些称为它们的内参，一般用C表示。于是投影方程变为：
又，因为相机本身相对外部世界存在着一个运动。这个运动包括三维空间里的旋转和位移，由一个旋转矩阵R和平移向量T描述，所以最后的投影关系为：
这个方程描述了相机内参、位姿和像素之间的关系。其中内参可以通过标定相机求取，p可以在图像中观察到，而z,X,R,T则是待估计的变量。
特征匹配
基于特征的方法是当前视觉里程计的主流方式，有很长时间的研究历史。特征方法认为，对于两张图像，应该首先选取一些具有代表性的点，称为特征点。之后，仅针对这些特征点估计相机的运动，同时估计特征点的空间位置。图像里其他非特征点的信息，则被丢弃了。特征点方法把一个对图像的运动估计转换为对两组点之间的运动估计。于是，它的主要问题为：1. 我们如何获取图像特征点？如何匹配它们？2. 如何根据已知特征点，计算相机的运动？计算特征点第一个问题属于计算机视觉的研究范围，和几何关系不大，我们在此简单地加以介绍。过去的研究中，人们设计了很多特征点提取方法，包括图像中的角点、色块等。近年来流行的特征点计算则更为复杂，主要的目的是，在图像发生一定的改变后，特征点提取算法仍能提取出相同的点，并能判别它们之间的相关性。常用的特征点有Harris角点、SIFT、SURF、ORB等。它们能够标识出，计算机认为图像里哪些点比较特殊，具有较重要的意义。
一张图像中的Sift特征点对于每一个特征点，为了说明它与其他点的区别，人们还使用“描述子”（Descriptor）对它们加以描述。描述子通常是一个向量，含有特征点和周围区域的信息。如果两个特征点的描述子相似，我们就可以认为它们是同一个点。根据特征点和描述子的信息，我们可以计算出两张图像中的匹配点。匹配点的示意图根据匹配好的特征点估计相机运动在匹配好特征点后，我们可以得到两个一一对应的像素点集。接下来要做的，就是根据两组匹配好的点集，计算相机的运动了。在普通的单目成像中，我们只知道这两组点的像素坐标。而在双目和RGBD配置中，我们还知道该特征点离相机的距离。因此，该问题就出现了多种形式：
· 2D-2D形式：通过两个图像的像素位置来估计相机的运动。· 3D-2D形式：假设已知其中一组点的3D坐标，以及另一组点的2D坐标，求相机运动。· 3D-3D形式:两组点的3D坐标均已知，估计相机的运动。
那么问题就来了：是否需要为这三种情况设计不同的计算方法呢？答案是：既可以单独做，也可以统一到一个大框架里去做。
· 单独做的时候，2D-2D使用对极几何的方法，3D-2D使用PnP求解算法，而3D-3D则称为ICP方法（准确地说，ICP不需要各点的配对关系）。· 统一的框架，就是指把所有未知变量均作为优化变量，而几何关系则是优化变量之间的约束。由于噪声的存在，几何约束通常无法完美满足。于是，我们把与约束不一致的地方写进误差函数。通过最小化误差函数，来求得各个变量的估计值。这种思路也称为Bundle Adjustment(BA，中文亦称捆集优化或光束法平差)。代数方法简洁优美，但是它们对于噪声的容忍性较差。存在误匹配，或者像素坐标存在较大误差时，它给出的解会不可靠。而在优化方法中，我们先猜测一个初始值，然后根据梯度方向进行迭代，使误差下降。Bundle Adjustment非常通用，适用于任意可以建模的模型。但是，由于优化问题本身非凸、非线性，使得迭代方法往往只能求出局部最优解，而无法获得全局最优解。也就是说，只有在初始值足够好的情况下，我们才能希望得到一个满意的解。因此，在实际的VO中，我们会结合这两种方法的优点。先使用代数方法估计一个粗略的运动，然后再用Bundle Adjustment进行优化，求得可精确的值。重建
在2D-2D情形下，我们通过两个图像的像素位置，估计了相机的运动。在此过程中，并没有用到这些像素点的3D空间位置信息（而且我们也不知道）。不过，在得到相机运动之后，就可以根据这个运动信息，计算各个特征点的空间位置，该问题也称为三角化（Triangularization）。在原理上，只要我们在空间中的不同位置拍摄到了同一个点，就可以通过照片中的像素位置，来估计这个点在真实空间中的位置。
在SLAM中，三角化的结果，可以视为一种地图。这样，我们建立了对地图的一种点云描述:即通过大量的空间点，来估计整个地图的样貌。这虽然是一种较粗略的描述，但我们也能看出地图里究竟有些什么东西。在特征点方法中，因为我们只重构特征点的空间位置，这种方法也常常被称为稀疏重构。直接法（Direct Methods）除了使用特征点以外，直接法为我们提供了另一种思路。直接法认为，对图像提取特征点的过程中，丢弃了图像里大量有用的信息。因为之后的运动估计、重建过程并未使用除特征点以外的信息。举例来说，对于一张640x480的图像，原本有30万左右的像素，但是我们只用了其中几百个特征点而已。此外，由于特征点的设计缺陷，我们无法保证对于每个图，都能提以足够的、有效的特征点。经验告诉我们，这只是在大部分时候是可行的。直接法跳过了提取特征点的步骤。它构建一个优化问题，直接根据像素信息（通常是亮度），来估计相机的运动。这种方法省去了提特征的时间，然而代价则是，利用了所有信息之后，使得优化问题规模远远大于使用特征点的规模。因此，基于直接法的VO，多数需要GPU加速，才能做到实时化。此外，直接方法没有提特征点，它假设相机运动是连续的、缓慢的。只有在图像足够相似时才有效。而特征点方法在图像差异较大时也能工作。因为利用了图像中所有的信息，直接法重构的地图是稠密的，这与基于稀疏特征点的VO有很大不同。在稠密地图里，你可以看到每处的细节，而不是离散的点。