Reference
- Semi-Dense Visual Odometry for a Monocular Camera. LSD-SLAM深度滤波的论文
- https://www.zybuluo.com/lancelot-vim/note/412293 讲解LSD-SLAM中深度滤波的博客
- https://blog.csdn.net/kokerf/article/details/78006703 也是一篇讲解LSD-SLAM中深度滤波的博客
- REMODE: Probabilistic, Monocular Dense Reconstruction in Real Time. SVO采用的深度滤波的论文
- Supplementary matterial Parametric approximation to posterior. SVO的补充材料,推到了高斯--均匀模型到高斯--Beta分布的推导过程
- https://www.cnblogs.com/ilekoaiq/p/8228324.html. 比较详细的推导了高斯--均匀模型
- 视觉SLAM十四讲. 第十三章对逆深度基于高斯分布的融合进行了讲解
- https://blog.csdn.net/qq_18343569/article/details/49003993 讲解一些像素匹配算法
- https://towardsdatascience.com/beta-distribution-intuition-examples-and-derivation-cf00f4db57af Beta分布的参考
本文会比较长,因为本身这部分就比较复杂,笔者自身加入了自己的一些思考和理解,不对的地方请及时指出,一同进步。
一切的开始——简单的深度滤波
这部分主要参考高博的slam十四讲
我们都知道,SLAM中建图是一个很重要的部分,SLAM进行位姿推断也是基于之前建立的地图(即三维空间的坐标点)进行的,目前最流行的方法通常是直接根据两帧或者多帧图像看到同一个特征点,之后对这个特征点直接三角化作为一个初值,之后在后续的优化问题中不断的对这个坐标点进行优化或者剔除;这样的做的优点就是简单、易行,但是缺点也比较明显,如果某个时刻点的生成出现了问题,那么对于系统的稳定性可能是灾难性的;基于此,另一种方法进入人们的视野——深度滤波。
我个人觉得深度滤波其实更偏向于一个后端的技术,前端得到相对正确的位姿,后端根据位姿进行深度值不断的迭代更新,使得深度值收敛到一个稳定的值。
常规来说,深度滤波都会使用以下几个步骤:
- 根据两帧的位姿计算当前帧上的极线搜索范围;
- 在要搜算的极线段上通过对比算法获得最佳的匹配位置;
- 通过得到的最佳匹配点计算深度值;
- 把该深度值作为观测进行滤波器的迭代;
一种较为简单的方法就是把高斯分布
下面根据上面的几个步骤分解一下该深度滤波方法:
第一步:根据位姿计算当前帧上的极线搜索范围
该步骤用图表示为如下形式:
从图中可以看到,在运用深度滤波的时候:
- 通常已经通过前端得到了相对稳定的位姿,记为
和;
- 当前深度滤波得到的深度值为
,标准差为,此时可以得到一个可能的深度范围,记为;
- 将可能的深度范围映射到当前帧上,就可以得到极线上的搜索范围;
第二步:在极线搜索段上找到最佳的匹配点
经过上一步之后就可以在当前帧上得到一个大概的搜算范围,算法会认为真实深度所对应的投影点就在这个搜算范围内,这里得到对应投影点的方法并不会使用基于特征的匹配方法(运算量比较大),而是采用基于像素块的匹配方法,具体的做法如下:
- 从搜算范围的一端一个步长一个步长的向另一端移动;
- 每在极线方向上移动一步(步长通常设为
),就将当前帧的窗口内的像素值与参考帧的窗口内的像素值进行比较,得到一个分数用来表征两者是否相似,最后取最相似的那个位置作为匹配点;
其中相似的方法参考Reference中的【5】.
但是这个过程中必然伴随着误匹配(特征点法也无法避免这样的情况!),也就是一个搜索范围内很多地方满足要求,十四讲中给出下面的图,可以看到在整个搜索范围内有较多的点都是峰值,虽然下图中有一个最大值,但是其实这个最大值也不一定就是真正的对应匹配:
在LSD-SLAM中也给出一个例子表示这样的情况,可以看到,基线越小(两个图像间的移动越小),搜索的时候最小值是唯一的,但是不确定性会很大(后面会说为什么),也就是方差会很大;相反,较大的基线,搜索的时候会有较多的最小值,但是不确定性会很小;这也是深度滤波存在的一个重要的理由之一:
这部分稍微再拓展一下,因为在整个深度滤波中,好的匹配会加快整个滤波的收敛过程,所以SVO在这方面引入了一个仿射矩阵来更好的对比两个像素块,其实在上图中可以看到,reference图像的像素块假设是一个标准的正方形,那到了后面的帧中,这个正方形很可能就可能变作了一个平行四边形,在多视图几何中,这样的变化在2D到2D平面通常被建模成仿射变换,公式如下:
有了公式(1)之后,可以在Reference图像中取三个点,分别设为
于是可以得到以下关系:
于是我们可以通过
于是我们就可以得到A矩阵的第一列了,同样的方法应用到
- 在求解对应关系的时候,以
为例,有,这里的d一定使用的是中心点的d;
- 通常算法会反过来使用A,即认为当前帧上的是标准的正方形,参考帧上有对应的仿射变换,这样可以节约很多计算量;
- 求解出A之后,算法通常还会根据A的行列式(表示仿射变换对于图像影响的程度),找到最佳的图像金字塔层来更好的进行匹配点的搜索;
第三步:通过得到的最佳匹配点计算深度值
这部分比较简单,通常就是正常的三角化,这里不再多进行赘述。
但是这里记录一下三角化中比较著名的矛盾问题,同时也是解释上面说的为什么较小的视差会引入较大的不确定性:
这个关系上图体现的比较清晰,当引入一个观测角度误差
在简单的深度滤波中,因为深度估计被建模为高斯分布,因此观测值也要符合高斯分布才可以,至此需要观测的不确定度,也就是方差,通常算法会认为当前帧的观测点(在图像坐标系下)有一个像素的误差,然后根据差了这一个像素的匹配位置来估计深度
可以看到,当引入一个像素的误差之后,整个误差分析建立在极平面上,从而把整个三维的问题映射到了二维平面上,这里的最后的误差具体的计算方式就不详细展开了,主要是求解出上图中的
第四步:把该深度值作为观测进行滤波器的迭代
根据上面的三个步骤,算法就已经得到了符合高斯分布的观测模型
两个高斯分布的联合分布(个人理解也可以认为就是加权平均)公式如下,其中不加下标的表示联合分布的期望与方差,下标为1和2表示两个不同的分布:
更进一步——对方差进行更精细的建模
这部分主要参考论文Semi-Dense Visual Odometry for a Monocular Camera
上面的过程中可能不少读者都发现一个问题:算法将观测的方差简单的认为是一个像素偏差引起的误差。这个近似对于学者来说当然是不能苟同的,参考【1】就对这个问题进行了深入的分析,对方差进行了更精细的建模。
影响深度计算的因素有哪些?
作者首先梳理了什么因素会影响最终的匹配正确性,作者认为**影响深度计算的主要是在极线上找到的最佳匹配点,也可以认为是视差的长度
作者最终分析认为影响最终深度值的主要因素有(就是公式(6)中的括号内的输入变量):
- 旋转
的估计误差和相机内参的误差;
- 图像的光度差异引起的误差,记为
;
这个函数
公式(7)表示输入的误差是如何影响最终的输出误差的(这个公式在KF中也有),其中
下面详细说明笔者对这两个地方是如何影响最终的匹配的理解:
1. 旋转和相机内参的误差如何影响匹配
我们都知道,一个不准确的位姿和内参参数会造成映射的不准确,,因而引起极线不准确,就像下面的例子一样:
其中:
- A和L表示根据估计的位姿映射得到的极线和无穷远映射点,而L'和A'是真实的极线和无穷远点所在的位置;
- 蓝色的点线表示各项同向误差
对于错误映射点的约束,这个误差值使得错误的映射点必须是极线和点线圆的交点;
- 黑色的虚线表示等势线,与下面的波浪线垂直;
如果算法沿着估计的极线进行搜索,那么对于一个很小的图像部分(因为图像是非线性的,这里取小部分图像以获取图像的线性特性)而言,算法最多可以搜索到真值的等值线上,所以作者对这个过程建模的如下(上图中除了A和A‘的点就是在极线上能搜索到的等势线上的点):
上式表示:最佳的匹配点是极线和映射真值点等势线的交点。
其中:
-
表示无穷远点的映射点,表示极线的方向向量,是被归一化之后的;
-
理论上表示在真值的等势线上的任意一点,但是实际中取真值点,所以这个点其实是不知道的!因此这里作者用表示期望等于的意思;
-
表示图像的梯度方向,表示等势线的方向;
我们先不管实际代码中怎么实现的,单纯对公式(6)进行求解(两边乘图像梯度),可以得到最佳的匹配为:
对于公式(9),逐个看其中的变量:
-
是无穷远的映射点,因此必然受到旋转误差和内参误差的影响;
-
是真值的等势线上的一点,这个点虽然不知道,但这个点是不会受到任何因素的影响的,毕竟真值;
-
是极线的方向向量,这个是确定的,不会受到干扰的,或者说这部分干扰都归入了第一项中;
-
是等势线的方向向量,也不会受到位姿的影响;
综合下来,最佳匹配中的误差来源主要是
公式(9)里面涉及到了向量的内积,因此和角度息息相关,所以作者在这个地方给出如下两个图对角度的影响进行说明,笔者这里也说一下自己的理解:
图中
然后我们从公式和图示上可以看出,当图像的梯度
仿照公式(7),可以得到最佳的视差
其中:
-
也是表示函数对于变量的求导;
-
表示输入变量的方差矩阵;
2. 图像的光度差异引起的误差
这部分对于深度值的影响其实比较明显,因为图像的像素差异直接影响滑动窗口评分,因而影响到最好的匹配点,所以这部分的误差还是很有必要去分析的。
这部分建模为如下公式:
使用泰勒展开并进行迭代的方式可以获得:
其中:
-
表示迭代的初始值,这部分不受图像光度差异的影响;
-
表示参考帧上的像素值,表示迭代初值在当前帧的像素值,这两个部分是受到图像光度差异影响的主要部分;
-
表示在当前帧上极线p上的梯度,这部分也不受到两个图像光度差异的影响;
综上所述,两个图像光度差异
可以看到,当极线部分的梯度
同样仿照公式(7),可以得到最佳的视差
其中:
-
,表示函数对输入变量的求导;
-
表示输入变量的方差矩阵;
3. 逆深度计算的误差
现在回过头去看公式(10)和(13),发现得到的方差都是视差关于输入变量的方差,但是最终需要的是逆深度(注意不是深度哈,公式是不一样的,但是可以相互转换)对于输入变量的误差,因此其实我们还差一步;
根据求导的链式法则:
这里论文特地加了一句话:因为旋转量比较小,所以逆深度
这里简单的推导一下:
上式中
上式中
可以看到,两个等式求解一个未知量
得到了上述的公式,就可以使用斜率的公式进行求解
其中
小结
至此整个对匹配方差的建模就结束了,整个公式其实推导起来不算太难,而且作者的思路其实也很清晰,小结两个部分,第一部分是LSD-SLAM中是如何实现这部分的;第二部分是笔者自己没有思考明白的问题;
- 在LSD-SLAM的实现中,笔者主要想记录的其实是旋转和内参对于视差的影响(下面简称误差的第一部分),因为其他部分其实都按照公式在走,也都比较好看懂,但是这个第一部分作者的做法是不在参考帧上计算,而是在关键帧上计算,个人理解有两个原因:一是因为旋转和内参其实都会影响两帧上的极线位置,且影响的程度是一样的,所以这样做个人觉得是没有问题的;二是因为在推导的时候,算法确实需要真值的梯度,在关键帧上计算刚好满足这一点;
- 论文在做误差第一部分的分析的时候,直接用了无穷远点,但是感觉在做匹配的时候,其实很大程度上跟无穷远点没有关系,通常还是与位移有关系的,这个地方其实不算是特别明白这样的分析是否符合常理;
最后就是感觉LSD-SLAM对深度滤波这块儿真的是狠下功夫,不仅仅是这个方差的建模,后面还考虑了深度图的传播、平滑(代码中称为正则化,个人感觉就是高斯分布的加权平均,跟求联合分布一样),这部分确实要比SVO好太多。
改进深度模型——均匀-高斯逆深度模型
这部分主要参考REMODE: Probabilistic, Monocular Dense Reconstruction in Real Time.
至此我们已经可以得到较好的方差了,那接下来一个问题:如果得到的匹配是错的怎么办?
其实滤波本质上就是想通过滤波算法滤出野值,可是按照上述的方法来做的话,其实仅仅是减缓了野值的影响,但是还是添加进来的野值,虽然权重(方差)可能比较小。
所以作者把逆深度模型建模为均匀-高斯模型,先祭出建立的模型如下:
其中:
-
表示根据极线搜索出来的逆深度,是滤波中的测量;
-
表示逆深度的估计值,是滤波中的一个状态变量;
-
表示当前估计是可用的估计(内点)的概率,是滤波中的一个状态变量;
该模型是说测量逆深度的概率密度是均匀概率密度和高斯概率密度的加权,可以看出当我们知道当前估计是内点的时候(
所以根据上述的似然,由贝叶斯公式可以得到后验概率为:
公式里面因为
引入隐变量
上面说道穷举法实在太逆天,一般根本无法使用,那么作者在公式(20)的基础上想:如果引入一个变量表示当次的测量是好是坏呢?于是作者引入了一个隐变量
该图是统计了150副图像的测量深度的直方图(我比较迷为啥还有负值),可以看到两个事情:
- 测量深度的模型确实更加满足均匀--高斯分布;
- 如果给出了某次测量是好是坏,那么确实可以帮助算法更好的计算,比如在峰值附近告诉算法这是好的测量,那么算法可以直接更新高斯部分;如果是坏的分布,那么就不更新高斯部分;
所以引入了隐变量之后的模型变做:
其中所有的变量上面都介绍过了,不过对于第二个公式笔者并没有特别的理解,不过这个公式是正确的,推导如下:
读者可以把公式(22)带入公式(23)中就可以得到公式(20),那么目前得到了添加进来隐变量之后的模型,此时求解联合概率分布:
其中:
-
表示所有观测量组成的向量;
-
表示所有隐变量组成的向量;
-
表示完全数据,分开写的话表示不完全数据;
到这里因为涉及到了隐变量,这里作者就用KL散度求了近似的分布,然后求解最近的分布来代替求解估计的参数,有点类似于EM中的E步,但是公式上笔者没有对上,后来看参考【6】中大佬自己硬推了一遍,内心无比佩服,总之近似的公式如下:
其中q分布就是近似的分布。
可以看到我们刚好有内部的联合分布,因此带入之后得到:
两边同时把
可以看到公式(27)有几个部分组成,分别是:
- 多个高斯分布的乘积部分:
- 内点概率部分,这部分其实就是一个Beta分布,注意不是伯努利分布,伯努利分布的是已知概率求情况的概率,Beta分布是已知情况求概率:
- 平均分布的累计:
- 先验部分:
可以看到最后的后验概率等于高斯分布、Beta分布、均分分布的乘积、先验分布的乘积,除去一些常量之后(均匀分布和先验分布,这里先验分布其实也是均匀的,因为不知道
其中最后的高斯分布
按照上面的递推公式的话,可以预想到下一次测量来了之后迭代的过程:
这里就不多进行推导了,总之上式还是和隐变量
去除隐变量
可以看到,公式(29)还是与隐变量有关,这怎么能行,这个隐变量在迭代过程中可是不知道的,因此我们还是需要把这个隐变量去掉,数学化一点来说,我们需要把隐变量从公式中边缘化掉。
嗯,看到公式(30)的前半部分,我们可以迅速想到公式(23)了,所以接着往下继续化简:
其实可以看到最后化简出来了两个Beta
本文不打算过多涉及这部分,仅仅给出概念和一些结论:
- 如果先验分布是似然分布的共轭分布的话,那么这两个分布做了贝叶斯推导的话,其后验概率与先验概率满足一样的分布特点。
- Beta分布是伯努利分布、二项分布、负二项分布以及几何分布的共轭先验;
- Guassian分布是Guassian分布的共轭先验;
说回来,这里的先验
所以运用这个性质可以快速的得到后验分布其实也是一个Beta分布和Gaussian分布,因此可以直接往这个方向去推导,之后的过程太过繁琐,这里就不涉及了,想了解的就去看参考【6】,个人感觉这之前的过程理解了就可以了,后面的过程就不必太过纠结了。
总结
到这里整个深度滤波就算是告一段落,这里稍微小结一下。
- 首先需要强调的还是整个深度滤波的过程,即:
- 根据两帧的位姿计算当前帧上的极线搜索范围;
- 在要搜算的极线段上通过对比算法获得最佳的匹配位置;
- 通过得到的最佳匹配点计算深度值;
- 把该深度值作为观测进行滤波器的迭代;
- 其次因为步骤2中,算法估计的位姿和相机内参不准确导致极线段会和真实的极线段有位置误差,这部分误差会最终影响到深度或者逆深度的计算,我们称为
;
- 依旧在步骤2中,同时在搜索的过程中,因为两帧图像的光度差异也会导致计算最佳匹配时候产生误差,因而影响深度或逆深度的计算,我们称为
;
- 最后,在步骤4中,如果简单的认为每次的观测都是好的,那么就一直使用Gaussian分布的平均作为最终估计的分布;但是如果考虑了有误匹配的现象,那么可以把这个过程考虑为一个伯努利实验,如果是硬币正面,则更新Guassian分布;如果是硬币反面,则使用均匀分布