SVO: Fast Semi-Direct Monocular Visual Odometry

论文翻译
论文讲解1
论文讲解2

一、符号规定

在论文的第三部分，为了方便后面的叙述，将用到的大部分符号都做了一定的规定。
首先，论文采用的是图片的灰度值，所以将第k时刻的图片的灰度值记为Ik，这个Ik可以看作是一张灰度图，图是在k时刻拍摄的，如果要查找具体点的灰度值，就要在后面加坐标。坐标分为环境中的3d坐标和像素坐标系中的2d坐标，所有的3d坐标都使用p来表示，而2d坐标使用u来表示，二者之间利用针孔相机的成像模型来表示：

这里k并不是常数，而是一个前下标，表示坐标系，如果写的是k就表示k时刻的相机坐标系，后面也有写w的，这表示在世界坐标系下，这种写法属实有些奇怪，一般都是用后下标，很少有这种写法。所以这是利用3d点转换为2d点，翻过来等式也是一样成立的：

2d还原到3d就比前面多一个参数-深度，这里用du来表示u这个特征点的深度信息，利用逆投影函数，得到k时刻相机坐标系3d点p的坐标。
此外，相机在k时刻的位姿用刚体变换的变换矩阵来描述，记作Tkw，两个下标后者表示源坐标系前者表示当前坐标系，于是就有：

在运动估计时使用到了扰动模型，这里使用了李代数中的变换向量，写法如下：

二、运动估计-A基于稀疏模型的图像对齐

运动估计的部分是这篇论文最主要的部分，里面分了三部分来进行，首先是最能体现半直接思路的图像对齐。对于一般的直接法，主要的思路是通过最小化光度误差，来找出一个最优的帧间变换矩阵，即：

这个式子虽然使用的是积分的形式，但是本质上和求和没有区别，这个式子中ρ表示的是求平方然后除以二的函数，即：

而其中的残差部分表示的是光度残差，即：

光度残差的概念可以在slam十四讲中第八讲中找到，对于这个式子，它的意思是说一个对于一个特征u，首先这个特征必须是k-1时刻的特征，其次这个特征根据变换转换到k时刻，计算出来的位置必须要在k时刻图像的范围内。对于符合这样要求的特征u，利用深度信息和反投影函数得到3d点，之后利用变换矩阵T转换到k时刻，再通过投影函数π转换为像素坐标系中的点，然后得到这个点上的灰度值，与投影前灰度值相减，得到的就是光度误差。
这种是一般的直接法使用的光度误差，而在SVO中，由于使用的是半直接法，所以光度残差也会发生一定的变化。首先，这里使用了一个小的区块（patch）作为光度残差比较的内容，在开始的翻译的链接里面这个区块被翻译成了面片，属实有些出戏。这里选取了一个特征点附近4×4像素的小区块，关于光度残差的计算就是基于这个小区块，这里同样是利用u来表示特征，只不过这里u表示的是区块展开成的向量，在这种情况下，问题描述转变为：

也就是说我们要通过最小化帧间的区块光度残差，去寻找一个最优的帧间变换矩阵Tkk-1。相比于一般的直接法，这里将检查点的灰度值换成了检查区块的灰度值，但本质上还是用的直接法那一套。
问题确实是看着简单，但是在这里问题的优化过程上出现了一种比较奇怪的过程。对于上面待优化的式子，其中对变换矩阵是非线性的，所以残差的计算应该采用增量更新的方法，也就是先做一个假设的初始变换矩阵，通过不断增加扰动，来优化这个变换矩阵，最后得到最优值。这里的扰动就需要使用四元数的变换向量来表示。

也就是说通过增加对上一个时刻的扰动，不断优化残差，再将残差加在自己的初始的变换矩阵上，不断优化变换矩阵：

这个过程图示为：

具体的推导论文里没有，完全体现在代码里，这里参考前面的论文讲解2的链接。
开始之前笼统地扣个帽子，整个过程利用两帧图像、前一帧图像的特征点以及特征点的深度信息，求两帧之间的变换矩阵。这个过程的思路是先利用深度信息，沿着2d点在针孔相机模型成像的线，找到对应的3d点，以此作为空间点的位置，之后投影到当前帧，通过对上一帧进行扰动，不断优化两帧之间的变换矩阵。
对于一般的直接法优化，都是固定上一帧，不断调整变换矩阵，从而改变在当前帧上的投影位置，看这两个位置的光度误差从而调整变换矩阵。但是这里并不是这样做的，而是固定当前帧去修改上一帧。
如果这里使用一般的直接法，优化的残差应该为：

这些式子来自那个关于代码的论文讲解，使用的字母有些和论文里不一样，这里也只能猜着进行，这里patch感觉应该就是投影到当前帧的区块的灰度值。按照增加扰动的理论，增加左扰动之后变为：

所以残差关于扰动的雅可比矩阵就变为：

对于这个式子，采用链式求导法则会发现，每次迭代都会让雅可比矩阵发生变化，从而每次迭代都需要重新计算一下雅可比矩阵。而论文中采取的反向的方法就不会出现这种现象。

这个式子表示的是论文中的残差，可以看出，被减数部分是投影过来的区块的灰度值，而减数的部分则是投影前的区块的灰度值，为了后续的展开，这里加了个相同时刻的变换矩阵，其实加不加都一样。这里将上一帧称为参考帧，我们给参考帧的位姿增加一个扰动，变换到（k-1）‘，于是有：

所以残差与扰动的关系可以表示为：

对于这个残差的式子，可以看出里面有一个逆矩阵，这个矩阵在求导的时候会很不方便，所以这里做了一个代换：

在计算过程中替代逆矩阵，最后再翻过来，从而大大简化求雅可比的过程，这种方法也叫做残差的逆合成公式。代换后式子变为：

到这一步就可以看出，论文中的计算过程省略了很多，用这个式子再求雅可比，或者说求导，利用链式求导法则，可以展开成下面的式子：

现在对于求导的式子，可以看出所有的内容在更新的过程都不需要重新计算，所以开始计算一次即可。利用高斯牛顿法算出扰动：

通过这种方式，解出变换向量，从而更新出最优的扰动，将扰动应用到参考帧上，从而相当于优化了变换矩阵。

上面这一大块都是利用给参考帧增加扰动的方法进行优化的过程，那么问题就是为什么要采用这种扰动方式，或者说这种扰动的合理性在哪里。以下为个人理解，上面的解释是用扰动，让位姿移动到另一个位置：

这个图是博客里的配图，a为原始位姿，利用这个位姿的信息恢复出空间点p，之后投影到b，之后给a一个扰动，解出最优的扰动，让a变换到a’位置，再在这个位置投影出新的空间点p’，重复这个过程，最后优化出变换矩阵。这种思路是用变换位姿的思路去解释的，但是对于优化的式子，个人感觉利用改变位置去解释更容易理解。

这个是关于扰动的残差的式子，由于优化过程中保证空间点不变，所以这里直接就都写成了p，而没有用反投影函数去写，这里并不影响，博客里的解释是说，增加一个扰动，让k-1变换到（k-1）‘，相当于变换了坐标系，个人感觉这种操作实际上就是改变了投影位置和初始位置，我们目前就是有两张图，下面这一部分其实就是更换了像素坐标系上的位置：

原来没有扰动的时候，位置就是pk-1在图中的位置，扰动之后，更换了位置，而固定了当前帧的位置。也就是说，一般的直接法是固定上一帧，不断修改当前帧的投影位置，看匹配程度如何，从而优化变换矩阵进而修改投影位置，但是这里是固定当前帧，不断给参考帧的源位置来增加变换，看匹配程度，虽然看着有些怪，但是确实不断修改了两帧之间的变换矩阵，向着更加优化的方向进行。

在参考帧上增加扰动，相当于在参考帧上不断改变原来的投影位置，左边这一组是正向投影的过程，可以看出参考帧和当前帧上西片的脸部位置不是完全重合的，是存在一定的偏差的，也就是光度误差，这时固定当前帧的投影位置和现实中的西片的位置，给参考帧增加，相当于改变了参考帧上的位置，也就是右边这组中的很多个红色框，最后找到一个光度误差最小的红色框，此时计算出的变换矩阵就是最优化的。

回到论文本身，得到帧间变换矩阵，就可以利用变换矩阵得到当前帧和世界坐标系的变换关系：

经过这一步，我们得到了帧间变换矩阵，通过帧间变换矩阵的延伸，可以得到当前帧在世界坐标系下的坐标。

三、运动估计-B通过特征对齐松弛

最难理解的一步已经过去了，后面的部分相对都简单不少。上一步中得到的帧间变换矩阵相当于简单定义了一个3d点的特征位置的初步猜测，但是在这个过程中存在累积误差，所以会导致偏移，所以相机的位姿应该与地图对齐，而不是应该与上一帧对齐，所以需要一步对位姿的优化。

根据我们上一步的估计，在这里已经可以得到一个大概的投影位置，这个位置时根据帧间变换矩阵得到的投影位置，这个位姿由于误差的存在导致投影位置和真实位置之间存在偏差，也就是图中蓝色的位置，而灰色代表真实的位置，现在我们就是要优化这个位置。
这一步优化使用的是地图中的三维空间点，在前面的过程中，有一些特征点其实已经被保存了下来，对于一些新来的帧，其实里面的点可能是之前地图中保存下来的点，由于我们已经在上一步中求出了帧间变换矩阵，所以前面保存的点也可以通过不断投影，最终投影到当前帧上，这一步其实就是利用这个约束，去优化投影位置，为后面的进一步优化做铺垫。
依然是利用光度误差，但是这里使用的参考帧已经变了，不再是上一步利用参考帧，而是利用前面的一个关键帧进行优化，具体的优化式子如下：

在这一步中，优化的目标是投影位置，最小化当前帧上投影位置的灰度值和投影前的灰度值，这里投影前指的并不是上一帧，而是根据投影的3d点追溯到的这个3d点所在的关键帧，所以这里使用的是Ir，此外，由于关键帧和当前帧之间的距离可能很远，所以在这里还加了一个仿射变换，也就是对关键帧中的特征区块进行拉伸之类的仿射变换才进行比较。针对距离可能比较远这个问题，论文中还将区块扩大到8×8像素的大小，通过这种方式来避免距离远造成的影响。其实这一步有些想光流法中找特征点，只不过光流法是在找最优的帧间变换，而这里只找了最优位置。

通过这种方式，我们在忽略对极几何的情况下利用前面求出变换矩阵，找出了前面的关键帧上的特征在当前帧上的尽可能准确的位置，这个相对准确的位置会用在后面的优化中。

四、运动估计-C姿态和结构优化

这一步就是利用上一步中的相对准确的位置进行位姿的优化。再第一步中，我们是利用帧间的光度误差，找出了帧间变换矩阵，进而得到了相机在世界坐标系下的位姿，而第二步中，我们又利用帧间关系，找出了前面关键帧上特征在当前帧中一个比较准确的位置，下面就利用这个位置对世界坐标系下的位姿做优化。

在这里使用的就是重投影误差了，通过最小化真实点通过投影到相机平面与真实的差距，来得到一个世界坐标系下的最优位姿。这样我们在世界坐标系下的位姿就准确了。

关于运动估计这一部分，一共是三步，第一步利用最小化帧间特征区块的光度误差，得到一个存在误差的帧间变换矩阵，延伸计算出当前帧在世界坐标系下的位姿；第二步，利用地图中的3d点信息，找出一个在当前帧上比较准确的投影位置；最后第三步，利用最小化特征区块的重投影误差，也就是最小化第二步校正的位置和根据位姿投影的位置的误差，从而校正在世界坐标系下的位姿。

也就是说，第一步只是为了给出一个基础的位姿，第二步根据这个基础的位姿，在投影位置附近找到真正准确的位置，第三步则根据这个相对准确的投影位置，翻过来更新相机的位姿。在论文中还有一个第四部分，这一部分补充了一下作者关于采用这三步的理由，单独使用二三步其实也可以，但是运算花费的时间会更长，因为没有一个比较准确的初始值，只用第一步则会导致累积误差太大。

五、制图

制图这部分主要是利用了一个混合的概率模型。在这部分中，论文主要讲的就是一个事情-深度估计。这个事情其实是直接法里面一个和特征点法区别比较大的地方，因为特征点法中两帧之间的特征对应关系是已经确定的，所以可以直接用对角化去估计深度，但是直接法中对下一帧中特征的位置是一个估计值，所以并不能直接用三角化，在slam十四讲中讲到过一个不确定性分析，就是针对直接法中的深度估计的。
在论文中，除了利用极线搜索去进行三角化，还引入了一个类似卡尔曼滤波的贝叶斯概率模型去进行数据的融合。

关于极线搜索三角化的内容就不再赘述了，在slam十四讲里面都有，这里主要介绍一下论文使用的贝叶斯概率模型。首先，对同一个特征点在不同帧上做三角化可以得到多个深度，深度有可能不一样，但是总是满足一定的分布，论文中直接使用了另一篇论文中的假设：

对于这个模型，ρ表示一个权重，对应正确估计的可能性，那么翻过来，1-ρ表示的就是不正确估计的可能性。这个模型假设正确测量符合正态分布，而不正确的测量则符合一个在深度最小值到深度最大值之间的均匀分布，这个均匀分布的意义是假设会有一定的概率出现错误的深度估计值。因为使用的是另一篇论文中的内容，所以对于这个模型论文也没有做过多的解释。

在制图的过程中，每个对特征点的后续的观察都会被用于更新这个分布，当分布的方差变得足够小的时候，我们就认为深度估计收敛，这时候恢复为3d点，同时将这个点用于前面的运动估计中。

此外，每个分布都是和一个关键帧关联的，也就是说初始化的时候都是需要在关键帧的时候，如果是个关键帧，就对其中的特征进行提取，平均深度设置为参考帧中的平均场景深度，之后通过非关键帧不断进行更新。

这张图展示了这个算法的过程，这个算法分为运动估计和制图两个线程，运动估计中分为三步，对于一个新来的帧，利用上一帧和当前帧先计算出帧间变换，延伸计算出世界坐标系下的位姿，之后用地图信息找出在当前帧上的准确的投影位置，利用重投影误差校准世界坐标系下的位姿，之后进入制图线程的帧序列中，判断是否为关键帧，如果是关键帧就初始化一个深度滤波器，如果不是关键帧就只用于更新当前的深度滤波器，收敛的深度滤波器的特征点就固定下来作为地图信息，为运动估计中的第二步再提供帮助。

这张图是算法深度估计的效果图，可见只需要很小范围的运动就可以让深度估计达到一个比较准确的程度。

六、实现细节

在这一部分中论文主要是对两点内容做了补充，一个是为了处理更大的运动，算法采用了由粗到细的稀疏图像对齐算法，这种方法貌似就是图像金字塔。然后在最粗级别上优化强度残差，直到收敛。随后，在下一个更精细的级别初始化优化。为了节省处理时间，我们在第三级收敛后停止，此时估计值足够精确，可以初始化特征对齐。
此外，为了提高效率，算法采用了类似窗口的思想，维护一定数量的关键帧，如果新帧相对于所有关键帧的欧式距离超过平均场景深度的12%，就被选为关键帧。在地图中插入新的关键帧时，将移除距离相机当前位置最远的关键帧。

除了论文中提到的部分，其实初始化有关的操作在这里都没有提到，从算法的流程可以看出，运动估计中需要使用地图中存储下来的深度信息，但是这个深度信息是在制图的时候通过关键帧一点一点积累出来的，而关键帧的选取需要和前一帧比较深度变化，也就是说没有上一帧就无法判断这一帧，这就意味着对于这个算法本身而言是不能进行初始化的，初始化需要用别的方法。
关于初始化，体现在代码里的流程为：处理第一张图像，检测FAST特征点和边缘特征，如果图像中间的特征点数量超过50个，就把这张图像作为第一个关键帧。然后处理第一张之后的连续图像，跟第一个关键帧进行三角化，之后按照算法里的流程进行深度滤波器的更新。