转载文章：SVO详细解读

说明

本篇转载至博主"极品巧克力"在博客园中的一篇文章《SVO详细解读》，参考链接为：
https://www.cnblogs.com/ilekoaiq/p/8659631.html

前言

接上一篇文章《深度滤波器详细解读》。

SVO（Semi-Direct Monocular Visual Odometry）是苏黎世大学Scaramuzza教授的实验室，在2014年发表的一种视觉里程计算法，它的名称是半直接法视觉里程计，通俗点说，就是结合了特征点法和直接法的视觉里程计。目前该算法已经在github上面开源（https://github.com/uzh-rpg/rpg_svo）。贺一家在它的开源版本上面进行改进，形成了SVO_Edgelet（https://github.com/HeYijia/svo_edgelet）。相比原版，SVO_Edgelet增加了一些功能，比如结合本质矩阵和单应矩阵来初始化，把边缘特征点加入跟踪等，对SVO的鲁棒性有非常大的改善。

虽然SVO已经有论文[1]了，但是论文里面只是讲了最核心的算法理论，可以用论文来了解其算法思想。但是具体的实现方法和技巧，都隐藏在源代码里。要想彻底掌握它，并且在具体实践中灵活运用它，还是必须要阅读源代码才行。

所以我通读了这2万多行的源代码，力求从代码中反推出所有的具体实现、算法、公式、技巧、作者的意图。

源码之下，了无秘密。

在把这2万多行的代码全部搞懂之后，我把代码里面的具体实现方法全都一五一十地还原出来，研究其优缺点、适用情况，探讨其可以改进的地方，总结成本文，与各位分享。

本文对应的程序为SVO_Edgelet。

本文目标读者：对SVO有一定了解的SLAM算法工程师。

流程图

1.跟踪

其实，SVO的跟踪部分的本质是跟ORBSLAM一样的，TrackWithMotionModel和TrackLocalMap，只是匹配的方法从特征点法改成了灰度值匹配法。

但是，随后，与ORBSLAM有不同的地方，SVO在优化出相机位姿之后，还有可选项，可以再优化地图点，还可以再把地图点和相机位姿一起优化。

1.1初始化

图像刚进来的时候，就获取它的金字塔图像，5层，比例为2。

然后处理第一张图像，processFirstFrame()。先检测FAST特征点和边缘特征。如果图像中间的特征点数量超过50个，就把这张图像作为第一个关键帧。

然后处理第一张之后的连续图像，processSecondFrame()，用于跟第一张进行三角初始化。从第一张图像开始，就用光流法持续跟踪特征点，把特征像素点转换成在相机坐标系下的深度归一化的点，并进行畸变校正,再让模变成1,映射到单位球面上面。

如果匹配点的数量大于阈值，并且视差的中位数大于阈值。如果视差的方差大的话，选择计算E矩阵，如果视差的方差小的话，选择计算H矩阵。如果计算完H或E后，还有足够的内点，就认为这帧是合适的用来三角化的帧。根据H或E恢复出来的位姿和地图点，进行尺度变换，把深度的中值调为1。

然后把这一帧，作为关键帧，送入到深度滤波器中。（就是送到的深度滤波器的updateSeedsLoop()线程中。深度滤波器来给种子点在极线上搜索匹配点，更新种子点，种子点收敛出新的候选地图点。如果是关键帧的话，就初始化出新的种子点，在这帧图像里面每层的每个25x25大小的网格里，取一个最大的fast点。在第0层图像上，找出canny边缘点。）

之后就是正常的跟踪processFrame()。

1.2基于稀疏点亮度的位姿预估

把上一帧的位姿作为当前帧的初始位姿。

把上一帧作为参考帧。

先创建n行16列的矩阵ref_patch_cache_，n表示参考帧上面的特征点的个数，16代表要取的图块的像素数量（即像素块的数量为4*4）。

再创建6行n*16列的矩阵jacobian_cache_。代表图块上的每个像素点误差对相机位姿的雅克比。

要优化的是参考帧相对于当前帧的位姿。

把参考帧上的所有图块结合地图点，往当前帧图像的金字塔图像上投影。在当前帧的金字塔图像上，从最高层开始，一层层往低层算。每次继承前一次的优化结果。如果前一次的误差相比前前次没有减小的话，就继承前前次的优化后的位姿。每层的优化，迭代30次。

要优化的残差是，参考帧I_k-1上的特征点的图块与投影到当前帧I_k上的位置上的图块的亮度残差。投影位置是，参考帧I_k-1中的特征点延伸到三维空间中到与对应的地图点深度一样的位置，然后投影到当前帧I_k。这是SVO的一个创新点，直接由图像上的特征点延伸出来，而不是地图点（因为地图点与特征点之间也存在投影误差），这样子就保证了要投影的图块的准确性。延伸出来的空间点肯定也与特征点以及光心在一条直线上。这样子的针孔模型很漂亮。

SVO的另外一个创新点（指的是逆光流算法）是，以当前帧上的投影点的像素值为基准，通过优化调整参考帧投影过来的像素点的位置，以此来优化这两者像素值残差。这样子，投影过来的图块patch上的像素值关于像素点位置的雅克比，就可以提前计算并且固定了。（而以前普通的方法是，以参考帧投影过去的像素值为基准，通过优化投影点的位置，来优化这两者的残差。）

残差用公式表示为：

其中，l表示半个图块的大小。

级联求导，但是，这样子的话，每次迭代后，雅克比J都会发生改变。一般情况下的优化，都会遇到这样的问题。

所以，采用近似的思想。首先，认为空间点P固定不动，只调整参考帧k-1的位姿，所以这个扰动不影响在当前帧上的投影点的位置，只会影响图块patch的内容。然后，参考帧在新的位姿上重新生成新的空间点，再迭代下去。虽然只是近似优化，但每次迭代的方向都是对的，假设步长也差不多，所以最终也可以优化成功。

1.3基于图块的特征点匹配

因为当前帧有了1.1的预估的位姿。对于关键帧链表里面的那些关键帧，把它们图像上的分散的5点往当前帧上投影，看是否能投影成功，如果能投影成功，就认为共视。再把所有的共视关键帧，按照与当前帧的距离远近来排序。然后，按照关键帧距离从近到远的顺序，依次把这些关键帧上面的特征点对应的地图点都往当前帧上面投影，同一个地图点只被投影一次。如果地图点在当前帧上的投影位置，能取到8x8的图块，就把这个地图点存入到当前帧投影位置的网格中。

再把候选地图点都往当前帧上投影，如果在当前帧上的投影位置，能取到8x8的图块，就把这个候选地图点存入到当前帧投影位置的网格中。如果一个候选点有10帧投影不成功，就把这个候选点删除掉。

然后，对于每一个网格，把其中对应的地图点，按照地图点的质量进行排序（TYPE_GOOD> TYPE_UNKNOWN> TYPE_CANDIDATE> TYPE_DELETED）。如果是TYPE_DELETED，则在网格中把它删除掉。

遍历网格中的每个地图点，找到这个地图点被观察到的所有的关键帧。获取那些关键帧光心与这个地图点连线，与，地图点与当前帧光心连线，的夹角。选出夹角最小的那个关键帧作为参考帧，以及对应的特征点。（注意，这里的这种选夹角的情况，是只适合无人机那样的视角一直朝下的情况的，应该改成ORBSLAM那样，还要再把视角转换到对应的相机坐标系下，再筛选一遍）。这个对应的特征点，必须要在它自己的对应的层数上，能获取10x10的图块。

然后，计算仿射矩阵。首先，获取地图点在参考帧上的与光心连线的模。然后它的对应的特征点，在它对应的层数上，取右边的第5个像素位置和下边的第5个像素位置，再映射到第0层。再转换到单位球上，再映射到三维空间中，直到与地图点的模一样的长度。把对应的特征点也映射到三维空间中，直到与地图点的模一样的长度。然后，再把这3个点映射到当前帧的（有畸变的）图像上。根据它们与中心投影点的位置变换，算出了仿射矩阵A_cur_ref。A_cur_ref.col(0) = (px_du - px_cur)/halfpatch_size; A_cur_ref.col(1) = (px_dv - px_cur)/halfpatch_size;。（www.cnblogs.com/ilekoaiq）仿射矩阵A，就是把参考帧上的图块在它自己对应的层数上，转换到当前帧的第0层上。（这种把比例变换转换成矩阵表示的方法，很好）。

然后，计算在当前帧的目标搜索层数。通过计算仿射矩阵A_cur_ref的行列式，其实就是面积放大率。如果面积放大率超过3，就往上一层，面积放大率变为原来的四分之一。知道面积放大率不再大于3，或者到最高层。就得到了目标要搜索的层数。

然后，计算仿射矩阵的逆仿射矩阵A_ref_cur。然后，这样子，如果以投影点为中心（5,5），取10x10的图块，则图块上每个像素点的（相对中心点的）位置，都可以通过逆仿射矩阵，得到对应的参考帧上的对应层数图像上的（相对中心点的）像素位置。进行像素插值。就是，把参考帧上的特征点附近取一些像素点过来，可以组成，映射到当前帧上的对应层数的投影点位置的附近，这些映射到的位置刚好组成10x10的图块。

然后，从映射过来的10x10的图块中取出8x8的图块，作为参考图块。对这个图块的位置进行优化调整，使得它与目标位置的图块最匹配。残差表达式为。

在这里，SVO有两个创新点。



如果是一个TYPE_UNKNOWN类型的地图点，它找匹配失败的次数大于15次，就把它变为delete类型的点。如果是一个TYPE_CANDIDATE类型的点，它匹配失败的次数大于30次，就把它变为delete类型的点。

如果匹配成功的话，就在当前图像上，新生成一个特征点（包括坐标和层数），特征点指向那个地图点。如果对应的参考帧上的特征点是边缘点的话，则新的特征点的类型也设为边缘点，把梯度也仿射过来，归一化后，作为这个新特征点的梯度。

每个网格里，只要有一个地图点匹配成，就跳出这个网格的遍历循环。如果有180个网格匹配成功了，直接跳出所有网格的循环。循环结束后，如果成功匹配的网格的数量小于30个，就认为当前帧的匹配失败。

1.4进一步优化位姿

1.5优化地图点

1.6 BA

SVO里面有个选项，可以开启使用g2o的BA功能。

如果开启使用这个功能的话，则在一开始的两张图像初始化之后，两张图像以及初始化出来的地图点，会用BA来优化。用的是g2o里面的模板。

另外，会在1.5优化完地图点后，对窗口里的所有的关键帧和地图点，或者全局关键帧和地图点，进行优化。用的是g2o里面的模板。

1.7对畸变图像处理的启发

SVO的跟踪都是在畸变的鱼眼图像上跟踪的，没有对图像进行校正，这样子可以尽可能地保留图像的原始信息。

又因为在1.2中的逆向图块雅克比的方法，除了可以加快计算外，还避免了对畸变参数的雅克比计算。因为如果用正向图像雅克比的话，在计算雅克比的时候，必须要把畸变参数也考虑进来。

而在1.3中，图块匹配就是用畸变的图块取匹配的，保证了准确性。为了避免对畸变参数的雅克比计算，在匹配完成后，把投影点位置和匹配点位置都从畸变的图像上，转换到了单位平面上。以后在畸变图像上，计算重投影误差，就用这样的方法。

2.创建地图点

特征点提取的方法，放在了地图线程里。因为与ORBSLAM不同的是，它跟踪的时候，不需要找特征点再匹配，而是直接根据图块亮度差匹配的。

而如果是vins的话，也可以参考这个方法，把特征点提取放到地图线程里，连续帧之间的特征点用光流匹配。但光流要求帧与帧之间不能差别太大。

而在SVO中，后端的特征点是只在关键帧上提取的，用FAST加金字塔。而上一个关键帧的特征点在这一个关键帧上找匹配点的方法，是用极线搜索，寻找亮度差最小的点。最后再用depthfilter深度滤波器把这个地图点准确地滤出来。

选取30个地图点，如果这30个地图点在当前帧和最近一个关键帧的视差的中位数大于40，或者与窗口中的关键帧的距离大于一定阈值，就认为需要一个新的关键帧。然后把当前帧设置为关键帧，对当前帧进行操作。

2.1初始化种子

当关键帧过来的时候，对关键帧进行处理。在当前图像上，划分出25像素*25像素的网格。

首先，当前帧上的这些已经有的特征点，占据住网格。

在当前帧的5层金字塔上，每层头提取fast点，首先用3x3范围的非极大值抑制。然后，对剩下的点，全部都计算shiTomasi分数，有点像Harris角点里面的那个分数。再全部映射到第0层的网格上，每个网格只保留分数最大的，且大于阈值的那个点。

找边缘点的话，都只在第0层上面找。同样也是画网格，然后再每个网格中找canny线，然后对于网格中的在canny线上的点，计算它的梯度的模，保留模梯度最大的那个点，作为边缘点。梯度方向是二维的，就是这个点的右左下上梯度。程序里用了cv::Scharr结合cv::magnitude来快速算出所有点的横纵方向的梯度。

然后，对于所有的新的特征点，初始化成种子点。用高斯分布表示逆深度。均值为最近的那个点的深度的倒数。深度范围为当前帧的最近的深度的倒数，即1.0/depth_min。高斯分布的标准差为1/6*1.0/depth_min。

2.2更新种子，深度滤波器

如果新来一个关键帧，或者是当前的普通的帧，或者之前的关键帧，用于更新种子点。对于每个种子点，通过正负1倍标准差，确定逆深度的搜索范围。这些参数都是对应种子点在它自己被初始化的那一帧。

然后把深度射线上的最短和最长的深度，映射到当前帧的单位深度平面上，其实就得到的在单位平面上的极线线段。然后，再把逆深度的均值对应的深度，映射到当前帧，就是跟1.3中的同样的方法，得到图块仿射矩阵，和最佳搜索层数。

（对于边缘点，如果把梯度仿射过来后，梯度的方向与极线方向的夹角大于45度，就认为沿着极线找，图块像素也不会变化很大，就不搜索了，直接返回false。）

把极线线段投影到对应的层数上，如果两个端点间的像素距离小于2个像素，就直接进行优化位置。用的是1.3中的找图块匹配的方法，把对应的图块映射过来。找到最佳匹配位置后，进行三角定位。三角定位的方法参考《视觉SLAM十四讲》的三角定位，矩阵分块计算。

如果两个端点间像素距离大于2个像素，就在极线上进行搜索。首先，确定总步长数，以两端点间的距离除以0.7，得到总步长数n_steps。然后，把单位深度平面上的极线线段分n_steps段，从一个端点开始往另外一个端点走，每走一步，就把位置投影（包括畸变）到对应层数的图像上，坐标取整后，获取图块。（这里可以改进，不应该对坐标进行取整，而应该改成插值）。然后，计算投影过来的图块与投影位置图块的相似度，相似度的计算公式如下，其中有消除均值的影响。

3.重定位

SVO中重定位，实现很简单，就是在跟丢之后，仍然假设当前帧的位姿和前一帧一样，往这个位姿上投地图点，用第1部分中的方法去优化计算，如果优化成功，就重定位回来，如果优化不成功，就继续下一帧。所以，在跟丢后，只能再回到跟丢时的位置，才能重定位回来。

这样子实现重定位的方法很简单，可重定位的效果就很差了。这地方可以进行改进。

4.总结

SVO的定位很好，抖动很小。尤其在重复纹理的环境中，表现得比基于特征点法的ORBSLAM2要出色。

将来可以在上面增加更鲁棒的重定位，回环闭环，全局地图的功能，来满足更多的实际应用场景，比如室内机器人、无人机、无人车。