本文介绍一种单目视觉里程计的方法，重点说明当前帧的半稠密深度图估计过程，估计到的半稠密深度图反过来又可以用于跟踪当前相机位姿状态。这里只考虑估计图像中梯度足够大的像素，对每一个像素估计的深度都用一个高斯概率分布来表示（基于一个先验假设，所有像素的逆深度服从高斯分布），当新的图像到来后，我们需要将估计到的信息传递到新的一帧，并且结合新的图像更新这个估计值。
对应的文章见参考文献[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的真值位置，由于高斯噪声的存在，这个位置被确定为上方直线左边点的位置，由此造成了极线的位置被确定为上方直线（下方直线才是其真值），在极线上搜索与下方黑点梯度相同的位置即为视差匹配点。由图可知，当极线方向越接近梯度方向时，误差越小，当极线方向越垂直梯度方向时，误差越大。
我们假定等梯度线在局部是线性的，即局部的梯度方向是一个常量，可以给出下式：
其中g=（gx，gy）是梯度方向，g0表示等梯度线上的一点。将上式l0移到等式右边，两边同乘g，最后将左边系数移到右边，可以得到下式：
根据方差传递公式，可以得到：
g表示图像归一化梯度方向，l表示归一化极线方向，sigma_l是误差epsilon_l的方差，由此可以看出，几何误差项只取决于ξ和π。
2.1.3.2光度误差
我们同过最小化图像亮度差的方式寻找视差匹配，如下式：
其中i_ref是参考帧图像的强度，I_p是在极线上视差为λ对应位置的强度，我们假定已经有一个良好初始化的λ0存在，在I_p位置使用一阶泰勒近似展开，得到下式：
其中g_p表示I_p沿着极线方向的梯度。使用方差传递公式可以得到光度误差的不确定度：
sigma_i是图像亮度噪声的方差。
如图2所示，图像沿极线方向梯度越大，光度视差误差越小，梯度越小，则光度误差视差越大。
2.1.3.3像素到逆深度的转换
对于微小旋转的情况，逆深度值近似与视差成比例，逆深度的测量方差可以如下式计算：
这个比例系数α在图像没有对齐的情况下，每个像素的值通常是不同的，其取决于平移方向和像素在图像上的位置，因此我们通过下式来计算：
其中，分子是搜索逆深度值的间隔的长度，分母是搜索的极线段的长度。
2.1.4测量深度值融合
获得了当前帧的深度测量值之后，我们就要将其融合的深度图中，如果没有对应的先验假设，我们就将其直接合并进深度图中，如果存在先验深度，我们就需要将观测值与先验值合并起来更新深度图，对应的更新公式如下：
2.1.对不确定度的总结
- 光度误差：取决于图像沿极线方向的梯度大小
- 几何误差：取决于图像方向与极线方向的夹角
- 像素到逆深度比例：取决于相机平移、焦距和像素位置。
2.2深度图传播
深度图的传播指的是根据估计得到的对应像素的逆深度d0，计算其3D位置，再将其投影到新的一帧中，从而得到新的逆深度d1的过程。
我们假设相机旋转很小，d1可以如下计算：
tz是相机沿着光轴方向的平移，d1对应的方差可以如下给定
sigma_p表示预测不确定度。
如果两个深度值传递给了同一个像素，我们会采用如下两种方式处理：
- 如果它们是相似的，即小于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.
