LSD_SLAM是基于高灰度梯度的半稠密直接法视觉里程计,在CPU上可以实时运行。
主要参考博客:LSDSLAM算法解析
-
Tracking部分
用当前帧和参考关键帧做最小化光度误差,利用高斯牛顿法+huber核函数,在李代数空间下求解相机位姿。
公式中点p是把所有当前帧跟踪到,并且深度估计收敛的点加进去,每个深度点的协方差是由图像高斯噪声和逆深度不确定性共同影响的。式(1)中的分母是由逆深度的协方差传递过来的。
- 关键帧的选取
关键帧的选择是根据当前帧和上一个关键帧之间的坐标变换是否大于一定距离阈值决定的。阈值是由场景平均深度决定的。
-
Depth Map estimation
主要分为3步:
第一步:选择一些“好的”像素点
第二步:为每个像素自适应选择合适的参考帧
第三步:在参考帧的极线上进行立体匹配(采用SSD),并计算不确定度
第四步:深度观测融合
- 第一步,通过建模对逆深度的不确定度进行估计,以此挑选出好的像素点。极线搜索匹配过程中有三个方面的不确定度。
1)几何视差误差,由位姿和相机内参中的噪声将影响第1步极线的位置,从而导致匹配点位置的误差;
- 位姿扰动造成的误差σ2越大,则几何视差误差越大
- 梯度g和极线l的夹角越小(内积越大),几何视差误差越小
2)光度视差误差,在图像I0和I1上的噪声将影响第2步匹配位置的求取
- 图像噪声越大,光度视差误差越大
- 极线方向上梯度越大,光度视差误差越小
3)逆深度计算误差,逆深度误差除了与上述两个误差有关外,还与基线、像素位置等有关。接下来将对这几个误差一一建模分析。其中 阿尔法,表示的是极线上一个像素点的改变会造成多大深度的变化,括号里的是前两步获得的不确定性(匹配点位置)的和。通过阿尔法将位置的不确定性传递到深度的不确定性,阿尔法的求解看:LSD-SLAM笔记之DepthMap
总结一下:几何视差误差与梯度方向有关,光度视差误差与梯度大小有关,权重α与基线长度和 像素位置等有关。这三个因素共同影响立体观测精度,我们可以据此只把好的像素的观测结果保留,并利用这计算的方差进行地图的更新
- 第二步,选择合适的参考帧
- 小基线,准确度accuracy高,精度precision低
- 大基线,准确度accuracy低(有局部最小值,易误匹配),精度precision高
在当前帧中第一步已经挑选好了一些“好的”像素点,这就证明这些像素点是有深度的,因为跟踪到的点都是有深度的。因为小基线,准确度accuracy高,精度precision低。而大基线,准确度accuracy低(有局部最小值,易误匹配),精度precision高的特点,先在能够看到点的最远的帧进行寻找,找不到就找再进一点的帧,直到所有“好”的点都找到合适的关键帧。这时就计算深度,更新关键帧地图(就是地图中的点)。
立体匹配是在当前帧(current frame)和参考帧(reference frame)之间进行的,而不是关键帧(key frame),不过关键帧也可以作为一部分像素的参考帧。
- 第三步 立体匹配使用SSD
- 深度观测融合用卡尔曼滤波融合
融合的时候,先是判断1.不确定度如果过大则舍弃掉。2.判断两个逆深度的差值如果小于2倍标准差,则证明融合有效。3.
-
深度图的传播
若把当前帧判断为关键帧,则把上一个关键帧中的地图点全部投影到当前帧上,并且也要对不确定度进行更新。同时遍历当前帧的每一个地图点,看其在图像平面的梯度是否很大,若是则保留,不是则剔除掉。
-
MAP Optimization
主要是在闭环检测和全局优化的时候使用,主要解决尺度问题。下面的帧都是关键帧。 优化的时候将关键帧的深度图逆深度进行归一化(这时传递过来的像素深度会改变),光度偏差和深度偏差加到一起,用sim3,进行高斯牛顿优化。
下面看一下回环检测部分。由于论文中采用的是直接法,虽然代码中有fabmap检测闭环的部分,但是其默认检测闭环的方式是帧与帧之间做双向跟踪(Reciprocal tracking check)。
在当前帧的附近寻找回环候选关键帧,之后候选帧与当前帧进行双向sim3跟踪,如果两个马氏距离都足够小,则回环成功,在位姿图中加入边。其中回环在对齐时用到 高效二阶最小化方法(Efficient Second Order Minimization,ESM) 和 从粗到细(Coarse-to-Fine)方法,也就是图像金字塔的方式
--------------------------------------以上为单目的LSD_SLAM(IJRR),双目的LSD_SLAM(ECCV)以后补充--------------------------