LSD-SLAM笔记之DepthMap

LSD-SLAM中深度图构建部分相对比较复杂，在LSD-SLAM论文的3.4节做了简单的陈述，详细的算法在论文Semi-Dense Visual Odometry for a Monocular Camera中进行说明。我们首先看一下代码的入口，建图线程在SlamSystem类的构造函数中启动，入口为函数SlamSystem::mappingThreadLoop

thread_mapping = boost::thread(&SlamSystem::mappingThreadLoop, this);

我们找到SlamSystem::mappingThreadLoop函数，如下：

void SlamSystem::mappingThreadLoop()
{
    printf("Started mapping thread!\n");
    while(keepRunning)
    {
        if (!doMappingIteration())
        {
            boost::unique_lock::mutex> lock(unmappedTrackedFramesMutex);
            unmappedTrackedFramesSignal.timed_wait(lock,boost::posix_time::milliseconds(200));  // slight chance of deadlock otherwise
            lock.unlock();
        }

        newFrameMappedMutex.lock();
        newFrameMappedSignal.notify_all();
        newFrameMappedMutex.unlock();
    }
    printf("Exited mapping thread \n");
}

在SlamSystem::trackFrame函数返回之前，都会有

    unmappedTrackedFramesMutex.lock();
    unmappedTrackedFramesSignal.notify_one();
    unmappedTrackedFramesMutex.unlock();

由此看，正常情况下，每次跟踪一次图像之后，建图线程都会调用一次SlamSystem::doMappingIteration()。该函数就是整个建图线程的主体函数。

1. 深度地图估计

首先介绍一下LSD-SLAM地图构建的原理。LSD-SLAM构建的是半稠密逆深度地图（semi-dense inverse depth map），只对有明显梯度的像素位置进行深度估计，用逆深度表示，并且假设逆深度服从高斯分布。一旦一个图像帧被选为关键帧，则用其跟踪的参考帧的深度图对其进行深度图构建，之后跟踪到该新建关键帧的图像帧都会用来对其深度图进行更新。当然，追溯到第一帧，肯定是没有深度图的，因此第一帧的深度图是有明显梯度区域随机生成的深度。
总的来说，建图线程可以分为两种情况

1. 构建关键帧，则构建新的深度图（Depth Map Creation）
2. 不构建关键帧，则更新参考关键帧的深度图（Depth Map Refinement）

接下来按照论文Semi-Dense Visual Odometry for a Monocular Camera第二章中的逻辑，先介绍深度图的更新（不构建关键帧时），然后讲深度图的传播（构建新的关键帧）

1.1 深度图的更新

在跟踪线程中对当前帧的位姿进行估计后，当前帧就被送到建图线程用于估计其参考关键帧的深度图。论文中称为基于立体匹配的深度更新（Stereo-Based Depth Map Update），通过极线搜索在图像帧中找到与参考关键帧匹配的图像点，然后通过新匹配的观测点对逆深度进行更新。

1.1.1 参考图像帧选取

我们先讨论如何选取与当前关键帧做极线匹配的图像帧。

注意：在论文和代码中都把与关键帧做立体匹配的图像帧称为参考帧（Reference Frame）。

考虑到准确性，立体匹配时尽可能选取视察和观测角小的两帧图像。这个不难理解，通过下图我们可以看到，当两帧图像间的基线过长，则会有很多错误的匹配出现。而考虑到精度，小基线的两个视图对深度进行估计误差往往比较大。协调这两者，论文中使用了一种自适应的方式。

论文中提到一个像素“年龄”的问题，结合代码以及按照我的理解，这里的年龄也就是对应于代码中像素深度类DepthMapPixelHypothesis中的变量nextStereoFrameMinID，也就是和当前关键帧中该像素点进行匹配的图像帧的与该关键帧的最小帧间隔。论文中把距离当前关键帧近的图像帧称为“老”的关键帧，反之那些最近获取的图像帧称为“新”，论文中有一段话：

We use the oldest frame the pixel was observed in, where the disparity search range and the observation angle do not exceed a certain threshold(see Fig. 4). If a disparity search is unsuccessful (i.e., no good match is found), the pixel’s “age” is increased, such that subsequent disparity searches use newer frames where the pixel is likely to be still visible.

这里的意思是如果最“老”的图像帧上的像素满足视差和观测角的要求，则使用最“老”的关键帧；如果极线搜索失败了，增加像素的“年龄”，也就是使用比较新的图像帧来做极线搜索。这样就比较好理解了，在对关键帧进行深度更新的时候都是从距离关键帧最近的图像帧开始进行深度更新，除非有的像素点设置有帧间隔（年龄）则需要从最近的关键帧跳过帧间隔后的较新的图像中开始更新。其实在代码中，只有当搜索的极线段过小时才会增加匹配的帧间隔，也就是这里的像素“年龄”。具体代码将在后续详细分析。

1.1.2 立体匹配策略

为在选取的参考帧（之后默认使用论文中的这个说法）上找到与需要更新深度的当前关键帧上的点的对应点，这里使用到了对极线搜索的方式。在已知两帧图像间的位姿变换，关键帧上的一点对应参考帧上的一条对极线。实际上我们不可能在整条对极线上进行搜索，而是在某一个对极线段上进行搜索。关键帧上的点如果已有逆深度的先验假设 N(d,σ2d) ，则设置的逆深度搜索范围为 d±2σd ，否则在整个范围内搜索。

现在我们可以理解在上一个小节就提到的两帧图像基线太长会找到误匹配的问题。做深度更新的时候基线都是比较短的，并且不考虑旋转，对于一个给定的逆深度搜索范围，两帧图像基线越长，则对应在第二帧图像上投影的极线段的长度也就越长（当然这是在基线较短的假设前提上的）。图像是非凸的，搜索范围过长则会陷入局部最小。

在立体匹配的过程中，论文采用了对极线段上５个采样点计算SSD误差的方式。采用５个点主要的方式很大程度上提高了匹配的效率。由于这５个点是相邻的，在极线段上移动的时候，每次只需要更新一个点的值，这就非常高效了。在立体匹配的误差计算方式上有很多中方法，论文中使用了SSD（Sum of Squared Distance）误差：

ESSD(u)=∑i[I1(xi+u)−I0(xi)]2

1.1.3 不确定性估计

通过立体匹配找到在参考帧上的对应点后，我们可以求得新的逆深度值。除此之外我们还需要求解逆深度的不确定度。通过对两图像 I0 ， I1 立体匹配求得的最佳匹配点对应的逆深度可以表示为：

d∗=d(I0,I1,ξ,π)(1)

这里的 ξ 为两帧间位姿变换， π 为相机投影模型参数。从而我们可以求得 d∗ 的误差方差（error-variance）为

σd=JdΣJTd(2)

这里的 Jd 是 d 的雅克比， Σ 是输入误差的方差（这个公式的推导参考了书 Multivariate error analysis，但是这本书网上找不到，但是在 Wiki上可以找到说明，证明可以见 Probability Models in Engineering and Science第四章）。

考虑到在计算逆深度主要分为３个步骤：
1. 计算在参考帧中的对极线
2. 在对极线上找到最好的匹配位置 λ∗∈R （视差）
3. 通过 λ∗ 求出逆深度 d∗

这三个步骤分别涉及三个误差：几何视差误差，由 ξ 和 π 中的噪声将影响第１步对极线的位置，从而导致匹配点位置的误差；光度视差误差，在图像 I0 和 I1 上的噪声将影响第２步匹配位置的求取；逆深度计算误差，逆深度误差主要来源与两个地方，一个是匹配的像素位置，这个就和前两个误差有关，另外就是两个图像间的基线长度。接下来将对这几个误差如何建模进行分析。

a. 几何视差误差

论文中称之为几何视差误差（Geometric disparity error），记为 ϵλ 。就是由极线位置误差导致视差求取出现的误差，体现在参考帧中匹配的点在极线的位置上，也就是 λ∗ 上。几何视差误差是由 ξ 和 π 中的噪声引起的，理论上我们可以计算准确的 ξ 和 π 中噪声的方差，但是论文中指出这样增加计算的复杂度但是没有等价地提升准确性，因此论文中使用了一个简单的近似。定义极线段：

L:={ l0+λ(lxly)∣λ∈S}(3)

这里的 λ 是在搜索范围 S 内的视差， (lx,ly)T 是归一化的极线方向， l0 是极线上对应无穷远点的图像点。论文中指出，当搜索的极线段比较小，并且旋转误差较小的情况下，这个近似还是比较好的。

这里为了方便说明，另外把极线位置误差记作 ϵl ，是各向同性的高斯噪声，其实 ξ 和 π 中的噪声直接影响的是极线位置误差，而几何视差误差是取决于极线位置误差。论文中有一图说明了这个问题：

可以看出，极线位置误差使得极线上的点 lλ 落在以 ϵl 为半径的圆上（图中蓝色虚线圆形），由于 (3) 中的定义，噪声都在点 l0 上，根据这个假设，极线的方向是不会变的。在立体匹配的时候，必然会找到与 lλ 灰度相同点的位置，也就是落在 lλ 的灰度等值线上（图中黑色虚线）。由于等值线和图像梯度是垂直的，我们可以看到当极线位置误差 ϵl 一定，极线方向与梯度方向夹角越大则几何视差误差越大。我们要求的点就在极线 L 和等值线的交点上，假设梯度局部不变的，则有：

l0+λ(lxly)＝!g0+γ(−gygx)γ∈R(4)

这里的 g0 为等值线上的一点，梯度 g:=(gx,gy) ，这里假设了 g0 和梯度 g 是不收噪声影响，对 (4) 两边同乘以梯度 g 并且整理后可得：

λ∗(l0)=⟨g,g0−l0⟩⟨g,l⟩(5)

根据 (2) 式，我们可以得到几何视差误差的方差：

σ2λ(ξ,π)=Jλ∗(l0)(σ2l00σ2l)JTλ∗(l0)=σ2l⟨g,l⟩2(6)

这里的 g 为归一化的图像梯度， l 为归一化的极线向量， σ２l 是误差 ϵl 的方差。其实这个式子有点问题，根据 (5) 式，有：

Jλ∗(l0)=∂λ∗(l0)∂l0=−gT⟨g,l⟩

因此 (6) 式应改为：

σ2λ(ξ,π)=⟨g,g⟩⋅σ2l⟨g,l⟩2(6*)

需要注意的是，这里的 极线位置误差 ϵl 的方差 σ２l 只和相机的位姿 ξ （这里需要提一下，这个变量论文中一直称为 relative camera orientation，难道这里的误差之和旋转有关系？）和相机参数 π 有关， 和图像的灰度噪声没有关系。论文中没有提及这里的 σ２l 是怎么计算的，可能是考虑到精确建模也太过麻烦，这里的 ξ 和 π 的误差实际上在跟踪环节中体现在光度残差上了。因此在代码实现时， σ２l 是和参考帧跟踪关键帧计算位姿时所计算的图像平均残差成一个简单的线性关系，具体在代码剖析部分说明。

b. 光度视差误差

在极线搜索的时候，我们找到的点满足SSD误差最小，也就有：

λ∗＝minλ(iref−Ip(λ))2(7)

这里的 iref 是关键帧的灰度（论文中称参考帧的灰度，好乱啊！ 我一律以代码中的命名为标准）， Ip(λ) 是参考帧图像（对！按照代码中的来，极线搜索是在参考帧中进行的）极线在视差为 λ 位置的灰度。假设在极线上有过彻底的搜索，得到一个很好的初始位置