lsd-slam源码解读第五篇:DepthEstimation

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(nextIndex < maxIndex)
            {