直接法及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/

 

posted @ 2017-06-30 09:13  Coco_酱  阅读(11563)  评论(0编辑  收藏  举报