关于LSD SLAM的代码解析已经有人详细的介绍过了[1],这篇博客主要是自己总结一下代码中自己看的一些思路,免得时间长了就忘记了。
首先放上LSD SLAM整个算法框架:
在LSD SLAM的mapping中,会首先判断当前frame是不是keyframe,判断的标准就是离上一帧keyframe的baseline是不是超过了一定的阈值,如果超过了阈值,那么就会把这current frame作为keyframe,反之就需要refine 当前的keyframe的深度值。
在mapping部分中,有几个主要的文件,分别是DepthMap.cpp,DepthMap.h,DepthMapPixelHypothesis.cpp,DepthMapPixelHypothesis.h。如下图所示:
这四个文件中一个有两个类(class):
(1)class DepthMapPixelHypothesis:
主要描述的是深度图上的像素点的一些信息:是否有效、在不在黑名单中、要逃过的最小帧数、被有效观测的次数、像素点的逆深度值、像素点的你深度值的方差、以及平滑过的逆深度值和方差。
(2)class DepthMap:
主要是这个类里面的成员函数实现了mapping部分的功能,主要有:
a、初始化函数:
void initializeFromGTDepth(Frame* new_frame);
void initializeRandomly(Frame* new_frame);
LSD SLAM一般选择的是随机初始化像素的深度值,初始化的时候分配一个随机化的均值和一个较大的方差。(假设深度值是服从高斯分布的,所以只需要用一个均值和一个方差来表示就好),代码为:
for(int y=1;y<height-1;y++)
{
for(int x=1;x<width-1;x++)
{
if(maxGradients[x+y*width] > MIN_ABS_GRAD_CREATE)
{
float idepth = 0.5f + 1.0f * ((rand() % 100001) / 100000.0f);
currentDepthMap[x+y*width] = DepthMapPixelHypothesis(
idepth,
idepth,
VAR_RANDOM_INIT_INITIAL,
VAR_RANDOM_INIT_INITIAL,
20);
}
else
{
currentDepthMap[x+y*width].isValid = false;
currentDepthMap[x+y*width].blacklisted = 0;
}
}
}
b、updateKeyframe()函数
当前帧不被选为keyframe的时候,调用updateKeyframe()来refine当前keyframe与current frame有overlap点的深度值。具体做法是:
输入:keyframe的一个指针队列,从这个队列中找到与current frame有足够overlap的keyframe。找到之后,调用observeDepth()函数来计算current frame的深度值:
- -有深度值就利用observeDepthUpdate()来更新深度;
- -如果没有深度就用observeDepthCreate()产生一个新的深度值。
observeDepthUpdate()函数中,会调用makeAndCheckEPL()去计算极线,然后用doLineStereo()函数来计算极线匹配的误差。
误差满足要求则开始深度融合,高斯分布加上一个均匀分布来融合,融合的代码是:
在observedepthcreate()函数中,没有融合这一部分,是直接利用三角化求出深度即可,核心代码是:
c、propagateDepth()函数
如果当前帧被判定为keyframe,那么需要从把一个keyframe上的点投影到这一帧上,并且对相同的3d点的深度进行融合,这里就是进行的高斯融合,对应代码为:
参考文献:
[1] https://www.zybuluo.com/lancelot-vim/note/412293
[2] Engel, Jakob, Thomas Schöps, and Daniel Cremers. “LSD-SLAM: Large-scale direct monocular SLAM.” European Conference on Computer Vision. Springer, Cham, 2014.
[3] https://github.com/tum-vision/lsd_slam