本文介绍一种单目视觉里程计的方法，重点说明当前帧的半稠密深度图估计过程，估计到的半稠密深度图反过来又可以用于跟踪当前相机位姿状态。这里只考虑估计图像中梯度足够大的像素，对每一个像素估计的深度都用一个高斯概率分布来表示（基于一个先验假设，所有像素的逆深度服从高斯分布），当新的图像到来后，我们需要将估计到的信息传递到新的一帧，并且结合新的图像更新这个估计值。
对应的文章见参考文献[1]。

本文分为三个部分

• 对本文视觉里程计总览
• 半稠密深度图的建图方法（重点说明）
• 对新帧的跟踪

一、系统总览

系统主要分为两个线程：跟踪（使用最近的深度图跟踪新帧的位姿状态）和深度估计（连续地将逆深度值传递到下一帧，并使用立体视觉比较估计新帧的逆深度值，最后再使用正则化去除局外点）。
初始两帧的位姿使用基于关键点的方法得到，这个位姿被当作已知用于后续的深度估计。实验表明这个初始的位姿即使是非常不精确的，后续的方法也可以恢复出一个精确的深度图。

二、半稠密深度图估计

深度图的估计分为三个主要的部分：

• 使用立体视觉的方法得到测量值，并且将其与先验深度相合并
• 深度图在帧与帧之间的传播
• 正则化和局外点的去除

2.1基于立体视觉的深度更新

整体的深度图更新步骤如下：

• 提取视差搜索精度足够大的像素
• 对每一个选取的像素，单独为其选择合适的参考帧，并执行一个一维的视差搜索
• 传递的先验被用来减小视差搜索范围、降低计算复杂度、消除错误极小值
• 将获得的深度估计融合到深度图中

2.1.1参考帧的选取

理想情况下，参考帧的选取需要满足最大化立体精度，同时保证视差搜索范围和观察视角足够小。选择参考帧的策略是：包含该像素的最早的一帧图像，同时要满足视差搜索范围和观察视角的条件，否则就向后延一帧。

2.1.2立体匹配的方法

在参考帧沿着极线方向穷尽搜索对应的像素强度，并且对匹配视差进行亚像素精度定位。如果先验的逆深度假设存在，则搜索的范围就限制在d+-2sigma_d的范围，这里的d和sigma_d表示先验假设中的均值和方差。如果没有先验，则要搜索整条极线。

2.1.3不确定度的估计

本小节主要说明如何推导逆深度值d的方差。
我们可以把最优的逆深度值作为输入参数的函数：

则其方差可以如下给定：

• 在参考帧上计算极线
• 确定最佳视差匹配位置λ
• 根据视差λ计算逆深度值

前两个步骤涉及到两个误差：几何误差（第一步，来自于ξ和π）、光度误差（第二步，来自于图像噪声）

2.1.3.1几何误差

我们将极线定义如下

这里的λ对应的是视差搜索间隔，（lx，ly）是归一化极线方向，l0是对应无限深度的点。
我们现在考虑l0位置对视差误差的影响，这个位置只受影响于各向同性的高斯噪声εl，如图1所示，图中下方直线上的点表示l0的真值位置，由于高斯噪声的存在，这个位置被确定为上方直线左边点的位置，由此造成了极线的位置被确定为上方直线（下方直线才是其真值），在极线上搜索与下方黑点梯度相同的位置即为视差匹配点。由图可知，当极线方向越接近梯度方向时，误差越小，当极线方向越垂直梯度方向时，误差越大。

图1.几何误差。黑色虚线表示等梯度线

我们假定等梯度线在局部是线性的，即局部的梯度方向是一个常量，可以给出下式：

g表示图像归一化梯度方向，l表示归一化极线方向，sigma_l是误差epsilon_l的方差，由此可以看出，几何误差项只取决于ξ和π。

2.1.3.2光度误差

我们同过最小化图像亮度差的方式寻找视差匹配，如下式：

其中i_ref是参考帧图像的强度，I_p是在极线上视差为λ对应位置的强度，我们假定已经有一个良好初始化的λ0存在，在I_p位置使用一阶泰勒近似展开，得到下式：

sigma_i是图像亮度噪声的方差。
如图2所示，图像沿极线方向梯度越大，光度视差误差越小，梯度越小，则光度误差视差越大。

图2.光度误差

2.1.3.3像素到逆深度的转换

对于微小旋转的情况，逆深度值近似与视差成比例，逆深度的测量方差可以如下式计算：

这个比例系数α在图像没有对齐的情况下，每个像素的值通常是不同的，其取决于平移方向和像素在图像上的位置，因此我们通过下式来计算：

2.1.4测量深度值融合

获得了当前帧的深度测量值之后，我们就要将其融合的深度图中，如果没有对应的先验假设，我们就将其直接合并进深度图中，如果存在先验深度，我们就需要将观测值与先验值合并起来更新深度图，对应的更新公式如下：

2.1.对不确定度的总结

• 光度误差：取决于图像沿极线方向的梯度大小
• 几何误差：取决于图像方向与极线方向的夹角
• 像素到逆深度比例：取决于相机平移、焦距和像素位置。

2.2深度图传播

深度图的传播指的是根据估计得到的对应像素的逆深度d0，计算其3D位置，再将其投影到新的一帧中，从而得到新的逆深度d1的过程。
我们假设相机旋转很小，d1可以如下计算：

tz是相机沿着光轴方向的平移，d1对应的方差可以如下给定

• 如果它们是相似的，即小于2倍sigma，就将其合并起来（使用深度值融合公式）
• 否则，离相机较远的点被去除，因为其被视为遮挡的点。

2.3深度图的正则化

我们使用邻域像素逆深度的加权平均作为该点的逆深度值，权重即为各像素对应的方差的倒数。为了保留边缘，逆深度的差大于2倍sigma的邻域像素不参与平均。在正则化过程中，每个像素对应的方差不发生变化，我们在定义立体搜索范围时，使用邻域中最小的方差，并将其作为tracking线程中的权重因子。

2.3.1局外点（outlier）移除

在传递过程中，我们将每个逆深度假设的有效性（即其为局外点或变无效的概率）作为其是否为局外点的标准，如果一个像素的亮度发生显著变化，或者其图像梯度下降到一定的阈值，立体匹配就是失败的，其无效的概率就会上升。当概率大于某一阈值时，就将其移除，同样地，当其小于一定的阈值时，就要利用其邻域像素，将该像素逆深度值创建出来。

三、直接法跟踪相机位姿

该部分可参考我的博客直接法估计相机位姿。在此不做详细的说明。

参考文献
[1] Engel J, Sturm J, Cremers D. Semi-dense visual odometry for a monocular camera[C]//Proceedings of the IEEE international conference on computer vision. 2013: 1449-1456.