里程表数据算法（深度视觉里程计---推导公式来了）

作者：Taylor Guo 博客：http://www.qiqitek.com/vo.html

视觉里程计问题的公式推导

假设在时间k，通过相机移动，追踪环境图像。如果是单目相机，k时间上拍摄的一组图像表示为I0:n={I0，…，In}。如果是立体相机，每个时刻有左右两幅图像，记为Il, 0:n={Il,0，…，Il,n}和Ir, 0:n={Ir,0，…，Ir,n}。图1表示了这个具体问题。

相邻的相机位置的相对位姿Tk,K-1（或相机系统位置），通过视觉特征进行计算，将位姿连起来获得关于初始坐标帧k=0时的绝对位姿Ck。 假设相机的图像帧坐标为机器人的坐标。立体相机，为不失一般性，左眼的坐标为原点。在连续时刻k-1和k，相机位置关系通过变换Tk，k-1єR4x4记为：

其中Rk，k-1єSO(3)是旋转矩阵，tk，k-1єR3x1是平移向量。这组向量T1:n={T,.0，…，Tn,n-1}包括连续的相机运动。为了简便，我们将Tk，k-1记为Tk。最后，相机位姿C0:n={C0, … , Cn}包含了相机相对初始坐标k=0的变换。当前位姿Cn可以通过联立变换Tk（k=1…n）计算，因此，Cn=Cn-1Tn，其中C0是相机k=0时的位姿，用户可以任意指定。 视觉里程计的主要任务就是计算图像Ik到Ik-1的相对变换，然后联立变换重建相机的C0:n的轨迹。这意味着视觉里程计一个位姿一个位姿的增量式地构建轨迹。这个步骤之后，前m个位姿迭代优化可以获得局部轨迹更精确的估计。这个迭代优化的过程，是计算重构前m图像3D云点重射影误差开方的和的最小化值（称为窗口化捆集调整，因为它在m帧图像上的子窗口执行。捆集调整将在第二部分详细介绍）。三角化图像上的点获得3D云点（参考“三角化和关键帧的选择”）。

单目视觉里程计有两种方法计算相对运动Tk：基于场景外观的（或全局的）方法，采用两个输入图像的所有像素的稠密信息；基于特征的方法，只采用从图像中提取（追踪）的显著的、可重复的特征。全局方法的精度比基于特征的方法低，计算复杂度更高一些。（在之前“视觉里程计的历史”章节中，大部分基于场景外观的方法应用于单面视觉里程计中。主要是由于相比立体相机中更容易执行。）基于特征的方法要求对图像帧的特征提取（追踪）具有鲁棒性，它比全局方法更快、更精确。因此，大部分视觉里程计的执行都采用基于特征的方法。

视觉里程计的主要流程方法如图2 所示。对每个新的图像Ik（如果是立体相机，就是图像对），前两步包括检测和匹配2D特征，是通过与之前的图像帧做对比而进行的。从不同的图像帧上提取的3D特征是2维特征的重投影，这是图像的对应关系。（我们将在第二部分详细讲解特征匹配和特征跟踪的区别。第一个特征匹配是在图像中单独检测特征，然后基于相似的特点再将它们进行匹配； 第二个特征跟踪是先查找一幅图像中的特征，然后用局部搜索方法，比如关联程度，在下一幅图像中跟踪它们。）第三步是计算两个时刻间k-1和k的相对运动Tk。根据对应关系是3维或2维的不同，有3种不同的方法处理这个问题（如“运动估计”章节所示）。相机位姿Ck就可以通过联立位姿Tk和之前的位姿计算获得。最后，通过对前m帧图像进行迭代优化（捆集调整）可以更精确地估计局部轨迹。

本文对运动估计也有详细说明（参考“运动估计”章节）。特征检测和匹配，捆集调整将在第二部分详细说明。同样的，一个精确地运动计算，特征对应不应该包含离群点（比如错误的数据关联）。即使出现离群点也要确保精确的运动估计，是鲁棒估计的主要任务，也将在第二部分详细说明。大部分视觉里程计的执行都假设相机已经校准过。我们将浏览一下透视相机或全景相机的标准模型和校准过程。

相机模型和校准

视觉里程计可以采用透视相机和全景相机。我们将在本章主要讨论一下相机的模型。

1透视图相机模型

透视相机常用模型是针孔射影系统：物体反射的光线通过透镜中心（投影中心）在焦点平面形成图像，如图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 viewpoint)，即在视觉系统中只存在一个投影中心。其主要原因是单视点可从全方位摄像头采集的图像中产生正确的几何透视图像，同时还适用于成熟的外极线几何理论。反折射相机中，当光线通过镜面相交于一个点C时，才会形成投影中心。如果存在这个点，可以使我们能够从一个单视点到圆球通过对应关系构建全景投影模型。为了方便，通常会采用单位球体。

值得注意的是球体模型不仅可以应用于全景相机中，还可以应用于透视相机。如果相机校准过，透视图像或全景图像上的任何点就可以映射到单位球体上的一个向量。如图3（C）所示，单位向量指向观察到的图像场景中的点。这些向量是单位球体上归一化图像点。

相机校准

相机校准的目的是为了获得相机系统更精确的内部参数和外部参数。在多相机系统中（比如立体或三相机），外部参数描述了每个相机对的彼此位置关系和方向信息。最常用的模型是采用平面棋盘模型。棋盘上方块的位置已知。为了精确地计算校准参数，用户需要在不同的位置和方向拍几张棋盘的图片，拍的时候要确保相机视角尽可能地覆盖场景。相机的内部参数和外部参数可以通过最小二乘法计算。输入数据是棋盘方格的四角的2D位置和它们对应的每幅图像的像素坐标。

许多相机校准工具都有MATLAB和C版本。最新的可以在论文68中找到。在这些工具中，最常用的是论文69中的Matlab和论文70到72的透视和全景相机。透视相机的C语言校准版本OpenCV，论文73中有提供，一个开源计算机视觉库。

运动估计

运动估计在视觉里程计中对每幅图像来说是一个核心步骤。精确的说，是计算运动估计中当前图像和之前的图像间的相机运动。将这些单次运动链接起来，就可以重构相机的运动轨迹。本章详细解释如何从时刻K-1和K的两组对应的特征fk-1和fk中计算两幅图像Ik-1和Ik的变换Tk。根据特征对应关系是2维还是3维，有3中不同的方法：

• 2D到2D：fk-1和fk都是2维图像坐标；

• 3D到3D：fk-1和fk都是3维图像坐标。需要三角化每个时刻的3维云点，比如使用立体相机；

• 3D到2D：fk-1是3维，fk是它们对应的图像上的2D重投影。单目相机情况下，3D结构需要从2个相邻的相机视图（比如Ik-2和Ik-1）进行三角化，然后和第3幅视图中的2D图像特征进行匹配（比如，Ik）。在单目算法中，需要匹配至少3幅视图。

注意，特征可能是点或者线。通常，在无结构的场景中由于缺乏直线，就在视觉里程计中使用点特征。一个深入分析这三种点和线特征方法的论文，在参考论文74中可见。本文只讲解点特征的推导。

2D到2D：图像特征匹配中的运动

估计本征矩阵

校准过的相机图像Ik和Ik-1之间的几何关系可以通过本征矩阵E表示。E中包含了相机运动参数具有一个未知的平移变换因子：

符号 表示等式右边是标量乘法。

2D到2D的特征对应关系可以计算本征矩阵，旋转和平移矩阵可以通过本征矩阵进行分解计算。2D到2D的运动估计最重要的特征是对极约束，它形成一条直线连接两个对应的特征点p’和p，p是另一幅图像像对应的特征点，如图4所示。

这个约束可以通过等式p’TEp=0推导，其中p’是其中一幅图像Ik的特征位置，p是另一幅图像Ik-1上对应的特征的位置。p’和p的图像坐标是在同一坐标系一致的。为了简化，我们将归一化的坐标记为p=[u,v,1]T，（详细参考“透视相机模型”）。类似的等式，也可以通过单位圆球的归一化坐标推导，（如“圆球模型”所示）。

用对极约束计算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点算法只适用于校准过的（透视或全景）相机。

从本征矩阵E提取旋转矩阵R和平移矩阵t

从计算估计出来的E中，可以提取旋转和平移矩阵。通常，对同一本征矩阵有4种不同的方法求解R，t；其实，只要三角化一个点，就可以求出一对R和t。这4种方法是：

一种将E有效分解成R和t的方法在论文76中有详细介绍。

通过三角化云点旋转正确的方案，选择点的时候这个点要在两个相机的正前方，用估计的R，t作为初始值，可以采用非线性优化方法对旋转和平移参数进行优化。最小化重投影误差的方程用公式10定义。

捆集调整可用于优化相机轨迹的局部估计。

计算相关尺度

为了重构图像序列的轨迹，需要将不同的变换T0:n进行链接。为了达到这个目标，两幅图像的平移绝对尺度无法计算，需要计算它们的相对尺度。图像子集变换的相对尺度是可以计算出来的。其中一种方法是三角化两个图像子集的一对图像集Xk-1和Xk的3维点。从对应的3维点坐标可以计算两个3维点之间的距离。计算图像对Xk-1和Xk之间的距离的比值r可以得出相应的尺度。

考虑到鲁棒性，计算了很多的比例因子，采用平均值，如果出现离群值就取中值。平移向量t也可以通过这个距离比值计算。相对尺度的计算要求在多个图像帧上的特征已经匹配好（或被追踪），至少是三幅图像帧。与采用外部三角化3维点不同的是，比例因子还可以通过寻找2维特征的三视图匹配之间的三焦点约束来计算，如论文参考22《多视图几何》所示。

2D到2D对应关系的视觉里程计算法如算法1所示。

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所示。

为了计算变换，也可以不用立体相机3D点的三角化，而采用4焦点约束。论文21讲解了这个方法。4焦点张量也可以直接从2D到2D的立体对应关系中计算变换。

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个约束，如下所示。

其中，每个PjT都是一个4维向量（j表示PK的第j行），x,y,z是3D点Xk-1的三个坐标值。 讲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所示。

三角化和关键帧的旋转

之前有些运动估计方法要求从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）。第2部分中也提到了，越少的云点，运动估计的速度越快。

立体相机的算法与单目相比，除3D特征可以在绝对尺度下直接计算外，它的优势是只需要计算两幅图像之间的匹配而不是单目中的三幅图像。另外，3D结构可以通过单一的立体图像对计算，而不是单目中连续多个图像帧，立体算法比单目算法在较小的运动上具有更小的漂移。当相机到场景之间的距离远远大于立体相机的基线时，立体视觉里程计就退化成单目相机。在这种情况下，立体视觉没有效率，就会采用单目方法。

除运动计算方法的选择之外，局部捆集调整（前m个图像帧）用于计算更精确的轨迹估计。捆集调整后，运动估计方法的效果会打折扣。

结论

本文讨论了视觉里程计的历史，问题的推导，不同的运动估计方法。视觉里程计是机器人上必要的一部分，比较容易理解。第二部分将会总结视觉里程计的其他部分：如何检测和匹配图像间显著的可重复的特征，出现无效数据后的鲁棒估计，和捆集调整。另外，还有误差迭代，应用，和开源代码。

转载请注明转自AR酱

并附原文链接

,