直接法及SVO算法细节

 

直接法及SVO算法细节

简介：

特征点法：最小化重投影误差

直接法：基于灰度不变假设。（最小化光度误差也成为测量误差）

灰度不变假设：同一个空间点的像素灰度，在各个图像中是固定不变的。

（相机自动曝光会影响图像整体变亮或变暗，这时这种假设是不成立的，会影响结果，目前的解决方法是使用更细致的光度模型标定相机，以便在曝光时间变化时也能工作）

在直接法中，我们同样是解一个优化问题，但这个优化最小化的不是重投影误差，而是测量误差（Photometric Error）即光度误差，也就是 P的两个像的亮度误差：

e=I1(p1)−I2(p2)

误差e是如何随着相机位姿ξ变化的，需要分析它们的导数关系，（有导数关系，这里暂不分析，先介绍思路）

SVO：

SVO，虽然按照作者的理解，称为“半直接法”，目前大家也称其为“稀疏直接法”可能更好一些。VO主要分为特征点法和直接法。而SVO的实现中，混合使用了特征点与直接法：它跟踪了一些关键点（角点，没有描述子，由FAST实现），然后像直接法那样，根据这些关键点周围的信息，估计相机运动以及它们的位置。这与特征点法（以ORB-SLAM为代表）那样，需要对每张图像提取特征点和描述子的实现，是有明显不同的。所以，作者称之为“半直接法”。

而SVO只跟踪稀疏的关键点，所以不妨称之为“稀疏直接法”

在直接法中（包括稀疏的，半稠密的以及稠密的），使用稀疏的直接法，既不必费力去计算描述子，也不必处理像稠密和半稠密那么多的信息，能够达到极快的速度。因此，SVO即使在低端计算平台上也能达到实时性，而在PC平台上则可以达到100多帧每秒的速度。在作者后续工作SVO 2.0中，速度更达到了惊人的400帧每秒。这使得SVO非常适用于计算平台受限的场合，例如无人机、手持AR/VR设备的定位。无人机也是弗斯特等人最初开发SVO的目标应用平台。开源的是SVO1.0阉割版。SVO2.0商用未开源。

测试在重复度高的图片上效果也不错，因为SVO采用的是跟踪的方式，开源测试的图片是足球场图片，重复度极高，但纹理相对比较丰富，如下：

 

建议：

若在arm平台实现，前期VO使用SVO，构建地图时，想要回环检测（需要特征点描述符），加入特征点描述符的计算。

直接法算法细节

 

SVO算法细节

SVO算法框图：

 

追踪部分：

1.sparse model-based image alignment

  使用直接法最小化图像块重投影残差来获取位姿。如图所示：其中红色的Tk,k−1为位姿，即优化变量。

 

 

 

追踪的第一步是将当前帧与上一个追踪成功的帧进行对比，粗略估计当前帧的位姿。该问题的基本形式为：已知上一帧对地图点的观测（包括2D投影位置和深度），以及当前帧的图像，如何计算当前帧的位姿？用数学语言说，已知帧的位姿，并且知道它的观测量时，求解。

直接法具体过程如下： 
  step1. 准备工作。假设相邻帧之间的位姿Tk,k−1已知，一般初始化为上一相邻时刻的位姿或者假设为单位矩阵。通过之前多帧之间的特征检测以及深度估计，我们已经知道第k-1帧中特征点位置以及它们的深度。 

step2. 重投影。知道Ik−1中的某个特征在图像平面的位置(u,v)，以及它的深度d，能够将该特征投影到三维空间pk−1，该三维空间的坐标系是定义在Ik−1摄像机坐标系的。所以，我们要将它投影到当前帧Ik中，需要位姿转换Tk,k−1，得到该点在当前帧坐标系中的三维坐标pk。最后通过摄像机内参数，投影到Ik的图像平面(u′,v′)，完成重投影。      

step3. 迭代优化更新位姿。按理来说对于空间中同一个点，被极短时间内的相邻两帧拍到，它的亮度值应该没啥变化。但由于位姿是假设的一个值，所以重投影的点不准确，导致投影前后的亮度值是不相等的。不断优化位姿使得这个残差最小，就能得到优化后的位姿Tk,k−1。 

将上述过程公式化如下：通过不断优化位姿Tk,k−1最小化残差损失函数。

其中

加粗u=(u.v)^T ,du是点的深度，π（）是相机投影模型函数，π^-1（）是逆投影函数，公式中第一步为根据k-1图像位置和深度逆投影到三维空间，第二步将三维坐标点旋转平移到当前帧坐标系下，第三步再将三维坐标点投影回当前帧图像坐标。当然在优化过程中，残差的计算方式不止这一种形式：有前向(forwards)，逆向(inverse)之分，并且还有叠加式(additive)和构造式(compositional)之分。这方面可以读读光流法方面的论文，Baker的大作《Lucas-Kanade 20 Years On: A Unifying Framework》。选择的方式不同，在迭代优化过程中计算雅克比矩阵的时候就有差别，一般为了减小计算量，都采用的是inverse compositional algorithm。 

上面的非线性最小化二乘问题，可以用高斯牛顿迭代法求解，位姿的迭代增量ξ(李代数)可以通过下述方程计算：

 

SVO自己实现了高斯——牛顿法的迭代下降，并且比较取巧地使用了一种反向的求导方式：即雅可比在k-1帧图像上进行估计，而不在k帧上估计。这样做法的好处是在迭代过程中只需计算一次雅可比，其余部分只需更新残差即可（即G-N等式右侧的）。这能够节省一定程度的计算量。另一个好处是，我们能够保证k-1帧的像素具有梯度，但没法保证在k帧上的投影也具有梯度，所以这样做至少能保证像素点的梯度是明显的。

实现当中另一个需要注意的地方是金字塔的处理。这一步估计是从金字塔的顶层开始，把上一层的结果作为下一层估计的初始值，最后迭代到底层的。顶层的分辨率最小，所以这是一个由粗到精的过程（Coarse-to-Fine），使得在运动较大时也能有较好的结果。

值得一提的是，完全可以使用优化库，例如g2o或ceres来实现所有的步骤。在优化库中，可以选用更多的优化方式，而且L-M或Dogleg的结果也会比G-N更有保证。

2.Relaxation Through Feature Alignment

我们得到了当前帧位姿的粗略估计。因为它是基于上一帧的结果来计算的，所以如果把它当作真实位姿估计的话，将有较大的累积误差。因此，需要进一步和地图之间进行特征点比对，来对当前帧位姿进一步优化。

 

图中灰色的特征块为真实位置，蓝色特征块为预测位置。幸好，他们偏差不大，可以构造残差目标函数，和上面直接法类似，不过优化变量不再是相机位姿，而是像素的位置(u′,v′)，通过迭代对特征块的预测位置进行优化。这就是svo中提到的Feature Alignment。

地图模型通常来说保存的就是三维空间点，因为每一个Key frame通过深度估计能够得到特征点的三维坐标，这些三维坐标点通过特征点在Key Frame中进行保存。所以SVO地图上保存的是Key Frame 以及还未插入地图的KF中的已经收敛的3d点坐标（这些3d点坐标是在世界坐标系下的），也就是说地图map不需要自己管理所有的3d点，它只需要管理KF就行了。先看看选取KF的标准是啥？KF中保存了哪些东西？当新的帧new frame和相邻KF的平移量超过场景深度平均值的12%时(比如四轴上升)，new frame就会被当做KF，它会被立即插入地图。同时，又在这个新的KF上检测新的特征点作为深度估计的seed，这些seed会不断融合新的new frame进行深度估计。但是，如果有些seed点3d点位姿通过深度估计已经收敛了，怎么办？map用一point_candidates来保存这些尚未插入地图中的点。所以map这个数据结构中保存了两样东西，以前的KF以及新的尚未插入地图的KF中已经收敛的3d点。 

通过地图我们保存了很多三维空间点，很明显，每一个new frame都是可能看到地图中的某些点的。由于new frame的位姿通过上一步的直接法已经计算出来了，按理来说这些被看到的地图上的点可以被投影到这个new frame中，即图中的蓝色方框块。上图中分析了，所有位姿误差导致这个方框块在new frame中肯定不是真正的特征块所处的位置。所以需要Feature Alignment来找到地图中特征块在new frame中应该出现的位置，根据这个位置误差为进一步的优化做准备。基于光度不变性假设，特征块在以前参考帧中的亮度应该和new frame中的亮度差不多。所以可以重新构造一个残差，对特征预测位置进行优化：

 

注意这里的优化变量是像素位置，这过程就是光流法跟踪嘛。并且注意，光度误差的前一部分是当前图像中的亮度值，后一部分不是Ik−1而是Ir，即它是根据投影的3d点追溯到的这个3d点所在的key frame中的像素值，而不是相邻帧。由于是特征块对比并且3d点所在的KF可能离当前帧new frame比较远，所以光度误差和前面不一样的是还加了一个仿射变换，需要对KF帧中的特征块进行旋转拉伸之类仿射变换（Affine Warp）后才能和当前帧的特征块对比。 仿射变换的计算方式在PTAM论文的5.3节有介绍，似乎是一种比较标准的处理方式。

这时候的迭代量计算方程和之前是一样的，只不过雅克比矩阵变了。

 

  通过这一步我们能够得到优化后的特征点预测位置，它比之前通过相机位姿预测的位置更准，所以反过来，我们利用这个优化后的特征位置，能够进一步去优化相机位姿以及特征点的三维坐标。所以位姿估计的最后一步就是Pose and Structure Refinement。

3.Pose and Structure Refinement

  在一开始的直接法匹配中，我们是使用的光度误差，这里由于优化后的特征位置和之前预测的特征位置存在差异，这个能用来构造新的优化目标函数。

 

上式中误差变成了像素重投影以后位置的差异（不是像素值的差异），优化变量还是相机位姿，雅克比矩阵大小为2×6(横纵坐标u，v分别对六个李代数变量求导)。这一步是就叫做motion-only Bundle Adjustment。同时根据根据这个误差定义，我们还能够对获取的三维点的坐标(x,y,z)进行优化，还是上面的误差像素位置误差形式，只不过优化变量变成三维点的坐标，这一步叫Structure -only Bundle Adjustment，优化过程中雅克比矩阵大小为2×3(横纵坐标u，v分别对点坐标(x,y,z)变量求导)。SVO实现中，是把Pose和Point两个问题拆开优化的，速度更快。

代码实现当中还需要注意一些细节。例如有些地方使用了网格，以保证观测点的均匀分布。还有Affine Warp当中需要注意特征点所在的金字塔层数，等等。

 

步骤小结：

• Frame-frame间位姿变换
• 遍历地图中的所有点，计算在当前帧的投影位置。由于当前帧有粗略的位姿估计，这个投影位置应该与真实位置有少量误差（2~3个像素）。
• 对每个成功投影的地图点，比较这些点的初始观测图像与当前帧的图像。通过计算光度的误差，求取更精准的投影位置。这步类似于光流，在SVO中称为Refinement。
• 根据更精确的投影位置，进行位姿与地图点的优化。这一步类似于Bundle Adjustment，但SVO实现中，是把Pose和Point两个问题拆开优化的，速度更快。
• 判断是否生成关键帧，处理关键帧的生成。

 

建图部分：

Mapping部分主要是计算特征点的深度。如前所言，单目VO中，刚刚从图像中提取的热乎的关键点是没有深度的，需要等相机位移之后再通过三角化，再估计这些点的深度。这些尚未具备有效深度信息的点，不妨称之为种子点（或候选点）。然而，三角化的成功与否（以及精度），取决于相机之间的平移量和视线的夹角，所以我们通常要维护种子点的深度分布，而不是单纯的一个深度值。牵涉到概率分布的，往往都是理论一大堆，实际可以操作的只有高斯分布一种——高斯只要在计算机里存均值和协方差即可。SVO使用了一种高斯——均匀混合分布的逆深度（由四个参数描述），推导并实现了它的更新方式，称为Depth Filter。它的工作逻辑大概是这样的：如果进来一个关键帧，就提取关键帧上的新特征点，作为种子点放进一个种子队列中。如果进来一个普通帧，就用普通帧的信息，更新所有种子点的概率分布。如果某个种子点的深度分布已经收敛，就把它放到地图中，供追踪线程使用。当然实现当中还有一些细节，比如删掉时间久远的种子点、删掉很少被看到的种子点等等。要理解Depth Filter，请搞清楚这两件事：基于高斯——均匀的滤波器，在理论上的推导是怎么样的？Depth Filter又是如何利用普通帧的信息去更新种子点的？

 

最基本的深度估计就是三角化，这是多视角几何的基础内容(可以参看圣经Hartly的《Multiple View Geometry in Computer Vision》中的第十二章structure computation；可以参看我的相应博客)。我们知道通过两帧图像的匹配点就可以计算出这一点的深度值，如果有多幅图像，那就能计算出这一点的多个深度值。这就像对同一个状态变量我们进行了多次测量，因此，可以用贝叶斯估计来对多个测量值进行融合，使得估计的不确定性缩小。如下图开始深度估计的不确定性较大(浅绿色部分)，通过三角化得到一个深度估计值以后，能够极大的缩小这个不确定性(墨绿色部分)。

  在这里，先简单介绍下svo中的三角化计算深度的过程，主要是极线搜索确定匹配点。在参考帧Ir中，我们知道了一个特征的图像位置，假设它的深度值在[dmin,dmax]之间，那么根据这两个端点深度值，我们能够计算出他们在当前帧Ik中的位置，如上图中草绿色圆圈中的线段。确定了特征出现的极线段位置，就可以进行特征搜索匹配了。如果极线段很短，小于两个像素，那直接使用上面求位姿时提到的Feature Alignment光流法就可以比较准确地预测特征位置。如果极线段很长，那分两步走，第一步在极线段上间隔采样，对采样的多个特征块一一和参考帧中的特征块匹配，用Zero mean Sum of Squared Differences 方法对各采样特征块评分，那个得分最高，说明他和参考帧中的特征块最匹配。第二步就是在这个得分最高点附近使用Feature Alignment得到次像素精度的特征点位置。像素点位置确定了，就可以三角化计算深度了。

  得到一个新的深度估计值以后，用贝叶斯概率模型对深度值更新。在LSD slam中，假设深度估计值服从高斯分布，用卡尔曼滤波(贝叶斯的一种)来更新深度值。这种假设中，他认为深度估计值效果很棒，很大的概率出现在真实值(高斯分布均值)附近。而SVO的作者采用的是Vogiatzis的论文《Video-based, real-time multi-view stereo》提到的概率模型：所示：

这个概率模型是一个高斯分布加上一个设定在最小深度dmin和最大深度dmax之间的均匀分布。这个均匀分布的意义是假设会有一定的概率出现错误的深度估计值。有关这个概率模型来由更严谨的论证去看看Vogiatzis的论文。同时，有关这个概率模型递推更新的过程具体可以看Vogiatzis在论文中提到的Supplementary material，论文中告知了下载地址。知道了这个贝叶斯概率模型的递推过程，程序就可以实现深度值的融合了，结合supplementary material去看svo代码中的updateSeeds(frame)这个程序就容易了，整个程序里的那些参数的计算递归过程的推导，现在有几篇博客对该部分进行了推导，卢彦斌：svo原理解析,东北大学孙志明：svo的Supplementary matterial 推导过程。

在深度估计的过程中，除了计算深度值外，这个深度值的不确定性也是需要计算的，它在很多地方都会用到，如极线搜索中确定极线的起始位置和长度，如用贝叶斯概率更新深度的过程中用它来确定更新权重(就像卡尔曼滤波中协方差矩阵扮演的角色)，如判断这个深度点是否收敛了，如果收敛就插入地图等等。SVO的作者Forster作为第二作者发表的《REMODE: Probabilistic, Monocular Dense Reconstruction in Real Time》中对由于特征定位不准导致的三角化深度误差进行了分析，如下图：它是通过假设特征点定位差一个像素偏差，来计算深度估计的不确定性。具体推导见原论文，简单的几何关系。

 

是自己学习SVO过程中所看的资料的汇总。加了一点点自己结合论文的理解，建议大家看到时候也仔细看看论文。

参考：

高博的公开课以及高博的知乎回答

所长的博客：http://blog.csdn.net/heyijia0327/article/details/51083398

冯兵大神的博客：http://fengbing.net/2015/08/08/%E4%B8%80%E6%AD%A5%E6%AD%A5%E5%AE%9E%E7%8E%B0%E5%8D%95%E7%9B%AE%E8%A7%86%E8%A7%89%E9%87%8C%E7%A8%8B%E8%AE%A12%E2%80%94%E2%80%94FAST%E7%89%B9%E5%BE%81%E6%A3%80%E6%B5%8B/