【泡泡机器人翻译专栏】视觉里程计——第一部分：前30年的发展和理论基础(上)中，我们翻译了视觉里程计的历史和视觉里程计问题的公式推导，本节主要介绍相机模型及校准方式、运动估计及结论等余下内容。
视觉里程计可以采用透视相机和全景相机。我们将在本章主要讨论一下相机的模型。
透视相机常用模型是针孔射影系统：物体反射的光线通过透镜中心（投影中心）在焦点平面形成图像，如图3a所示。令X=[x,y,z]T为相机参考帧上场景中的一个点，p=[u,v]T为图像屏幕上的一个像素的投影。从3D世界到2D图像之间的对应关系可以通过一下透视投影等式表示：
其中，λ是深度因子，αu和αv是焦距，u0和v0是图像投影中心的坐标。这些参数称为内参。当相机的视角大于45o时，就会产生光线扭曲，可以通过二次或更高阶多项式构建模型。完整的模型推到可以参考计算机视觉的教科书，如论文22和论文63所示。令p=[u,v,1]T=K-1[u,v,1]T为类似的图像坐标。归一化坐标将在后面的章节中大量使用。
全景相机的视角非常宽（甚至超过180度），可以用鱼眼镜头或通过镜像合并标准相机来制造[也称为反折射相机，如图3b所示]。反折射相机中典型的镜头形状是旋转的二次曲面（比如抛物面或双曲线体），因为它们产生一个投影中心，使得采用运动估计成为可能，这将在“运动估计”中详细讲解。
图3 （a）透视投影，（b）反射投影，和（c）透视相机和全景相机地球形模型。
图像点朝向观察点，在单位圆球中标准化。
目前，有两种全景相机的模型。第一个是论文64中的反折射相机（抛物面或双曲线的），第二个是论文65中的针对鱼眼和反折射相机统一的模型。论文66和论文67对着两种模型进行了研究。统一模型的投影等式为：
其中，和a0,a1,…,an是相机内参，取决于它是镜面镜头还是鱼眼镜头。如论文65所示，n=4对镜面或鱼眼镜头来说是一个比较恰当的选择。最后，这个模型假设图像平面满足理想特性，相机感光平面的轴心和镜面轴心是对齐的。尽管这个假设只是针对鱼眼镜头和反折射相机的，但可以通过引入理想平面和图像平面的透视投影构建偏移模型，如论文66所示。
我们希望相机有一个有效单视点(single effective view point)，即在视觉系统中只存在一个投影中心。其主要原因是单视点可从全方位摄像头采集的图像中产生正确的几何透视图像，同时还适用于成熟的外极线几何理论。反折射相机中，当光线通过镜面相交于一个点C时，才会形成投影中心。如果存在这个点，可以使我们能够从一个单视点到圆球通过对应关系构建全景投影模型。为了方便，通常会采用单位球体。
值得注意的是球体模型不仅可以应用于全景相机中，还可以应用于透视相机。如果相机校准过，透视图像或全景图像上的任何点就可以映射到单位球体上的一个向量。如图3（C）所示，单位向量指向观察到的图像场景中的点。这些向量是单位球体上归一化图像点。
相机校准的目的是为了获得相机系统更精确的内部参数和外部参数。在多相机系统中（比如立体或三相机），外部参数描述了每个相机对的彼此位置关系和方向信息。最常用的模型是采用平面棋盘模型。棋盘上方块的位置已知。为了精确地计算校准参数，用户需要在不同的位置和方向拍几张棋盘的图片，拍的时候要确保相机视角尽可能地覆盖整个场景。相机的内部参数和外部参数可以通过最小二乘法计算。输入数据是棋盘方格的四角的2D位置和它们对应的每幅图像的像素坐标。
许多相机校准工具都有MATLAB和C版本。最新的可以在网页68中找到。在这些工具中，最常用的是论文69中的Matlab和论文70到72的透视和全景相机。透视相机的C语言校准版本OpenCV，网页73中有提供一个开源计算机视觉库OpenCV。
运动估计对每幅图像来说在视觉里程计中是一个核心步骤。精确的说，是计算运动估计中当前图像和之前的图像间的相机运动。将这些单次运动链接起来，就可以重构相机的运动轨迹。本章详细解释如何从时刻K-1和K的两组对应的特征fk-1和fk中计算两幅图像Ik-1和Ik的变换Tk。根据特征对应关系是2维还是3维，有3中不同的方法：
注意，特征可能是点或者线。通常，在无结构的场景中由于缺乏直线，就在视觉里程计中使用点特征。一个深入分析这三种点和线特征方法的论文，在参考论文74中可见。本文只讲解点特征的推导。
4.1 2D到2D：图像特征匹配中的运动
4.1.1 估计本征矩阵
校准过的相机图像Ik和Ik-1之间的几何关系可以通过本征矩阵E表示。E中包含了相机运动参数具有一个未知的平移变换因子：
符号
表示等式右边是标量乘法。
2D到2D的特征对应关系可以计算本征矩阵，旋转和平移矩阵可以通过本征矩阵进行分解计算。2D到2D的运动估计最重要的特征是对极约束，它形成一条直线连接两个对应的特征点
和
，是另一幅图像像对应的特征点，如图4所示。
这个约束可以通过等式 
推导，其中是其中一幅图像Ik的特征位置，是另一幅图像Ik-1上对应的特征的位置。和的图像坐标是在同一坐标系一致的。
为了简化，我们将归一化的坐标记为
，（详细参考“透视相机模型”）。类似的等式，也可以通过单位圆球的归一化坐标推导，（如“圆球模型”所示）。
用对极约束计算2D到2D特征匹配的本征矩阵。最小化方案是采用5个2D到2D的对应关系，如论文75，Nister的论文76提供了一种实现方法。Nister的5点算法成为标准的2D到2D的运动估计方法，包括对离群点的处理。对于n>=8的非共面点的一种简单直接的计算方法是Longuet-Higgins的8点算法，如论文2中总结。每个特征匹配都给出一个约束：
8点法得到的这些约束进行排列可以得到一个线性方程组AE=0，通过解方程，就可以计算出E的值。这个齐次方程组可以很容易通过奇异值分解（SVD）计算，如论文2。如果超过8个点会产生超定方程组（方程个数大于未知量个数的方程组），如果超定方程组给定的条件（限制）过于严格，导致解不存在，就用最小二乘法进行拟合，求出最小二乘解，该方法对噪声具有鲁棒性。矩阵A的奇异值分解（SVD）形式为A=USVT，E的最小二乘估计为||E||=1可以用V的最后一行查找。然而，E的线性估计并不满足本征矩阵的特征约束，它是旋转矩阵R和反对称平移t的乘积。在本征矩阵的奇异值里面可以看到这些约束。经过奇异值分解（SVD）分解后的本征矩阵是E=USVT，提取主对角线上值diag(S)={s,s,0}，第一个和第二个奇异值相等，第三个是0。为了得到满足约束的有效的本征矩阵E，需要将它投影到具有有效的本征矩阵的空间中去。投影本征矩阵为
。
当3维空间中点共面时，8点算法方案退化。相应的，5点算法则可以应用于计算共面点。最后，8点算法对校准过的（透视或全景）相机和没有校准的相机都可适用，5点算法只适用于校准过的（透视或全景）相机。
4.1.2 从本征矩阵E提取旋转矩阵R和平移矩阵t
从计算估计出来的
中，可以提取旋转和平移矩阵。通常，对同一本征矩阵有4种不同的方法求解R，t；其实，只要三角化一个点，就可以求出一对R和t。这4种方法是：
一种将E有效分解成R和t的方法在论文76中有详细介绍。
通过三角化点云选择正确的方案，选择点的时候这个点要在两个相机的正前方，用估计的R，t作为初始值，可以采用非线性优化方法对旋转和平移参数进行优化。最小化重投影误差的方程用公式10定义。
捆集调整可用于优化相机轨迹的局部估计。——译者注。
4.1.3 计算相关尺度
为了重构图像序列的轨迹，需要将不同的变换T0:n进行链接。为了达到这个目标，两幅图像的平移绝对尺度无法计算，需要计算它们的相对尺度。图像子集变换的相对尺度是可以计算出来的。其中一种方法是三角化两个图像子集的一对图像集Xk-1和Xk的3维点。从对应的3维点坐标可以计算两个3维点之间的距离。计算图像对Xk-1和Xk之间的距离的比值r可以得出相应的尺度。
考虑到鲁棒性，计算了很多的比例因子，采用平均值，如果出现离群值就取中值。平移向量t也可以通过这个距离比值计算。相对尺度的计算要求在多个图像帧上的特征已经匹配好（或被追踪），至少是三幅图像帧。与采用外部三角化3维点不同的是，比例因子还可以通过寻找2维特征的三视图匹配之间的三焦点约束来计算，如《多视图几何》所示。
2D到2D对应关系的视觉里程计算法如算法1所示。
算法1： 视觉里程计，从2D到2D的对应。
1. 获取新的图像帧Ik；
2. 提取、匹配Ik-1和Ik的特征；
3. 计算图像对Ik-1和Ik的本征矩阵；
4. 将本征矩阵分解为Rk和tk，形成Tk；
5. 计算相关尺度，相应地重新调整tk；
6. 通过计算Ck=Ck-1Tk链接变换；
7. 从1）开始重复。
捆集调整可以精炼轨迹的局部估计。——译者注。
4.2 3D到3D：3D结构对应中的运动
对于3D到3D的特征对应，可以通过两组3D特征的一致变换计算相机的运动Tk。3D到3D的特征对应只适用于立体视觉中。
计算Tk的一般方法是计算两组3D特征之间的L2距离的最小值。
其中，i表示第i个特征，Xk,Xk-1表示3D点的齐次坐标，比如：
。
论文77中，最小化方案采用3组3D到3D非共线对应关系，可以用于出现离群点的鲁棒估计，将在第2部分讲解。当n>=3的对应点，可能的方法，论文78，是分别用3D特征组的不同质心计算平移部分，和用奇异值分解（SVD）计算旋转部分。平移部分通过下式计算：
其中-表示算术平均值。
旋转矩阵可以通过奇异值分解（SVD）计算，Rk=VUT，其中
，Xk-1和Xk是两组对应的3D点。
如果3D点的测量不确定性已知，可以通过论文17的方法添加权重。变换的计算具有绝对尺度，因此，图像序列的轨迹可以直接连接变换得到。
3D到3D对应关系的视觉里程计算法如算法2所示。
算法2： 视觉里程计，从3D到3D的对应。
1、获取两个图像对II,k-1，Ir,k-1和II,k，Ir,k；
2、提取、匹配II,k-1和II,k的特征；
3、对每个图像对，三角化匹配好的特征；
4、从3D特征Xk-1和Xk，计算Tk；
5、通过计算Ck=Ck-1Tk链接变换；
6、 从1）开始重复。
为了计算变换，也可以不用立体相机3D点的三角化，而采用4焦点约束。论文21讲解了这个方法。4焦点张量也可以直接从2D到2D的立体对应关系中计算变换。
4.3 3D到2D：3D结构和图像特征对应中的运动
Nister的论文1指出，3D到2D对应的运动估计比3D到3D对应的更精确，因为它的图像重投影误差更小（如论文10），而3D到3D的特征位置误差估计不是最小，如论文9。通过3D到2D的对应关系Xk-1和pk计算变换Tk。Xk-1可以用立体数据估计，或在单目视觉中，可以三角化图像量测pk-1和pk-2获得。然后，再使用三视图的图像对应关系。
计算Tk的一般方法是图像重投影误差的最小值：
其中
是3D点
根据Tk变换到图像Ik的投影。众所周知，这个问题是n点透视问题，有很多不同的方法可以用，如论文79。论文18中，最小化方法采用3个3D到2D的对应。称之为3点透视（P3P）方法，有4种方案用1个或更多点来消除歧义。（一种优化加速P3P算法如论文80所示，C代码可以在作者的网站上下载。）在3D到2D的状况下，P3P是一种用于在离群点出现的情况下鲁棒运动估计的标准方法，如论文18所示。鲁棒估计将在第2部分详细介绍。
一种简单直接的解决n>=6点的PnP问题的方法是直接线性变换算法，如论文22所示。一个3D到2D点的对应提供了Pk=[R|t]的2个约束，如下所示。
讲6个点的对应关系的约束排列产生一个线性方程组为AP=0。A中包含0向量，采用SVD奇异值分解，可以计算出P。旋转和平移部分通过Pk=[R|t]提取。生成的旋转矩阵R不需要正则化。然而，R和t都可以通过论文10中重投影误差非线性优化来处理，所以问题不大。
3D到2D的运动估计采用的2D图像点来自于单目相机。这意味着对立体相机，2D图像点要么是左边的相机要么是右边的相机生成。显然，我们希望同时使用两个相机的图像点。Nister在论文81中提出了一种对于外参校准（相机间的相对位置和方向已知）的相机的一般化的3D到2D运动估计算法，可应用于非共点的射线（比如，多相机的2D图像点）。
对于单目相机，需要三角化3D图像点，用一种独特的方法从3D到2D的匹配中估计位姿。这种独特的算法常常被称为SfM。从2个视图开始，通过2D到2D特征匹配计算3D点的初始点集和第一个变换。接下来的变换用3D到2D的特征匹配计算。为了达成这种做法，特征的匹配或跟踪，需要在多个图像帧中处理（至少是3个图像帧）。当新变换计算好后，将被加入到3D特征点集中，新的3D特征会被三角化。这种方法的挑战是需要维护一个一致的、精确的三角化的3D特征，在至少3个连续图像帧上创建3D到2D的特征匹配。
3D到2D对应的视觉里程计算法总结如算法3所示。
算法3： 视觉里程计，从3D到2D的对应。
2D到2D和3D到2D运动估计比3D到3D方法更精确。——译者注。
之前有些运动估计方法要求从2D图像对应中三角化3D点（结构）。捆集调整Bundle Adjustment也需要计算运动结构，从而计算出局部轨迹更精确的估计。
从至少2幅图像帧的2D图像对应的射线反向交汇，进行3D点三角化。理想情况下，这些射线将会在一个3D点处交汇。然而，由于图像噪声，相机模型和标定误差，还有特征匹配的不确定性，它们可能不会交汇。因此，在最小二乘情况下，最小距离的点，其所有射线交汇，可以作为3D点的位置的估计。所有射线交汇的三角化的3D点的距离的标准偏差是一种理想的3D点。具有较大不确定的3维点将会被丢弃。这种情况在图像帧非常靠近被拍摄的场景时会发生。这种情况发生时，3D点表现出非常大的不确定性。避免这种问题的方法是忽略这些图像帧，直到这些3D点的不确定性减小到一个设定的阈值。这些挑选的图像帧被称为关键帧。关键帧的选择是视觉里程计中非常重要的一步，它是在更新运动之前必须完成。
视觉里程计在一致性和实时性能方面需要做取舍。——译者注。
根据Nister论文1，对于运动的计算，与3D到3D方法相比2D到2D和3D到2D的方法具有优势。对于立体相机，Nister对比了视觉里程计的性能，3D到3D的和3D到2D的情况，发现3D到2D要比3D到3D好很多。主要原因是三角化的3D点在深度方面具有更多的不确定性。3D到3D的特征对应用于运动计算，不确定性会减少运动估计的效果。事实上，3D到3D的运动估计，3D位置误差，论文9，可以在3D到2D图像重投影误差下最小化。
在单目算法中，相对3D到2D，更推荐使用2D到2D方法，因为它避免了点云的三角化。但实际应用中，3D到2D方法使用的比2D到2D方法多。主要原因是它快速的数据关联。由于需要更精确的运动估计计算，对于输入数据排除离群点非常重要。去除离群点是非常微秒的一步，它的计算时间与估计运动的最少点数密切相关。如前所述，2D到2D的计算要求最少5点对应关系（参考5点算法）；然而，在3D到2D运动估计中只有3个对应关系（参考P3P）。点数越少，运动估计计算越快。
立体相机对比单目相机的优势，除了3D特征可以在绝对尺度下直接计算之外，特征匹配可以直接从2幅视图中计算而不是单目相机算法中的3幅视图。另外，3D结构从一对立体图像对中计算得出而不是单目相机中的联立多幅图像，在小运动中，立体算法比单目算法的漂移更小。单目方法更有意思，因为立体方法可能会退化成单目，当相机到场景点的距离远远大于立体相机间的基线（两个相机间的距离）时就会出现这种情况。这种情况下，立体相机方法无效，只能使用单目方法。
无论采用哪种运动估计算法，都需要执行局部捆集调整（前m个图像上）估计更精确的轨迹。捆集调整之后，运动估计的效果就得到非常大的缓解。
本文讨论了视觉里程计的历史，问题的推导，不同的运动估计方法。视觉里程计是机器人上必要的一部分，比较容易理解。第二部分将会总结视觉里程计的其他部分：如何检测和匹配图像间显著的可重复的特征，出现无效数据后的鲁棒估计，和捆集调整。另外，还有误差迭代，应用，和开源代码。
由于参考文献较多，本文中就不列出。
翻译的全文PDF及原文下载链接在【泡泡机器人翻译专栏】视觉里程计——第二部分：匹配、鲁棒、优化和应用(下)中给出，敬请关注。
长按下图，识别图中二维码，关注“泡泡机器人SLAM”（ID：paopaorobot_slam）公众号，我们将会持续推出这个翻译系列的文章。翻译的全文PDF及原文下载链接在【泡泡机器人翻译专栏】视觉里程计——第二部分：匹配、鲁棒、优化和应用(下)中给出，敬请关注。
【版权声明】泡泡机器人SLAM的所有文章全部由泡泡机器人的成员花费大量心血制作而成的原创内容，希望大家珍惜我们的劳动成果，转载请务必注明出自【泡泡机器人SLAM】微信公众号，否则侵权必究！同时，我们也欢迎各位转载到自己的朋友圈，让更多的人能进入到SLAM这个领域中，让我们共同为推进中国的SLAM事业而努力！
【注】商业转载请联系刘富强（[email protected]）进行授权。普通个人转载，请保留版权声明，并且在文章下方放上“泡泡机器人SLAM”微信公众账号的二维码即可。
