lsd-slam源码解读第五篇:DepthEstimation
标签 : lsd-slam
同样的,我希望在看这篇之前,读者已经对算法有了一定的了解,否则我估计这篇博客看起来会很吃力,如果还不太了解算法,建议先看第三篇:lsd-slam源码解读第三篇:算法解析 深度估计是整个lsd-slam最核心的部分,它和orb-slam在对深度的处理上有极大的不同,主要体现在:
orb-slam直接使用了三角化,计算得到深度,之后再进行校准
lsd-slam的方案是,初始化一个很不精确的深度(由于假设深度服从了高斯分布,因此可以选定一个均值,然后初始化一个极大的方差),当然,如果有些先验信息,这个分布可以选的比较好,可以注意到论文的深度传播部分,实际上就是根据先验知识初始化一个深度分布,之后根据观测帧,对深度分布进行修正的一个方法
DepthMapPixelHypothesis
我想一个科学的研究方式应该是先阅读这个类,从名字上来看,这个类是对于深度图上每个像素的一个深度估计,它只有公有成员,成员变量分别表达了
- 该像素是否有效
- 该像素是否列入黑名单
- 要逃过的最小帧数
- 有效的观测次数
- 深度均值
- 深度方差
- 平滑后的深度均值
- 平滑后的深度方差
接下来是3个构造函数,最后为了可视化,设置了一个返回rgb三个值的向量
DepthMap
这是一个很核心的类,首先我们来看构造函数DepthMap::DepthMap(int w, int h, const Eigen::Matrix3f& K)
构造函数需要传入图像的宽度以及高度,还有相机的内参,首先显然是分配内存,并且本地化相机参数
width = w;
height = h;
activeKeyFrame = 0;
activeKeyFrameIsReactivated = false;
otherDepthMap = new DepthMapPixelHypothesis[width*height];
currentDepthMap = new DepthMapPixelHypothesis[width*height];
validityIntegralBuffer = (int*)Eigen::internal::aligned_malloc(width*height*sizeof(int));
debugImageHypothesisHandling = cv::Mat(h,w, CV_8UC3);
debugImageHypothesisPropagation = cv::Mat(h,w, CV_8UC3);
debugImageStereoLines = cv::Mat(h,w, CV_8UC3);
debugImageDepth = cv::Mat(h,w, CV_8UC3);
this->K = K;
fx = K(0,0);
fy = K(1,1);
cx = K(0,2);
cy = K(1,2);
KInv = K.inverse();
fxi = KInv(0,0);
fyi = KInv(1,1);
cxi = KInv(0,2);
cyi = KInv(1,2);
然后调用reset()函数,将该像素的深度估计初始化为无效
void DepthMap::reset()
{
for(DepthMapPixelHypothesis* pt = otherDepthMap+width*height-1; pt >= otherDepthMap; pt--)
pt->isValid = false;
for(DepthMapPixelHypothesis* pt = currentDepthMap+width*height-1; pt >= currentDepthMap; pt--)
pt->isValid = false;
}
最后初始化一些其他参数,比如与计时相关的,计数相关的参数等
msUpdate = msCreate = msFinalize = 0;
msObserve = msRegularize = msPropagate = msFillHoles = msSetDepth = 0;
gettimeofday(&lastHzUpdate, NULL);
nUpdate = nCreate = nFinalize = 0;
nObserve = nRegularize = nPropagate = nFillHoles = nSetDepth = 0;
nAvgUpdate = nAvgCreate = nAvgFinalize = 0;
nAvgObserve = nAvgRegularize = nAvgPropagate = nAvgFillHoles = nAvgSetDepth = 0;
void DepthMap::updateKeyframe
这个函数需要传入参考帧的指针队列,根据参考帧更新当前关键帧,是尤其重要的一个函数
首先记录最”年轻”的参考帧与最”老”的参考帧,对应算法的这个部分
oldest_referenceFrame = referenceFrames.front().get();
newest_referenceFrame = referenceFrames.back().get();
referenceFrameByID.clear();
referenceFrameByID_offset = oldest_referenceFrame->id();
然后遍历所有帧,判断参考帧是不是当前关键帧,如果不是,就转换到当前关键帧,并且调用帧里面的prepareForStereoWith函数,算出需要用的投影矩阵,之后把帧这些准备好的帧压入参考帧队列,并初始化计数器
for(std::shared_ptr<Frame> frame : referenceFrames)
{
assert(frame->hasTrackingParent());
if(frame->getTrackingParent() != activeKeyFrame)
{
printf("WARNING: updating frame %d with %d, which was tracked on a different frame (%d).\nWhile this should work, it is not recommended.",
activeKeyFrame->id(), frame->id(),
frame->getTrackingParent()->id());
}
Sim3 refToKf;
if(frame->pose->trackingParent->frameID == activeKeyFrame->id())
refToKf = frame->pose->thisToParent_raw;
else
refToKf = activeKeyFrame->getScaledCamToWorld().inverse() * frame->getScaledCamToWorld();
frame->prepareForStereoWith(activeKeyFrame, refToKf, K, 0);
while((int)referenceFrameByID.size() + referenceFrameByID_offset <= frame->id())
referenceFrameByID.push_back(frame.get());
}
resetCounters();
之后是调用观测函数,这个函数实际上要向下调用threadReducer对象的reduce,然后通过boost的bind调用到了observeDepthRow函数,这无疑是一段很飘逸的代码,让我们来好好看下是如何实现的i
首先是记录时间并调用observeDepth(),完成之后记录消耗时间,并记录下本次观测
gettimeofday(&tv_start, NULL);
observeDepth();
gettimeofday(&tv_end, NULL);
msObserve = 0.9*msObserve + 0.1*((tv_end.tv_sec-tv_start.tv_sec)*1000.0f + (tv_end.tv_usec-tv_start.tv_usec)/1000.0f);
nObserve++;
observeDepth()函数实际上就一行有效代码,即
threadReducer.reduce(boost::bind(&DepthMap::observeDepthRow, this, _1, _2, _3), 3, height-3, 10);
这个函数是通过IndexThreadReduce的对象threadReducer,调用方法reduce,传入了一个函数对象(或者叫做仿函数)
this->observeDepthRow(int, int, RunningStats* ), 以及三个参数3, height-3,10
IndexThreadReduce::reduce
这个函数要求的传入参数是一个函数对象,三个int类型的整数,这个函数对象需要三个参数,分别是int,int,RunningStats*,返回值为void,这四个参数正好对应了this->observeDepthRow(int, int, RunningStats* ),3, height-3,10,
我想你读到这里,应该能够深刻体会到bind的强大之处,函数指针void()(int,int,RunningStats)和函数指针void DepthMap::observeDepthRow(int yMin, int yMax, RunningStats* stats)类型是不同的,实际如果只是简单使用一下函数指针传递,编译是无法通过的,但是bind内部维护了这个转化,让你能够轻松地使用类里面的函数
由于slam肯定使用多线程,传入的stepSize==10,所以我们可以直接跳过前面几行,进入到互斥锁,之后的操作是本地化参数
this->callPerIndex = callPerIndex;
nextIndex = first;
maxIndex = end;
this->stepSize = stepSize;
之后开始工作线程
// go worker threads!
for(int i=0;i<MAPPING_THREADS;i++)
isDone[i] = false;
// let them start!
todo_signal.notify_all();
然后运行线程,等待结束
//printf("reduce waiting for threads to finish\n");
// wait for all worker threads to signal they are done.
while(true)
{
// wait for at least one to finish
done_signal.wait(lock);
//printf("thread finished!\n");
// check if actually all are finished.
bool allDone = true;
for(int i=0;i<MAPPING_THREADS;i++)
allDone = allDone && isDone[i];
// all are finished! exit.
if(allDone)
break;
}
最后再还原线程相关参数
nextIndex = 0;
maxIndex = 0;
this->callPerIndex = boost::bind(&IndexThreadReduce::callPerIndexDefault, this, _1, _2, _3);
至于这个调用是咋调用的呢,看到这里,你应该说我晕啊,这个该死的程序咋嵌套这么麻烦。。。实际上是在下面这个地方,把他做成并行计算了
void workerLoop(int idx)
{
boost::unique_lock<boost::mutex> lock(exMutex);
while(running)
{
// try to get something to do.
int todo = 0;
bool gotSomething = false;
if(next