视觉里程计 matlab实现,深度 | 视觉里程计---推导公式来了

原标题：深度 | 视觉里程计---推导公式来了

作者：Taylor Guo

博客：

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

假设在时间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,