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<boost::mutex> lock(unmappedTrackedFramesMutex);unmappedTrackedFramesSignal.timed_wait(lock,boost::posix_time::milliseconds(200));  // slight chance of deadlock otherwiselock.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），只对有明显梯度的像素位置进行深度估计，用逆深度表示，并且假设逆深度服从高斯分布。一旦一个图像帧被选为关键帧，则用其跟踪的参考帧的深度图对其进行深度图构建，之后跟踪到该新建关键帧的图像帧都会用来对其深度图进行更新。当然，追溯到第一帧，肯定是没有深度图的，因此第一帧的深度图是有明显梯度区域随机生成的深度。
总的来说，建图线程可以分为两种情况

构建关键帧，则构建新的深度图（Depth Map Creation）
不构建关键帧，则更新参考关键帧的深度图（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)N(d,\sigma_d^2)，则设置的逆深度搜索范围为d±2σdd{\pm}2\sigma_d，否则在整个范围内搜索。

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

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

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

E_{SSD}(u)=\sum_{i}[I_1(x_i+u)-I_0(x_i)]^2

1.1.3 不确定性估计

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

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

d^*=d(I_0,I_1,\xi,\pi)\tag{1}
这里的 ξ\xi为两帧间位姿变换， π\pi为相机投影模型参数。从而我们可以求得 d∗d^*的误差方差（error-variance）为

σd=JdΣJTd(2)

\sigma_d=J_d{\Sigma}J_d^T\tag{2}
这里的 JdJ_d是 dd的雅克比，Σ\Sigma是输入误差的方差（这个公式的推导参考了书 Multivariate error analysis，但是这本书网上找不到，但是在 Wiki上可以找到说明，证明可以见 Probability Models in Engineering and Science第四章）。

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

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

a. 几何视差误差

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

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

L:=\begin{Bmatrix}l_0+\lambda\begin{pmatrix}l_x\\l_y\end{pmatrix}\mid\lambda\in S\end{Bmatrix}\tag{3}
这里的 λ\lambda是在搜索范围 SS内的视差，(lx,ly)T(l_x,l_y)^T是归一化的极线方向， l0l_0是极线上对应无穷远点的图像点。论文中指出，当搜索的极线段比较小，并且旋转误差较小的情况下，这个近似还是比较好的。

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

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

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

l_0+\lambda\begin{pmatrix}l_x\\l_y\end{pmatrix}\stackrel{!}＝g_0+\gamma\begin{pmatrix}-g_y\\g_x\end{pmatrix} \quad\quad \gamma\in\mathbb{R}\tag{4}
这里的g0g_0为等值线上的一点，梯度g:=(gx,gy)g:=(g_x,g_y)，这里假设了g0g_0和梯度gg是不收噪声影响，对(4)(4)两边同乘以梯度gg并且整理后可得：

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

\lambda^*(l_0)=\frac{\langle g,g_0-l_0 \rangle}{\langle g,l \rangle}\tag{5}
根据(2)(2)式，我们可以得到几何视差误差的方差：

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

\sigma_{\lambda(\xi,\pi)}^2=J_{\lambda^*(l_0)}\begin{pmatrix}\sigma_l^2&0\\0&\sigma_l^2\end{pmatrix}J_{\lambda^*(l_0)}^T=\frac{\sigma_l^2}{\langle g,l\rangle^2}\tag{6}
这里的 gg为归一化的图像梯度，ll为归一化的极线向量， σ２l\sigma_l^２是误差 ϵl\epsilon_l的方差。其实这个式子有点问题，根据 (5)(5)式，有：

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

J_{\lambda^*(l_0)}=\frac{\partial\lambda^*(l_0)}{\partial l_0}=-\frac{g^T}{\langle g,l \rangle}
因此 (6)(6)式应改为：

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

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

b. 光度视差误差

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

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

\lambda^*＝\min_{\lambda}(i_{ref}-I_p(\lambda))^2\tag{7}
这里的 irefi_{ref}是关键帧的灰度（论文中称参考帧的灰度，好乱啊！ 我一律以代码中的命名为标准）， Ip(λ)I_p(\lambda)是参考帧图像（对！按照代码中的来，极线搜索是在参考帧中进行的）极线在视差为 λ\lambda位置的灰度。假设在极线上有过彻底的搜索，得到一个很好的初始位置 λ0\lambda_0，对 (7)(7)式子中的 Ip(λ)I_p(\lambda)做一阶泰勒展开，然后对增量 Δλ\Delta\lambda求导并令式子为零，则可解出

λ∗(I)=λ0+Δλ=λ0+(iref−Ip(λ0))g−1p(8)

\lambda^*(I)=\lambda_0+\Delta\lambda=\lambda_0+(i_{ref}-I_p(\lambda_0))g_p^{-1}\tag{8}
这里的 g−1pg_p^{-1}是图像 IpI_p极线上的梯度，因此是一维的。这里同样不考虑梯度的噪声，只考虑两个图像的噪声。同样，我们有：

σ2λ(I)=Jλ∗(I)(σ2i00σ2i)JTλ∗(I)=2σ2ig2p(9)

\sigma_{\lambda(I)}^2=J_{\lambda^*(I)}\begin{pmatrix}\sigma_i^2&0\\0&\sigma_i^2\end{pmatrix}J_{\lambda^*(I)}^T=\frac{2\sigma_i^2}{g_p^2}\tag{9}
这里由于同时对关键帧和参考帧的灰度都计算了噪声，因此会有 ２这个系数，这个方程是一个线性方程，我们也可以这么求：

σ2λ(I)=Var(λ∗(I))=(Var(iref)+Var(Ip))g−2p=2σ2ig2p

\sigma_{\lambda(I)}^2=\text{Var}(\lambda^*(I))=(\text{Var}(i_{ref})+\text{Var}(I_p))g_p^{-2}=\frac{2\sigma_i^2}{g_p^2}
这里的 σ2i\sigma_i^2是图像的高斯噪声的方差，这里的 光度视差误差和 几何光度误差是独立的。我们可以看下图，当图像的亮度误差（高斯噪声） ϵi\epsilon_i一定的时候，图像的梯度越小， 光度视差误差就越大。因此在做立体匹配的时候，尽量使用梯度明显的像素。

c. 逆深度计算误差

论文中指出，当旋转比较小的时候，逆深度dd和视差λ\lambda近似成一个比例关系，所以构造的逆深度的观测方差为：

σ2d,obs=α2(σ2λ(ξ,π)+σ2λ(I))(10)

\sigma_{d,\text{obs}}^2=\alpha^2\begin{pmatrix}\sigma_{\lambda(\xi,\pi)}^2+\sigma_{\lambda(I)}^2\end{pmatrix}\tag{10}
这个模型不难理解，逆深度主要和两个因素有关，一个是像素的位置，一个是两图像的基线长度（体现在极线段的长度上）。像素位置噪声就是前两个噪声的和，作者把这里的 α\alpha定义为：

α:=∂d∂λ(11)

\alpha:=\frac{\partial{d}}{\partial{\lambda}}\tag{11}
这里就是逆深度搜索范围与极线段搜索范围的比例，像素的大小是固定的，如果一个像素大小对应了很大的逆深度范围，这样求得的逆深度误差必然会很大。
考虑到在极线上搜索匹配点的时候，是使用了多个点，因此实际上这里是给出了逆深度误差的上限：

σ2d,obs≤α2(min{σ2λ(ξ,π)}+min{σ2λ(I)})(12)

\sigma_{d,\text{obs}}^2\le\alpha^2\begin{pmatrix}\text{min}\{\sigma_{\lambda(\xi,\pi)}^2\}+\text{min}\{\sigma_{\lambda(I)}^2\}\end{pmatrix}\tag{12}
当然，这里涉及到 α\alpha到底该怎么求的问题。在阅读代码的时候，我一开始也没看明白，后来参考了范帝恺的LSD-SLAM的博客，这里把这部分公式推到一下。
首先，我们可以求得两帧图像对应点在归一化平面上的点：
p_k=\begin{pmatrix}x_k\\y_k\\1\end{pmatrix} \quad\quad p_r=\begin{pmatrix}x_r\\y_r\\1\end{pmatrix}=K^{-1}\begin{pmatrix}u_r\\v_r\\1\end{pmatrix}

pk=⎛⎝⎜xkyk1⎞⎠⎟pr=⎛⎝⎜xryr1⎞⎠⎟=K−1⎛⎝⎜urvr1⎞⎠⎟

p_k=\begin{pmatrix}x_k\\y_k\\1\end{pmatrix} \quad\quad p_r=\begin{pmatrix}x_r\\y_r\\1\end{pmatrix}=K^{-1}\begin{pmatrix}u_r\\v_r\\1\end{pmatrix}
根据两帧间的变换，有
p_r=R{\cdot}p_k{\cdot}d^{-1}+t=\begin{pmatrix}r_0\\r_1\\r_2\end{pmatrix}p_kd^{-1}+\begin{pmatrix}t_x\\t_y\\t_z\end{pmatrix}=\begin{pmatrix}r_0{\cdot}p_k{\cdot}d^{-1}+t_x\\ r_1{\cdot}p_k{\cdot}d^{-1}+t_y\\ r_2{\cdot}p_k{\cdot}d^{-1}+t_z\end{pmatrix}

pr=R⋅pk⋅d−1+t=⎛⎝⎜r0r1r2⎞⎠⎟pkd−1+⎛⎝⎜txtytz⎞⎠⎟=⎛⎝⎜⎜r0⋅pk⋅d−1+txr1⋅pk⋅d−1+tyr2⋅pk⋅d−1+tz⎞⎠⎟⎟

p_r=R{\cdot}p_k{\cdot}d^{-1}+t=\begin{pmatrix}r_0\\r_1\\r_2\end{pmatrix}p_kd^{-1}+\begin{pmatrix}t_x\\t_y\\t_z\end{pmatrix}=\begin{pmatrix}r_0{\cdot}p_k{\cdot}d^{-1}+t_x\\ r_1{\cdot}p_k{\cdot}d^{-1}+t_y\\ r_2{\cdot}p_k{\cdot}d^{-1}+t_z\end{pmatrix}
由于差一个尺度因子，我们现在相当得到两个方程。现在我们只需要解一个未知数 d dd，至少一个方程就可以。作者为了减少误差，在图像uuu， v vv方向选取了较长的那个方向的方程来解。这里以uu方向为例：

xr=r0⋅pk⋅d−1+txr2⋅pk⋅d−1+tz

x_r=\frac{r_0{\cdot}p_k{\cdot}d^{-1}+t_x}{r_2{\cdot}p_k{\cdot}d^{-1}+t_z}
解出 d−1d^{-1}有：

d=r2pk⋅xr−r0pktx−tz⋅xr

d=\frac{r_2p_k{\cdot}x_r-r_0p_k}{t_x-t_z{\cdot}x_r}
这样我们就可以求出 α\alpha:

α:=∂d∂λ=∂d∂x∂x∂u∂u∂λ=r2pk⋅tx−r0pk⋅tz(tx−tz⋅x)2⋅finvx⋅∂u∂λ(13)

\alpha:=\frac{\partial{d}}{\partial{\lambda}}=\frac{\partial{d}}{\partial{x}}\frac{\partial{x}}{\partial{u}}\frac{\partial{u}}{\partial{\lambda}}=\frac{r_2p_k{\cdot}t_x-r_0p_k{\cdot}t_z}{(t_x-t_z{\cdot}x)^2}{\cdot}f_{x}^{inv}{\cdot}\frac{\partial{u}}{\partial{\lambda}}\tag{13}
这里的 finvxf_x^{inv}是矩阵 K−1K^{-1}的第一个元素， rir_i为旋转矩阵 RR第ii行的行向量， ∂u∂λ\frac{\partial{u}}{\partial{\lambda}}表示了极线段在 uu方向上投影与极线段长度的比例。到此，误差分析都已经结束，具体实现在讲解代码的时候会详细说明。

1.1.4 深度观测融合

通过当前帧匹配的像素为深度提供一个新的观测值，然后就可以把当前观测的深度融合到关键帧的深度地图中去。这里有两种情况：当对应像素点没有深度先验时则由新的观测值构建新的先验；当已经有先验值的话，则把新观测值融合到先验中去。在这个融合的过程，使用了两个高斯分布乘法的方式：对于给定先验N(dp,σ2p)\mathcal{N}(d_p,\sigma_p^2)以及有噪声的观测值 N(do,σ2o)\mathcal{N}(d_o,\sigma_o^2)，给出后验估计：

N(σ2pdo+σ2odpσ2p+σ2o,σ2pσ2oσ2p+σ2o)(14)

\mathcal{N}\begin{pmatrix}\frac{\sigma_p^2 d_o + \sigma_o^2d_p}{\sigma_p^2 + \sigma_o^2}, \frac{\sigma_p^2\sigma_o^2}{\sigma_p^2 + \sigma_o^2}\end{pmatrix}\tag{14}

1.1.5 观测不确定性的总结

论文中不是使用一般的块匹配方式，而是用点匹配方式（其实是线上的连续５点）。沿着极线移动，找到SSD误差最小的位置。上面给出了三个主要的误差评估的参数

几何视差误差（方差）σ2λ(ξ,π)\sigma_{\lambda(\xi,\pi)}^2，取决于梯度与极线的夹角以及极线位置误差方差σ2l\sigma_l^2（这个由ξ\xi和π\pi的噪声决定）。
光度视差误差（方差）σ2λ(I)\sigma^2_{\lambda(I)}，取决于极线上图像梯度的大小
像素逆深度系数α\alpha，取决于相机的相对位姿ξ\xi、焦距以及立体匹配得到的像素点的位置

1.2 深度图的传播

深度图的传播发生在构建关键帧的时候。在构建新的关键帧时，使用其参考关键帧的深度图来构建当前帧的深度图。这里主要考虑构建新的深度图时，逆深度误差的传播。首先假设两帧间的旋转是很小的，逆深度就可以近似为（这里的减号应该是加号才对）：

d1(d0)=(d−10−tz)−1

d_1(d_0)=(d_0^{-1}\color{red}{-}t_z)^{-1}
这里的 tzt_z是相机沿着光轴方向的位移。同样利用 (2)(2)式计算 d1d_1的方差：

σ2d1=Jd1σ2d0JTd1+σ2p=(d1d0)4σ2d0+σ2p(15)

\sigma_{d_1}^2=J_{d_1}\sigma_{d_0}^2J_{d_1}^T+\sigma_p^2=\begin{pmatrix}\frac{d_1}{d_0}\end{pmatrix}^4\sigma_{d_0}^2+\sigma_p^2\tag{15}
这里的 σ2p\sigma_p^2为预测不确定性。

注意：在实际求逆深度的时候，是考虑旋转的，把参考关键帧上的点通过SE3变换到当前新的关键帧上来，然后求逆深度。但是求方差则是用上述近似方法求的。具体实现见函数DepthMap::propagateDepth。

疑问１：论文中讲这里的不确定性是对应(14)(14)融合更新步骤中扩展卡尔曼的预测步骤的，不明白。

在代码中，直接在σ2d1\sigma_{d_1}^2上增加了一点不确定性。论文中提到这样做的好处：降低点位置的漂移。这个角度讲表示不太理解。

一个像素点只能有一个逆深度的先验值，在得到一个新的观测值的时候，根据方差来判断对新的观测值融合或者舍弃。

当两个逆深度观测值的差值小于2σ2\sigma的时候，则认为观测有效，根据(14)(14)融合
否则，舍弃新的观测，并且增加先验逆深度的不确定性（深度图更新过程）；否则，当新估计的深度使得点远离相机，则认为该位置是阻塞的（逆深度假设无效），并且舍弃逆深度信息（逆深度图传播过程）。

接下来开始结合代码进行分析。

2. 代码剖析

接下来我们先看一下函数SlamSystem::doMappingIteration()

bool SlamSystem::doMappingIteration()
{if(currentKeyFrame == 0)return false;if(!doMapping && currentKeyFrame->idxInKeyframes < 0){if(currentKeyFrame->numMappedOnThisTotal >= MIN_NUM_MAPPED)finishCurrentKeyframe();elsediscardCurrentKeyframe();map->invalidate();printf("Finished KF %d as Mapping got disabled!\n",currentKeyFrame->id());changeKeyframe(true, true, 1.0f);}mergeOptimizationOffset();addTimingSamples();if(dumpMap){keyFrameGraph->dumpMap(packagePath+"/save");dumpMap = false;}// set mappingFrameif(trackingIsGood){if(!doMapping){//printf("tryToChange refframe, lastScore %f!\n", lastTrackingClosenessScore);if(lastTrackingClosenessScore > 1)changeKeyframe(true, false, lastTrackingClosenessScore * 0.75);if (displayDepthMap || depthMapScreenshotFlag)debugDisplayDepthMap();return false;}if (createNewKeyFrame){finishCurrentKeyframe();changeKeyframe(false, true, 1.0f);if (displayDepthMap || depthMapScreenshotFlag)debugDisplayDepthMap();}else{bool didSomething = updateKeyframe();if (displayDepthMap || depthMapScreenshotFlag)debugDisplayDepthMap();if(!didSomething)return false;}return true;}else{// invalidate map if it was valid.if(map->isValid()){if(currentKeyFrame->numMappedOnThisTotal >= MIN_NUM_MAPPED)finishCurrentKeyframe();elsediscardCurrentKeyframe();map->invalidate();}// start relocalizer if it isnt running alreadyif(!relocalizer.isRunning)relocalizer.start(keyFrameGraph->keyframesAll);// did we find a frame to relocalize with?if(relocalizer.waitResult(50))takeRelocalizeResult();return true;}
}

当前关键帧变量currentKeyFrame都是通过函数SlamSystem::changeKeyframe来改变的，但还没有加入关键帧集合集合keyFrameGraph->keyframesAll，新构建的关键帧只有通过函数SlamSystem::finishCurrentKeyframe之后才算是真正的关键帧。首先看两个函数：

SlamSystem::finishCurrentKeyframe，每当构造完一个关键帧都会调用，做了填补当前关键帧深度以及平滑深度图的工作，把关键帧设置为函数中就给图像帧设置了在关键帧中的编号idxInKeyframes。以及把当前关键帧currentKeyFrame加入关键帧集合keyFrameGraph->keyframesAll中等工作。
SlamSystem::discardCurrentKeyframe，由于在函数SlamSystem::trackFrame中把每一帧图像都加入了图keyFrameGraph（这个有点像关键帧候选队列），因此该函数主要作用就是把该关键帧直接从keyFrameGraph中剔除。

程序正常运行doMapping是为true的常量，在一开始的if中的内容是不会执行的。接下来主要看这个判断

if(trackingIsGood)

如果跟踪线程跟踪都很好，则进入建图部分，否则进行重定位，重定位讲不在本篇内容中涉及，我们主要看正常跟踪部分

        if (createNewKeyFrame){finishCurrentKeyframe();changeKeyframe(false, true, 1.0f);if (displayDepthMap || depthMapScreenshotFlag)debugDisplayDepthMap();}else{bool didSomething = updateKeyframe();if (displayDepthMap || depthMapScreenshotFlag)debugDisplayDepthMap();if(!didSomething)return false;}

如果在跟踪线程触发了构建新的关键帧，则把当前的关键帧currentKeyFrame处理好，然后把当前帧构建为currentKeyFrame。否则则更新当前帧的深度图。首先我们看更新当前帧深度图的部分。

2.1 SlamSystem::changeKeyframe

这个函数用来改变当前关键帧currentKeyFrame。如果在地图中存在与当前候选关键帧很相似的关键帧，则使用地图中已有的关键帧，否则重新构建关键帧。

void SlamSystem::changeKeyframe(bool noCreate, bool force, float maxScore)
{Frame* newReferenceKF=0;std::shared_ptr<Frame> newKeyframeCandidate = latestTrackedFrame;if(doKFReActivation && SLAMEnabled){struct timeval tv_start, tv_end;gettimeofday(&tv_start, NULL);newReferenceKF = trackableKeyFrameSearch->findRePositionCandidate(newKeyframeCandidate.get(), maxScore);gettimeofday(&tv_end, NULL);msFindReferences = 0.9*msFindReferences + 0.1*((tv_end.tv_sec-tv_start.tv_sec)*1000.0f + (tv_end.tv_usec-tv_start.tv_usec)/1000.0f);nFindReferences++;}if(newReferenceKF != 0)loadNewCurrentKeyframe(newReferenceKF);else{if(force){if(noCreate){trackingIsGood = false;nextRelocIdx = -1;printf("mapping is disabled & moved outside of known map. Starting Relocalizer!\n");}elsecreateNewCurrentKeyframe(newKeyframeCandidate);}}createNewKeyFrame = false;
}

2.2 SlamSystem::updateKeyframe

接下来先看关键帧深度更新的部分。一下是函数的主体部分：

    std::shared_ptr<Frame> reference = nullptr;std::deque< std::shared_ptr<Frame> > references;unmappedTrackedFramesMutex.lock();// remove frames that have a different tracking parent.while(unmappedTrackedFrames.size() > 0 &&(!unmappedTrackedFrames.front()->hasTrackingParent() ||unmappedTrackedFrames.front()->getTrackingParent() != currentKeyFrame.get())){unmappedTrackedFrames.front()->clear_refPixelWasGood();unmappedTrackedFrames.pop_front();}// clone listif(unmappedTrackedFrames.size() > 0){for(unsigned int i=0;i<unmappedTrackedFrames.size(); i++)references.push_back(unmappedTrackedFrames[i]);std::shared_ptr<Frame> popped = unmappedTrackedFrames.front();unmappedTrackedFrames.pop_front();unmappedTrackedFramesMutex.unlock();if(enablePrintDebugInfo && printThreadingInfo)printf("MAPPING %d on %d to %d (%d frames)\n", currentKeyFrame->id(), references.front()->id(), references.back()->id(), (int)references.size());map->updateKeyframe(references);popped->clear_refPixelWasGood();references.clear();}else{unmappedTrackedFramesMutex.unlock();return false;}

变量unmappedTrackedFrames是SlamSystem::trackFrame中使用了，是用来保存图像帧的队列:

if(unmappedTrackedFrames.size() < 50 || (unmappedTrackedFrames.size() < 100 && trackingNewFrame->getTrackingParent()->numMappedOnThisTotal < 10))unmappedTrackedFrames.push_back(trackingNewFrame);

是记录了所有的图像帧。函数一开始的代码的作用是把所有不是跟踪到当前关键帧的图像帧都从队列中剔除了。接下来把跟踪到当前关键帧的图像帧都放在references中（接下里摘出的代码把调试部分代码省去了），并且把最老的图像帧从队列中删除（因为会在本次更新关键帧用来建图，下次就用不着了）：

　　if(unmappedTrackedFrames.size() > 0){for(unsigned int i=0;i<unmappedTrackedFrames.size(); i++)references.push_back(unmappedTrackedFrames[i]);std::shared_ptr<Frame> popped = unmappedTrackedFrames.front();unmappedTrackedFrames.pop_front();unmappedTrackedFramesMutex.unlock();map->updateKeyframe(references);popped->clear_refPixelWasGood();references.clear();}else{unmappedTrackedFramesMutex.unlock();return false;}

2.2.1 DepthMap::updateKeyframe

接下来就是更新地图的函数DepthMap::updateKeyframe，把所有跟踪到当前关键帧的图像帧用于建图。这个函数主要做了如下几件事：

用最近一次观测取更新当前关键帧的深度（observeDepth）
对得到的深度图进行一次填补（regularizeDepthMapFillHoles）
计算平均深度图（regularizeDepthMap）

void DepthMap::updateKeyframe(std::deque< std::shared_ptr<Frame> > referenceFrames)
{assert(isValid());oldest_referenceFrame = referenceFrames.front().get();newest_referenceFrame = referenceFrames.back().get();referenceFrameByID.clear();referenceFrameByID_offset = oldest_referenceFrame->id();for(std::shared_ptr<Frame> frame : referenceFrames){assert(frame->hasTrackingParent());Sim3 refToKf;if(frame->pose->trackingParent->frameID == activeKeyFrame->id())refToKf = frame->pose->thisToParent_raw;elserefToKf = activeKeyFrame->getScaledCamToWorld().inverse() *  frame->getScaledCamToWorld();frame->prepareForStereoWith(activeKeyFrame, refToKf, K, 0);while((int)referenceFrameByID.size() + referenceFrameByID_offset <= frame->id())referenceFrameByID.push_back(frame.get());}observeDepth();//if(rand()%10==0){regularizeDepthMapFillHoles();}regularizeDepthMap(false, VAL_SUM_MIN_FOR_KEEP);// Update depth in keyframeif(!activeKeyFrame->depthHasBeenUpdatedFlag){activeKeyFrame->setDepth(currentDepthMap);}activeKeyFrame->numMappedOnThis++;activeKeyFrame->numMappedOnThisTotal++;
}

程序一开始记录了最老的关键帧和最新的关键帧等变量，这和做立体匹配的参考帧筛选有关。然后所有的参考帧都放入容器referenceFrameByID中，其实这里for循环里面的while完全可以去掉，有没有都对程序没影响。

接下来就是几个类DepthMap中比较重要的函数。

2.2.2 DepthMap::observeDepth

这个函有效的代码就一行

threadReducer.reduce(boost::bind(&DepthMap::observeDepthRow, this, _1, _2, _3), 3, height-3, 10);

这里的threadReducer具体实现不在这里说明，其功能就是使用多线程对当前关键帧的每一行做处理，具体执行代码落到函数DepthMap::observeDepthRow中:

void DepthMap::observeDepthRow(int yMin, int yMax, RunningStats* stats)
{const float* keyFrameMaxGradBuf = activeKeyFrame->maxGradients(0);int successes = 0;for(int y=yMin;y<yMax; y++)for(int x=3;x<width-3;x++){int idx = x+y*width;DepthMapPixelHypothesis* target = currentDepthMap+idx;bool hasHypothesis = target->isValid;// ======== 1. check absolute grad =========if(hasHypothesis && keyFrameMaxGradBuf[idx] < MIN_ABS_GRAD_DECREASE){target->isValid = false;continue;}if(keyFrameMaxGradBuf[idx] < MIN_ABS_GRAD_CREATE || target->blacklisted < MIN_BLACKLIST)continue;bool success;if(!hasHypothesis)success = observeDepthCreate(x, y, idx, stats);elsesuccess = observeDepthUpdate(x, y, idx, keyFrameMaxGradBuf, stats);if(success)successes++;}
}

这个函数就是遍历当前关键对应的深度图，这里像素位置梯度是一个硬性条件，只有当梯度足够大的时候，才能进行之后立体匹配的过程。并且在有逆深度假设的像素点位置（在regularizeDepthMapFillHoles函数中可能会产生这样的像素点），如果梯度不够大则把该逆深度假设设置为无效。

            // ======== 1. check absolute grad =========if(hasHypothesis && keyFrameMaxGradBuf[idx] < MIN_ABS_GRAD_DECREASE){target->isValid = false;continue;}if(keyFrameMaxGradBuf[idx] < MIN_ABS_GRAD_CREATE || target->blacklisted < MIN_BLACKLIST)continue;

接下来就是进行立体匹配：对没有逆深度假设的像素位置进行逆深度先验构建；对有逆深度先验的像素位置进行逆深度更新。

2.2.3 DepthMap::observeDepthCreate

bool DepthMap::observeDepthCreate(const int &x, const int &y, const int &idx, RunningStats* const &stats)
{DepthMapPixelHypothesis* target = currentDepthMap+idx;Frame* refFrame = activeKeyFrameIsReactivated ? newest_referenceFrame : oldest_referenceFrame;if(refFrame->getTrackingParent() == activeKeyFrame){bool* wasGoodDuringTracking = refFrame->refPixelWasGoodNoCreate();if(wasGoodDuringTracking != 0 && !wasGoodDuringTracking[(x >> SE3TRACKING_MIN_LEVEL) + (width >> SE3TRACKING_MIN_LEVEL)*(y >> SE3TRACKING_MIN_LEVEL)]){if(plotStereoImages)debugImageHypothesisHandling.at<cv::Vec3b>(y, x) = cv::Vec3b(255,0,0); // BLUE for SKIPPED NOT GOOD TRACKEDreturn false;}}float epx, epy;bool isGood = makeAndCheckEPL(x, y, refFrame, &epx, &epy, stats);if(!isGood) return false;if(enablePrintDebugInfo) stats->num_observe_create_attempted++;float new_u = x;float new_v = y;float result_idepth, result_var, result_eplLength;float error = doLineStereo(new_u,new_v,epx,epy,0.0f, 1.0f, 1.0f/MIN_DEPTH,refFrame, refFrame->image(0),result_idepth, result_var, result_eplLength, stats);if(error == -3 || error == -2){target->blacklisted--;if(enablePrintDebugInfo) stats->num_observe_blacklisted++;}if(error < 0 || result_var > MAX_VAR)return false;result_idepth = UNZERO(result_idepth);// add hypothesis*target = DepthMapPixelHypothesis(result_idepth,result_var,VALIDITY_COUNTER_INITIAL_OBSERVE);if(plotStereoImages)debugImageHypothesisHandling.at<cv::Vec3b>(y, x) = cv::Vec3b(255,255,255); // white for GOT CREATEDif(enablePrintDebugInfo) stats->num_observe_created++;return true;
}

在代码一开始的地方，获得当前参考帧。由于activeKeyFrameIsReactivated一直是false，所以使用的都是距离关键帧最近的参考帧oldest_referenceFrame。

Frame* refFrame = activeKeyFrameIsReactivated ? newest_referenceFrame : oldest_referenceFrame;

接下来的代码是判断当前像素在跟踪到参考帧时，是否在图像内部。

bool* wasGoodDuringTracking = refFrame->refPixelWasGoodNoCreate()

函数refPixelWasGoodNoCreate()的返回值是在SE3Tracker::calcResidualAndBuffers中赋值的，当当前关键帧跟踪到参考帧时，点落在图像外则设置为false。
这个函数接下来调用的两个函数则是最主要的两个函数，分别是计算归一化的极线向量和做立体匹配，如果成功则为当前的像素位置构建逆深度假设。接下来先分别看一下这两个函数。

2.2.4 DepthMap::makeAndCheckEPL

这个函数的作用是得到当前关键帧上从极点指向待匹配的像素点的归一化极线矢量。首先看一下代码：

bool DepthMap::makeAndCheckEPL(const int x, const int y, const Frame* const ref, float* pepx, float* pepy, RunningStats* const stats)
{int idx = x+y*width;// ======= make epl ========// calculate the plane spanned by the two camera centers and the point (x,y,1)// intersect it with the keyframe's image plane (at depth=1)float epx = - fx * ref->thisToOther_t[0] + ref->thisToOther_t[2]*(x - cx);float epy = - fy * ref->thisToOther_t[1] + ref->thisToOther_t[2]*(y - cy);if(isnanf(epx+epy))return false;// ======== check epl length =========float eplLengthSquared = epx*epx+epy*epy;if(eplLengthSquared < MIN_EPL_LENGTH_SQUARED){if(enablePrintDebugInfo) stats->num_observe_skipped_small_epl++;return false;}// ===== check epl-grad magnitude ======float gx = activeKeyFrameImageData[idx+1] - activeKeyFrameImageData[idx-1];float gy = activeKeyFrameImageData[idx+width] - activeKeyFrameImageData[idx-width];float eplGradSquared = gx * epx + gy * epy;eplGradSquared = eplGradSquared*eplGradSquared / eplLengthSquared;  // square and norm with epl-lengthif(eplGradSquared < MIN_EPL_GRAD_SQUARED){if(enablePrintDebugInfo) stats->num_observe_skipped_small_epl_grad++;return false;}// ===== check epl-grad angle ======if(eplGradSquared / (gx*gx+gy*gy) < MIN_EPL_ANGLE_SQUARED){if(enablePrintDebugInfo) stats->num_observe_skipped_small_epl_angle++;return false;}// ===== DONE - return "normalized" epl =====float fac = GRADIENT_SAMPLE_DIST / sqrt(eplLengthSquared);*pepx = epx * fac;*pepy = epy * fac;return true;
}

首先函数先计算了对极线，是当前关键帧的相机中心在参考帧的投影，也就是对极点和当前关键帧像素
有：

ek=KOr=⎛⎝⎜fx000fy0cxcy1⎞⎠⎟⎛⎝⎜oxoyoz⎞⎠⎟=⎛⎝⎜fxox+cxozfyoy+cyozoz⎞⎠⎟l=u−ek=(xy)−1oz(fxox+cxozfyoy+cyoz)=1oz(xoz−fxox−cxozyoz−fyoy−cyoz)

e_k=KO_r = \begin{pmatrix}f_x&0&cx\\0&f_y&c_y\\0&0&1\end{pmatrix}\begin{pmatrix}o_x\\o_y\\o_z\end{pmatrix} = \begin{pmatrix}f_xo_x+c_xo_z\\ f_yo_y+c_yo_z \\o_z\end{pmatrix}\\ l=u-e_k=\begin{pmatrix}x\\y\end{pmatrix} - \frac{1}{o_z}\begin{pmatrix}f_xo_x+c_xo_z\\ f_yo_y+c_yo_z\end{pmatrix} = \frac{1}{o_z}\begin{pmatrix}xo_z-f_xo_x-c_xo_z\\yo_z-f_yo_y-c_yo_z\end{pmatrix}
这里我们就需要知道参考帧相机中心坐标 OrO_r，如果已知当前关键帧到参考帧的位姿变换 Trk=[Rrk|trk]T_k^r=[R_k^r|t_k^r]那么就有

Or=−Rrk−1trk=tkr

O_r=-{R_k^r}^{-1}t_k^r=t_r^k
也就是说从当前关键帧观测到参考帧的光心就是从参考帧变换到关键帧的位移矢量。于是我们就可以看到这部分求极线的代码：

    // ======= make epl ========// calculate the plane spanned by the two camera centers and the point (x,y,1)// intersect it with the keyframe's image plane (at depth=1)float epx = - fx * ref->thisToOther_t[0] + ref->thisToOther_t[2]*(x - cx);float epy = - fy * ref->thisToOther_t[1] + ref->thisToOther_t[2]*(y - cy);

这里差了一个尺度因子1/oz1/o_z，并且方向是从极点指向待匹配的像素点。接下来看这个判断

    if(isnanf(epx+epy))return false;

什么时候才会出现这样的情况？也就是当参考帧相对于当前关键帧只做与光轴垂直的运动，这样极点就落到无穷远处了。一般这种情况不会发生，在ZZ轴上完全不动不太可能。按照本人的理解，这样的情况就算发生了，也是可以使用的，极线的方向和位移矢量trkt_k^r平行。在当前帧在参考图像帧上对极点与匹配点重合的时候，会出现计算的极线段的长度为nan。往下走看３个限制条件：往下走看３个限制条件：

极线段最小长度
梯度在极线方向投影（平方）的最小长度
梯度和极线的最大夹角

这里先讲极线段最小长度，代码如下

    // ======== check epl length =========float eplLengthSquared = epx*epx+epy*epy;if(eplLengthSquared < MIN_EPL_LENGTH_SQUARED){if(enablePrintDebugInfo) stats->num_observe_skipped_small_epl++;return false;}

显然，根据上面推导的公式，这里的极线少了一个尺度因子1/oz1/o_z，这里的MIN_EPL_LENGTH_SQUARED=1，也就是说实际的极线段长度不因小于1/oz1/o_z，也就是参考帧相对于关键帧在光轴方向的位移。我们考虑视差，忽略旋转的影响，我们看两种情况

沿光轴的位移很小，而与光轴垂直方向位移明显。最极端情况就是上一个判断极线长度无穷大的情况。
沿光轴的位移明显，而与光轴垂直方向位移很小。最极端情况就是极点落在图像中心，越靠近中心极线段越短。

视差小会在第二种情况下出现，因此抽象成如下模型：

这里以当前关键帧为参考系。当前关键帧像素点观测到的点PP，与参考帧构成极线段ll，参考帧的观测则构成极线段l′l'。根据相似三角形可以得到：

lL=fxpzl′L=fxoz+pz

\frac{l}{L}=\frac{fx}{p_z} \quad\quad \frac{l'}{L}=\frac{fx}{o_z+p_z}
这里的 pzp_z为点 PP在当前关键帧下沿着光轴的位移。计算视差得：

Δl=|l−l′|=|Lfxoz(oz+pz)pz|≈|Lfxp2zoz|=|lpzoz|

\Delta l = | l - l'| = | Lf_x\frac{o_z}{(o_z+p_z)p_z}| \approx |\frac{Lf_x}{p_z^2}o_z| = |\frac{l}{p_z}o_z|
参考帧相对于当前关键帧的位移是比较小的，与场景中点相比深度相加可以略去。这里不考虑场景中点的深度，我们给定一个最小视差，则有：

l2≥(pzΔlmin1oz)2⇒(l⋅oz)2≥(pzΔlmin)2

l^2 \ge (p_z{\Delta l}_{min}\frac{1}{o_z})^2 \quad\Rightarrow\quad (l{\cdot}o_z)^2 \ge (p_z{\Delta l}_{min})^2
所以极线最小长度限制的条件就明了了。

疑问２：这里的判断有没有必要把逆深度也作为参数？不同深度的点的计算对视差的要求一样吗？

对于第二、三个条件，我们看(9)(9)式子的光度视差误差

σ2λ(I)=2σ2ig2p

\sigma_{\lambda(I)}^2=\frac{2\sigma_i^2}{g_p^2}
以及公式(6*)的 几何视差误差

σ2λ(ξ,π)=⟨g,g⟩⋅σ2l⟨g,l⟩2=σ2l|l|2cosθ=σ2lcosθ

\sigma_{\lambda(\xi,\pi)}^2=\frac{\langle g,g\rangle\cdot\sigma_l^2}{\langle g,l\rangle^2} = \frac{\sigma_l^2}{|l|^2\text{cos}\theta} = \frac{\sigma_l^2}{\text{cos}\theta}
这里就有两个参数影响匹配的精度，分别为在极线上的梯度 gpg_p以及匹配点处的梯度和极线向量的夹角。显然这两个分母都不能过小，这也就是代码中的另外两个判断做的工作。

函数最后对极线进行归一化处理，返回归一化后的对极线矢量。

2.2.5 DepthMap::doLineStereo

接下来就是极线搜索的函数了。基本的思路是这样的，先计算得到在当前关键帧上极线上的５个点的灰度作为参考帧匹配的模板。然后得到点深度最大最小范围在参考帧上投影，确定参考帧搜索的极线段范围。最后在确定的极线段上进行匹配，得到最适合的匹配位置。
首先确定当前关键帧上的５个点位置。

    // calculate epipolar line start and end point in old imageEigen::Vector3f KinvP = Eigen::Vector3f(fxi*u+cxi,fyi*v+cyi,1.0f);Eigen::Vector3f pInf = referenceFrame->K_otherToThis_R * KinvP;Eigen::Vector3f pReal = pInf / prior_idepth + referenceFrame->K_otherToThis_t;float rescaleFactor = pReal[2] * prior_idepth;float firstX = u - 2*epxn*rescaleFactor;float firstY = v - 2*epyn*rescaleFactor;float lastX = u + 2*epxn*rescaleFactor;float lastY = v + 2*epyn*rescaleFactor;// width - 2 and height - 2 comes from the one-sided gradient calculation at the bottomif (firstX <= 0 || firstX >= width - 2|| firstY <= 0 || firstY >= height - 2|| lastX <= 0 || lastX >= width - 2|| lastY <= 0 || lastY >= height - 2) {return -1;}if(!(rescaleFactor > 0.7f && rescaleFactor < 1.4f)){if(enablePrintDebugInfo) stats->num_stereo_rescale_oob++;return -1;}// calculate values to search forfloat realVal_p1 = getInterpolatedElement(activeKeyFrameImageData,u + epxn*rescaleFactor, v + epyn*rescaleFactor, width);float realVal_m1 = getInterpolatedElement(activeKeyFrameImageData,u - epxn*rescaleFactor, v - epyn*rescaleFactor, width);float realVal = getInterpolatedElement(activeKeyFrameImageData,u, v, width);float realVal_m2 = getInterpolatedElement(activeKeyFrameImageData,u - 2*epxn*rescaleFactor, v - 2*epyn*rescaleFactor, width);float realVal_p2 = getInterpolatedElement(activeKeyFrameImageData,u + 2*epxn*rescaleFactor, v + 2*epyn*rescaleFactor, width);

首先计算在先验逆深度下在参考帧下的点的位置

PrpReal=KTrkPk=K[Rrk1dk|trk]⋅K−1pk=1dkKRrk⋅K−1pkKinvPpInf+Kt(16)

\underbrace{P_r}_{pReal}=KT_k^rP_k=K[R_k^r\frac{1}{d_k}|t_k^r]{\cdot}K^{-1}p_k=\frac{1}{d_k}\underbrace{KR_k^r{\cdot}\overbrace{K^{-1}p_k}^{KinvP}}_{pInf}+Kt\tag{16}
这里的 pkp_k是当前关键帧中的图像点，其反投影到 归一化平面得到 KinvP，然后除以投影到参考帧下的点 PrP_r。需要得到５个点，则需要以当前图像点为中心，前后沿着极线方向各取两个点。

首先这里作者考虑到了一个尺度的问题。往往当两帧图像间的变换比较大的时候，立体匹配的像素的形状会有一定的“畸变”，我们可以用过一个仿射变换来处理这个“畸变”。如果匹配的是图像块的话，SVO的做法就是把四个定点都投射到空间上然后在投影到另外一副图像。而这里做的是一维的线段，就其这相当于一个一维的相似变换，

设参考帧上匹配点为prp_r，对应极线段的方向矢量为lrl_r，设匹配的极线段长度为nn（实际上就是５个像素点），则按照逆深度drd_r投影到空间中有：

P1=1drK−1(pr−0.5nlr1)P2=1drK−1(pr+0.5nlr1)

P_1=\frac{1}{d_r}K^{-1}\begin{pmatrix}p_r-0.5nl_r\\1\end{pmatrix}\\ P_2=\frac{1}{d_r}K^{-1}\begin{pmatrix}p_r+0.5nl_r\\1\end{pmatrix}
分别变换到当前关键帧坐标系下

1dkK−1⋅(p11)=Rkr1drK−1(pr−0.5nlr1)+tkr1dkK−1⋅(p21)=Rkr1drK−1(pr+0.5nlr1)+tkr

\frac{1}{d_k}K^{-1}{\cdot}\begin{pmatrix}p_1\\1\end{pmatrix}=R_r^k\frac{1}{d_r}K^{-1}\begin{pmatrix}p_r-0.5nl_r\\1\end{pmatrix}+t_r^k\\ \frac{1}{d_k}K^{-1}{\cdot}\begin{pmatrix}p_2\\1\end{pmatrix}=R_r^k\frac{1}{d_r}K^{-1}\begin{pmatrix}p_r+0.5nl_r\\1\end{pmatrix}+t_r^k
整理后两式相减有：

(p2−p10)=dkdrKRkrK−1(nlr0)

\begin{pmatrix}p_2-p_1\\0\end{pmatrix} = \frac{d_k}{d_r}KR_r^kK^{-1}\begin{pmatrix}nl_r\\0\end{pmatrix}
那么在当前关键帧上匹配的极线长度为

|p2−p1|=|dkdr|⋅|KRkrK−1|⋅|nlr|≈ndkdr(17)

|p_2-p_1| = |\frac{d_k}{d_r}|\cdot|KR_r^kK^{-1}|\cdot|nl_r| \approx n\frac{d_k}{d_r} \tag{17}
也即是说当旋转比较小的时候，两帧间匹配的极线段长度按照 dkdr\frac{d_k}{d_r}成比例。也就是代码中的

    float rescaleFactor = pReal[2] * prior_idepth;

接下来就是求在当前关键帧中的５个（模板）点。

注意：这里的灰度变量中的realVal_p1、realVal_p2是远离极点方向的，另外两个是靠近极点方向的。

接下来计算在参考帧对极线上的搜索范围，首先根据得到最远点和最近点

    Eigen::Vector3f pClose = pInf + referenceFrame->K_otherToThis_t*max_idepth;// if the assumed close-point lies behind the// image, have to change that.if(pClose[2] < 0.001f){max_idepth = (0.001f-pInf[2]) / referenceFrame->K_otherToThis_t[2];pClose = pInf + referenceFrame->K_otherToThis_t*max_idepth;}pClose = pClose / pClose[2]; // pos in new image of point (xy), assuming max_idepthEigen::Vector3f pFar = pInf + referenceFrame->K_otherToThis_t*min_idepth;// if the assumed far-point lies behind the image or closter than the near-point,// we moved past the Point it and should stop.if(pFar[2] < 0.001f || max_idepth < min_idepth){if(enablePrintDebugInfo) stats->num_stereo_inf_oob++;return -1;}pFar = pFar / pFar[2]; // pos in new image of point (xy), assuming min_idepth

这里的pClose计算与式子(16)(16)基本相同，差别在于这里pClose多乘以了逆深度max_idepth，得到的一个向量

pClose=KRrk⋅K−1pkKinvPpInf+dmaxKt

p_\text{Close} = \underbrace{KR_k^r{\cdot}\overbrace{K^{-1}p_k}^{KinvP}}_{pInf}+d_\text{max}Kt
由于深度是正的，所以乘以深度点的方向不会改变。为保证最远最近点都在参考帧的视野内，所以如果这个点在参考帧的反面，则把点移到参考帧的正面来。在做这个操作到时候，不能改变点的方向，所以只能改变逆深度的大小。由于乘以逆深度之后，只有后面的那项和逆深度有关，所以这里直接对最后一项进行操作。这里的操作其实是把 K_otherToThis_t的第三个元素乘以一个系数，把 pClose的第三个元素消去，并且加上了一个小量 0.0010.001，只有就保证新的点在参考帧图像的正前方。

    // check for nan due to eg division by zero.if(isnanf((float)(pFar[0]+pClose[0])))return -4;// calculate increments in which we will step through the epipolar line.// they are sampleDist (or half sample dist) longfloat incx = pClose[0] - pFar[0];float incy = pClose[1] - pFar[1];float eplLength = sqrt(incx*incx+incy*incy);if(!eplLength > 0 || std::isinf(eplLength)) return -4;if(eplLength > MAX_EPL_LENGTH_CROP){pClose[0] = pFar[0] + incx*MAX_EPL_LENGTH_CROP/eplLength;pClose[1] = pFar[1] + incy*MAX_EPL_LENGTH_CROP/eplLength;}

这部分代码是把极线搜索的长度限制在一定范围内。

    incx *= GRADIENT_SAMPLE_DIST/eplLength;incy *= GRADIENT_SAMPLE_DIST/eplLength;

然后计算搜索的步进长度。

    // extend one sample_dist to left & right.pFar[0] -= incx;pFar[1] -= incy;pClose[0] += incx;pClose[1] += incy;// make epl long enough (pad a little bit).if(eplLength < MIN_EPL_LENGTH_CROP){float pad = (MIN_EPL_LENGTH_CROP - (eplLength)) / 2.0f;pFar[0] -= incx*pad;pFar[1] -= incy*pad;pClose[0] += incx*pad;pClose[1] += incy*pad;}

这部分则把过段的极线段增加到最小限定长度MIN_EPL_LENGTH_CROP。

// if inf point is outside of image: skip pixel.if(pFar[0] <= SAMPLE_POINT_TO_BORDER ||pFar[0] >= width-SAMPLE_POINT_TO_BORDER ||pFar[1] <= SAMPLE_POINT_TO_BORDER ||pFar[1] >= height-SAMPLE_POINT_TO_BORDER){if(enablePrintDebugInfo) stats->num_stereo_inf_oob++;return -1;}// if near point is outside: move inside, and test length again.if(pClose[0] <= SAMPLE_POINT_TO_BORDER ||pClose[0] >= width-SAMPLE_POINT_TO_BORDER ||pClose[1] <= SAMPLE_POINT_TO_BORDER ||pClose[1] >= height-SAMPLE_POINT_TO_BORDER){if(pClose[0] <= SAMPLE_POINT_TO_BORDER){float toAdd = (SAMPLE_POINT_TO_BORDER - pClose[0]) / incx;pClose[0] += toAdd * incx;pClose[1] += toAdd * incy;}else if(pClose[0] >= width-SAMPLE_POINT_TO_BORDER){float toAdd = (width-SAMPLE_POINT_TO_BORDER - pClose[0]) / incx;pClose[0] += toAdd * incx;pClose[1] += toAdd * incy;}if(pClose[1] <= SAMPLE_POINT_TO_BORDER){float toAdd = (SAMPLE_POINT_TO_BORDER - pClose[1]) / incy;pClose[0] += toAdd * incx;pClose[1] += toAdd * incy;}else if(pClose[1] >= height-SAMPLE_POINT_TO_BORDER){float toAdd = (height-SAMPLE_POINT_TO_BORDER - pClose[1]) / incy;pClose[0] += toAdd * incx;pClose[1] += toAdd * incy;}// get new epl lengthfloat fincx = pClose[0] - pFar[0];float fincy = pClose[1] - pFar[1];float newEplLength = sqrt(fincx*fincx+fincy*fincy);// test againif(pClose[0] <= SAMPLE_POINT_TO_BORDER ||pClose[0] >= width-SAMPLE_POINT_TO_BORDER ||pClose[1] <= SAMPLE_POINT_TO_BORDER ||pClose[1] >= height-SAMPLE_POINT_TO_BORDER ||newEplLength < 8.0f){if(enablePrintDebugInfo) stats->num_stereo_near_oob++;return -1;}}

这里判断近点和远点是否在图像内，如果近点在图像外则把近点沿着极线移到图像内。并且判断最终的极线段度是否满足要求。

疑问３：为什么当远点不在图像范围就返回失败，而近点在图像外则把近点移到图像内？

记下来开始遍历参考帧上的极线段。下面的代码申明了和当前关键帧匹配的５个点的灰度变量。

    // from here on:// - pInf: search start-point// - p0: search end-point// - incx, incy: search steps in pixel// - eplLength, min_idepth, max_idepth: determines search-resolution, i.e. the result's variance.float cpx = pFar[0];float cpy =  pFar[1];float val_cp_m2 = getInterpolatedElement(referenceFrameImage,cpx-2.0f*incx, cpy-2.0f*incy, width);float val_cp_m1 = getInterpolatedElement(referenceFrameImage,cpx-incx, cpy-incy, width);float val_cp = getInterpolatedElement(referenceFrameImage,cpx, cpy, width);float val_cp_p1 = getInterpolatedElement(referenceFrameImage,cpx+incx, cpy+incy, width);float val_cp_p2;

注意：这里的val_cp_m1和val_cp_m2靠近远点，val_cp_p1和val_cp_p2靠近近点。

一开始计算当前关键帧上的灰度时，realVal_p1、realVal_p2是远离极点方向的。这里两个５点线段的方向怎么确定呢？如下图，假设关键帧相邻５点的深度近似相等，则当前关键帧上把从极点指向无穷远点的５点有向线段在参考帧上投影的方向恰好是从远处点指向近处点。

也就是说参考帧上近处的点是和当前关键帧上远离极点的位置是对应的，即realVal_p1对应val_cp_m1，依次类推。

接下来就是在极线段上遍历，每一次都计算下一个点的灰度值，这里是从远处向近处搜索

while(((incx < 0) == (cpx > pClose[0]) && (incy < 0) == (cpy > pClose[1])) || loopCounter == 0)
{// interpolate one new pointval_cp_p2 = getInterpolatedElement(referenceFrameImage,cpx+2*incx, cpy+2*incy, width);

在每次循环结束的时候对变量进行更新，５个点依次沿着极线运动。

    // shift everything one further.eeLast = ee;val_cp_m2 = val_cp_m1; val_cp_m1 = val_cp; val_cp = val_cp_p1; val_cp_p1 = val_cp_p2;if(enablePrintDebugInfo) stats->num_stereo_comparisons++;cpx += incx;cpy += incy;loopCounter++;
}

循环内使用了两组不断切换的变量，分别对残差进行记录。

        // hacky but fast way to get error and differential error: switch buffer variables for last loop.float ee = 0;if(loopCounter%2==0){// calc error and accumulate sums.e1A = val_cp_p2 - realVal_p2;ee += e1A*e1A;e2A = val_cp_p1 - realVal_p1;ee += e2A*e2A;e3A = val_cp - realVal;      ee += e3A*e3A;e4A = val_cp_m1 - realVal_m1;ee += e4A*e4A;e5A = val_cp_m2 - realVal_m2;ee += e5A*e5A;}else{// calc error and accumulate sums.e1B = val_cp_p2 - realVal_p2;ee += e1B*e1B;e2B = val_cp_p1 - realVal_p1;ee += e2B*e2B;e3B = val_cp - realVal;      ee += e3B*e3B;e4B = val_cp_m1 - realVal_m1;ee += e4B*e4B;e5B = val_cp_m2 - realVal_m2;ee += e5B*e5B;}

把最好位置（误差最小）的残差、位置等都记录下来，并且把前一个最好位置更新，以及记录最好位置前个位置的数据。

// do I have a new winner??// if so: set.if(ee < best_match_err){// put to second-bestsecond_best_match_err=best_match_err;loopCSecond = loopCBest;// set best.best_match_err = ee;loopCBest = loopCounter;best_match_errPre = eeLast;best_match_DiffErrPre = e1A*e1B + e2A*e2B + e3A*e3B + e4A*e4B + e5A*e5B;best_match_errPost = -1;best_match_DiffErrPost = -1;best_match_x = cpx;best_match_y = cpy;bestWasLastLoop = true;}

并且记录最好位置的下一个位置的数据。

        // otherwise: the last might be the current winner, in which case i have to save these values.else{if(bestWasLastLoop){best_match_errPost = ee;best_match_DiffErrPost = e1A*e1B + e2A*e2B + e3A*e3B + e4A*e4B + e5A*e5B;bestWasLastLoop = false;}// collect second-best:// just take the best of all that are NOT equal to current best.if(ee < second_best_match_err){second_best_match_err=ee;loopCSecond = loopCounter;}}

经过上面的循环已经找到最优匹配的位置，接下来根据误差大小来确定这个位置是否满足要求。以及确认最好位置有没有明显优于第二好位置。如果不满足就返回失败。

    // if error too big, will return -3, otherwise -2.if(best_match_err > 4.0f*(float)MAX_ERROR_STEREO){if(enablePrintDebugInfo) stats->num_stereo_invalid_bigErr++;return -3;}// check if clear enough winnerif(abs(loopCBest - loopCSecond) > 1.0f && MIN_DISTANCE_ERROR_STEREO * best_match_err > second_best_match_err){if(enablePrintDebugInfo) stats->num_stereo_invalid_unclear_winner++;return -2;}

接下来是计算亚像素匹配点。在求亚像素点的时候，处理的很奇怪。先看下代码：
看亚像素计算前的误差梯度计算：

        // ================== compute exact match =========================// compute gradients (they are actually only half the real gradient)float gradPre_pre = -(best_match_errPre - best_match_DiffErrPre);float gradPre_this = +(best_match_err - best_match_DiffErrPre);float gradPost_this = -(best_match_err - best_match_DiffErrPost);float gradPost_post = +(best_match_errPost - best_match_DiffErrPost);

之前查找最优匹配的时候，分别保存了ECC误差最小的点的数据（位置和误差）和该点之前和之后点的数据。我们记这三个点的位置分别为u0u_0、u1u_1、u2u_2，其中u1u_1为最优匹配点的位置，它们之间的间隔为一个单位。我们需要找到亚像素点，当然，要存在这样的点，就必须保证两个点的误差梯度是不同号的。根据ECC误差，我们有：

E(u)=∑i=15e2i(u)=∑i=15(Ikf(vi)−Iref(v′i))2

E(u) = \sum_{i=1}^{5}e_i^2(u) = \sum_{i=1}^{5}(I_{kf}(v_i)-I_{ref}(v'_i))^2
这里的 viv_i是在当前关键帧图像 activeKeyFrameImage极线段上的点， viv_i是参考帧 referenceFrame对应极线段上以点 uu为中心的等距点，有u=v3u=v_3。我们需要求误差的梯度（导数），则有：
\frac{dE(u)}{du} = 2\sum_{i=1}^{5}e_i(u)e_i'(u)

dE(u)du=2∑i=15ei(u)e′i(u)

\frac{dE(u)}{du} = 2\sum_{i=1}^{5}e_i(u)e_i'(u)
这里的 e'(u) e′(u)e'(u)为误差在点 u uu处的导数，对于在点u_0u0u_0位置的导数，有：

e′(u0)=limu→u0e′(u)=e(u)−e(u0)u−u0

e'(u_0)=\lim_{u \to u_0}e'(u)=\frac{e(u)-e(u_0)}{u-u_0}
在计算亚像素点的时候有一段注释：

/** Subsequent exact minimum is found the following way:* - assuming lin. interpolation, the gradient of Error at p1 (towards p2) is given by*   dE1 = -2sum(e1*e1 - e1*e2)*   where e1 and e2 are summed over, and are the residuals (not squared).** - the gradient at p2 (coming from p1) is given by*   dE2 = +2sum(e2*e2 - e1*e2)** - linear interpolation => gradient changes linearely; zero-crossing is hence given by*   p1 + d*(p2-p1) with d = -dE1 / (-dE1 + dE2).**** => I for later exact min calculation, I need sum(e_i*e_i),sum(e_{i-1}*e_{i-1}),sum(e_{i+1}*e_{i+1})*    and sum(e_i * e_{i-1}) and sum(e_i * e_{i+1}),*    where i is the respective winning index.*/

在代码中，计算点u0u_0点的梯度gradPre_pre时，对应的e′(u0)e'(u_0)的逼近是从u1→u0u_1\to u_0的（从点的右侧逼近），因此有e′(u0)=(e(u1)−e(u0)e'(u_0)=(e(u_1)-e(u_0)，而计算点u1u_1的梯度gradPre_this时的逼近是从u0→u1u_0\to u_1的（从点的左侧逼近），因此有e′(u1)=−(e(u0)−e(u1))e'(u_1)=-(e(u_0)-e(u_1))。对照代码这里都是符合的，求的这四个梯度差了一个系数22，由于下面计算的时候是比值的形式，因此没有影响。

接下来判断在哪里插值，也就是找到有梯度左右变号的位置。

        // if one is oob: return false.if(enablePrintDebugInfo && (best_match_errPre < 0 || best_match_errPost < 0)){stats->num_stereo_invalid_atEnd++;}// - if zero-crossing occurs exactly in between (gradient Inconsistent),else if((gradPost_this < 0) ^ (gradPre_this < 0)){// return exact pos, if both central gradients are small compared to their counterpart.if(enablePrintDebugInfo && (gradPost_this*gradPost_this > 0.1f*0.1f*gradPost_post*gradPost_post ||gradPre_this*gradPre_this > 0.1f*0.1f*gradPre_pre*gradPre_pre))stats->num_stereo_invalid_inexistantCrossing++;}// if pre has zero-crossingelse if((gradPre_pre < 0) ^ (gradPre_this < 0)){// if post has zero-crossingif((gradPost_post < 0) ^ (gradPost_this < 0)){if(enablePrintDebugInfo) stats->num_stereo_invalid_twoCrossing++;}elseinterpPre = true;}// if post has zero-crossingelse if((gradPost_post < 0) ^ (gradPost_this < 0)){interpPost = true;}// if none has zero-crossingelse{if(enablePrintDebugInfo) stats->num_stereo_invalid_noCrossing++;}

开始插值，这里比较简单，就是按照斜率计算得到斜率为０的位置。并且重新计算亮度残差，这里是对最好匹配位置的残差做二阶泰勒展开进行拟合。

        // DO interpolation!// minimum occurs at zero-crossing of gradient, which is a straight line => easy to compute.// the error at that point is also computed by just integrating.if(interpPre){float d = gradPre_this / (gradPre_this - gradPre_pre);best_match_x -= d*incx;best_match_y -= d*incy;best_match_err = best_match_err - 2*d*gradPre_this - (gradPre_pre - gradPre_this)*d*d;if(enablePrintDebugInfo) stats->num_stereo_interpPre++;didSubpixel = true;}else if(interpPost){float d = gradPost_this / (gradPost_this - gradPost_post);best_match_x += d*incx;best_match_y += d*incy;best_match_err = best_match_err + 2*d*gradPost_this + (gradPost_post - gradPost_this)*d*d;if(enablePrintDebugInfo) stats->num_stereo_interpPost++;didSubpixel = true;}else{if(enablePrintDebugInfo) stats->num_stereo_interpNone++;}

然后判断最好位置的残差是否满足要求。一开始计算的是在当前关键帧极线上的梯度，如果梯度越大，则对残差的容忍就越大。

    // sampleDist is the distance in pixel at which the realVal's were sampledfloat sampleDist = GRADIENT_SAMPLE_DIST*rescaleFactor;float gradAlongLine = 0;float tmp = realVal_p2 - realVal_p1;  gradAlongLine+=tmp*tmp;tmp = realVal_p1 - realVal;  gradAlongLine+=tmp*tmp;tmp = realVal - realVal_m1;  gradAlongLine+=tmp*tmp;tmp = realVal_m1 - realVal_m2;  gradAlongLine+=tmp*tmp;gradAlongLine /= sampleDist*sampleDist;// check if interpolated error is OK. use evil hack to allow more error if there is a lot of gradient.if(best_match_err > (float)MAX_ERROR_STEREO + sqrtf( gradAlongLine)*20){if(enablePrintDebugInfo) stats->num_stereo_invalid_bigErr++;return -3;}

最后就是计算更新的逆深度以及其对应的不确定性（方差）。这里求的逆深度不确定性中的比例参数α\alpha，对应理论部分的式子(13)(13)。

α:=r2pk⋅tx−r0pk⋅tz(tx−tz⋅x)2⋅finvx⋅∂u∂λ

\alpha:=\frac{r_2p_k{\cdot}t_x-r_0p_k{\cdot}t_z}{(t_x-t_z{\cdot}x)^2}{\cdot}f_{x}^{inv}{\cdot}\frac{\partial{u}}{\partial{\lambda}}

    // ================= calc depth (in KF) ====================// * KinvP = Kinv * (x,y,1); where x,y are pixel coordinates of point we search for, in the KF.// * best_match_x = x-coordinate of found correspondence in the reference frame.float idnew_best_match; // depth in the new imagefloat alpha; // d(idnew_best_match) / d(disparity in pixel) == conputed inverse depth derived by the pixel-disparity.if(incx*incx>incy*incy){float oldX = fxi*best_match_x+cxi;float nominator = (oldX*referenceFrame->otherToThis_t[2] - referenceFrame->otherToThis_t[0]);float dot0 = KinvP.dot(referenceFrame->otherToThis_R_row0);float dot2 = KinvP.dot(referenceFrame->otherToThis_R_row2);idnew_best_match = (dot0 - oldX*dot2) / nominator;alpha = incx*fxi*(dot0*referenceFrame->otherToThis_t[2] - dot2*referenceFrame->otherToThis_t[0]) / (nominator*nominator);}else{float oldY = fyi*best_match_y+cyi;float nominator = (oldY*referenceFrame->otherToThis_t[2] - referenceFrame->otherToThis_t[1]);float dot1 = KinvP.dot(referenceFrame->otherToThis_R_row1);float dot2 = KinvP.dot(referenceFrame->otherToThis_R_row2);idnew_best_match = (dot1 - oldY*dot2) / nominator;alpha = incy*fyi*(dot1*referenceFrame->otherToThis_t[2] - dot2*referenceFrame->otherToThis_t[1]) / (nominator*nominator);}if(idnew_best_match < 0){if(enablePrintDebugInfo) stats->num_stereo_negative++;if(!allowNegativeIdepths)return -2;}

然后计算融合后的逆深度。这里先是计算两个噪声的方差，最终算出逆深度误差的方差。

// ================= calc var (in NEW image) ====================// calculate error from photometric noisefloat photoDispError = 4.0f * cameraPixelNoise2 / (gradAlongLine + DIVISION_EPS);float trackingErrorFac = 0.25f*(1.0f+referenceFrame->initialTrackedResidual);// calculate error from geometric noise (wrong camera pose / calibration)Eigen::Vector2f gradsInterp = getInterpolatedElement42(activeKeyFrame->gradients(0), u, v, width);float geoDispError = (gradsInterp[0]*epxn + gradsInterp[1]*epyn) + DIVISION_EPS;geoDispError = trackingErrorFac*trackingErrorFac*(gradsInterp[0]*gradsInterp[0] + gradsInterp[1]*gradsInterp[1]) / (geoDispError*geoDispError);//geoDispError *= (0.5 + 0.5 *result_idepth) * (0.5 + 0.5 *result_idepth);// final error consists of a small constant part (discretization error),// geometric and photometric error.result_var = alpha*alpha*((didSubpixel ? 0.05f : 0.5f)*sampleDist*sampleDist +  geoDispError + photoDispError); // square to make variance

对应(6∗)(6*)和(9)(9)式以及(12)(12)式子

σ2λ(ξ,π)=⟨g,g⟩⋅σ2l⟨g,l⟩2σ2λ(I)=2σ2ig2pσ2d,obs≤α2(min{σ2λ(ξ,π)}+min{σ2λ(I)})

\sigma_{\lambda(\xi,\pi)}^2=\frac{\langle g,g\rangle\cdot\sigma_l^2}{\langle g,l\rangle^2} \quad\quad \sigma_{\lambda(I)}^2=\frac{2\sigma_i^2}{g_p^2}\\ \sigma_{d,\text{obs}}^2\le\alpha^2\begin{pmatrix}\text{min}\{\sigma_{\lambda(\xi,\pi)}^2\}+\text{min}\{\sigma_{\lambda(I)}^2\}\end{pmatrix}
这里的 σ2l\sigma_l^2是变量 trackingErrorFac，也就是由残差给的。在计算最后的方差的时候，代码中还增加了一点 离散误差 sampleDist，这个其实就是最小步进距离。其实在做极线搜索的时候，也可以使用优化迭代的方式来求，按本人的理解，这样做的话理论上就可以省去这里的最小离散误差了。

这样新的逆深度估计和估计的方差都已经求出来了。在函数DepthMap::observeDepthCreate中直接由计算的逆深度来构建深度图中新的逆深度假设，而在DepthMap::observeDepthUpdate函数中则需要把新估计的值和之前的先验数据进行融合。

2.2.6 DepthMap::observeDepthUpdate

这个函数和DepthMap::observeDepthCreate的区别有三个地方：

参考帧的选取
极线搜索范围
构建新的逆深度假设或逆深度融合

第一个不同的地方为对于函数observeDepthCreate，参考帧的选择为最“老”的关键帧，也就是距离当前关键帧最近的参考帧（这里有activeKeyFrameIsReactivated=false）。

Frame* refFrame = activeKeyFrameIsReactivated ? newest_referenceFrame : oldest_referenceFrame;

而在本函数中，则判断是否需要跳过一定的帧数。

    if(!activeKeyFrameIsReactivated){if((int)target->nextStereoFrameMinID - referenceFrameByID_offset >= (int)referenceFrameByID.size()){if(plotStereoImages)debugImageHypothesisHandling.at<cv::Vec3b>(y, x) = cv::Vec3b(0,255,0);  // GREEN FOR skipif(enablePrintDebugInfo) stats->num_observe_skip_alreadyGood++;return false;}if((int)target->nextStereoFrameMinID - referenceFrameByID_offset < 0)refFrame = oldest_referenceFrame;elserefFrame = referenceFrameByID[(int)target->nextStereoFrameMinID - referenceFrameByID_offset];}elserefFrame = newest_referenceFrame;

这里就涉及到理论部分参考帧选取的部分，这里的变量nextStereoFrameMinID就相当于论文中所讲的“年龄”，这个变量在本函数最后的部分进行更新：

        // increase Skip!if(result_eplLength < MIN_EPL_LENGTH_CROP){float inc = activeKeyFrame->numFramesTrackedOnThis / (float)(activeKeyFrame->numMappedOnThis+5);if(inc < 3) inc = 3;inc +=  ((int)(result_eplLength*10000)%2);if(enablePrintDebugInfo) stats->num_observe_addSkip++;if(result_eplLength < 0.5*MIN_EPL_LENGTH_CROP)inc *= 3;target->nextStereoFrameMinID = refFrame->id() + inc;}

这里的变量result_eplLength在函数DepthMap::doLineStereo中计算，也就是参考帧上极线搜索的极线段长度。所以当搜索的极线段太短了，就会增加变量nextStereoFrameMinID的值。最终影响更新逆深度的参考帧选取。

第二点不同在于极线搜索的时候，函数observeDepthCreate构建新的逆深度时是对尽可能大的范围内进行搜索

    float error = doLineStereo(new_u,new_v,epx,epy,0.0f, 1.0f, 1.0f/MIN_DEPTH,refFrame, refFrame->image(0),result_idepth, result_var, result_eplLength, stats);

而本函数的搜索范围是由之前逆深度先验值给的。

    // which exact point to track, and where from.float sv = sqrt(target->idepth_var_smoothed);float min_idepth = target->idepth_smoothed - sv*STEREO_EPL_VAR_FAC;float max_idepth = target->idepth_smoothed + sv*STEREO_EPL_VAR_FAC;if(min_idepth < 0) min_idepth = 0;if(max_idepth > 1/MIN_DEPTH) max_idepth = 1/MIN_DEPTH;stats->num_observe_update_attempted++;float result_idepth, result_var, result_eplLength;float error = doLineStereo(x,y,epx,epy,min_idepth, target->idepth_smoothed ,max_idepth,refFrame, refFrame->image(0),result_idepth, result_var, result_eplLength, stats);

最后一个不同在于一个是构造新的逆深度假设

    // add hypothesis*target = DepthMapPixelHypothesis(result_idepth,result_var,VALIDITY_COUNTER_INITIAL_OBSERVE);

而本函数则对逆深度进行融合

        // do textbook ekf update:// increase var by a little (prediction-uncertainty)float id_var = target->idepth_var*SUCC_VAR_INC_FAC;// update var with observationfloat w = result_var / (result_var + id_var);float new_idepth = (1-w)*result_idepth + w*target->idepth;target->idepth = UNZERO(new_idepth);// variance can only decrease from observation; never increase.id_var = id_var * w;if(id_var < target->idepth_var)target->idepth_var = id_var;

这里需要注意的是，在新的逆深度估计上，增加了一点不确定度SUCC_VAR_INC_FAC，看定义

#define SUCC_VAR_INC_FAC (1.01f) // before an ekf-update, the variance is increased by this factor.

这就是对应理论部分深度图融合中所谓的增加一点不确定性。

到这里深度图估计的部分基本已经完成了。现在还剩下两个函数regularizeDepthMapFillHoles和regularizeDepthMap，这两个函数是对最终得到的深度图进行处理。

2.2.7 DepthMap::regularizeDepthMapFillHoles

这个函数的功能是对深度图做一次填补。这里也是使用了多线程处理的方式，调用函数DepthMap::regularizeDepthMapFillHolesRow来对深度图的每一行做处理。

void DepthMap::regularizeDepthMapFillHoles()
{buildRegIntegralBuffer();runningStats.num_reg_created=0;memcpy(otherDepthMap,currentDepthMap,width*height*sizeof(DepthMapPixelHypothesis));threadReducer.reduce(boost::bind(&DepthMap::regularizeDepthMapFillHolesRow, this, _1, _2, _3), 3, height-2, 10);if(enablePrintDebugInfo && printFillHolesStatistics)printf("FillHoles (discreteDepth): %d created\n",runningStats.num_reg_created);
}

首先看一下进入函数的时候先调用了buildRegIntegralBuffer，这个函数的作用是计算一个积分图validityIntegralBuffer，这个变量是和深度图相同尺寸的图像缓存区域，每一位对应逆深度从图像起始点到该点矩形区域中所有逆深度有效观测次数的统计。这个在后续会用到，计算某个像素领域位置所有有效逆深度的观测次数。

接下来看实际实现的代码

void DepthMap::regularizeDepthMapFillHolesRow(int yMin, int yMax, RunningStats* stats)
{// =========== regularize fill holesconst float* keyFrameMaxGradBuf = activeKeyFrame->maxGradients(0);for(int y=yMin; y<yMax; y++){for(int x=3;x<width-2;x++){int idx = x+y*width;DepthMapPixelHypothesis* dest = otherDepthMap + idx;if(dest->isValid) continue;if(keyFrameMaxGradBuf[idx]<MIN_ABS_GRAD_DECREASE) continue;int* io = validityIntegralBuffer + idx;int val = io[2+2*width] - io[2-3*width] - io[-3+2*width] + io[-3-3*width];if((dest->blacklisted >= MIN_BLACKLIST && val > VAL_SUM_MIN_FOR_CREATE) || val > VAL_SUM_MIN_FOR_UNBLACKLIST){float sumIdepthObs = 0, sumIVarObs = 0;int num = 0;DepthMapPixelHypothesis* s1max = otherDepthMap + (x-2) + (y+3)*width;for (DepthMapPixelHypothesis* s1 = otherDepthMap + (x-2) + (y-2)*width; s1 < s1max; s1+=width)for(DepthMapPixelHypothesis* source = s1; source < s1+5; source++){if(!source->isValid) continue;sumIdepthObs += source->idepth /source->idepth_var;sumIVarObs += 1.0f/source->idepth_var;num++;}float idepthObs = sumIdepthObs / sumIVarObs;idepthObs = UNZERO(idepthObs);currentDepthMap[idx] =DepthMapPixelHypothesis(idepthObs,VAR_RANDOM_INIT_INITIAL,0);if(enablePrintDebugInfo) stats->num_reg_created++;}}}
}

函数对深度图的像素位置进行遍历。代码中的这部分就是计算对应深度图像素位置5×55\times5领域的所有的有效观测次数。这里的代码其实是一个二次积分的形式，相当于对5×55\times5领域积分。

            int* io = validityIntegralBuffer + idx;int val = io[2+2*width] - io[2-3*width] - io[-3+2*width] + io[-3-3*width];

如果这个位置的点没有被设置到黑名单，则用其5×55\times5领域的所有逆深度的融合作为其逆深度假设。

2.2.8 DepthMap::regularizeDepthMap

这个函数相对比较简单，计算平滑的深度图。和其他函数一样，都是用多线程实现的，实现在函数DepthMap::regularizeDepthMapRow，平滑的窗口也是5×55\times5大小。

for(int dx=-regularize_radius; dx<=regularize_radius;dx++)for(int dy=-regularize_radius; dy<=regularize_radius;dy++){DepthMapPixelHypothesis* source = destRead + dx + dy*width;if(!source->isValid) continue;//stats->num_reg_total++;float diff =source->idepth - destRead->idepth;if(DIFF_FAC_SMOOTHING*diff*diff > source->idepth_var + destRead->idepth_var){if(removeOcclusions){if(source->idepth > destRead->idepth)numOccluding++;}continue;}val_sum += source->validity_counter;if(removeOcclusions)numNotOccluding++;float distFac = (float)(dx*dx+dy*dy)*regDistVar;float ivar = 1.0f/(source->idepth_var + distFac);sum += source->idepth * ivar;sumIvar += ivar;}

这里把所有逆深度在当前像素逆深度的方差范围内的点都进行统计，记录累计的有效观测次数val_sum，并对这些点的逆深度和逆深度的方差进行累加（高斯融合的方式）。对设置阻塞的情况removeOcclusions=true，则统计领域内点的逆深度与当前像素逆深度差大于一定值的点数numOccluding和小于的点数numNotOccluding。

            if(val_sum < validityTH){dest->isValid = false;if(enablePrintDebugInfo) stats->num_reg_deleted_secondary++;dest->blacklisted--;if(enablePrintDebugInfo) stats->num_reg_setBlacklisted++;continue;}if(removeOcclusions){if(numOccluding > numNotOccluding){dest->isValid = false;if(enablePrintDebugInfo) stats->num_reg_deleted_occluded++;continue;}}sum = sum / sumIvar;sum = UNZERO(sum);// update!dest->idepth_smoothed = sum;dest->idepth_var_smoothed = 1.0f/sumIvar;

如果有效观测数太小，则把改点逆深度设置为无效，并且设置黑名单。如果需要设置阻塞，则领域内逆深度和当前像素逆深度差差过大的点大于逆深度差小的点，则把当前像素设置为阻塞，也就是无效的状态。

如果当前逆深度像素点的状态良好，则更新逆深度的平滑值。
需要一提的是，只有在构造新的关键帧的时候，会调用两次函数regularizeDepthMap，并且第一次是会设置阻塞的，即removeOcclusions＝true。

2.3 SlamSystem::createNewCurrentKeyframe

这部分是构建新的当前关键帧，也是深度图在两帧图像间传播的情况。该函数的有效部分如下：

    if(SLAMEnabled){// add NEW keyframe to id-lookupkeyFrameGraph->idToKeyFrameMutex.lock();keyFrameGraph->idToKeyFrame.insert(std::make_pair(newKeyframeCandidate->id(), newKeyframeCandidate));keyFrameGraph->idToKeyFrameMutex.unlock();}// propagate & make new.map->createKeyFrame(newKeyframeCandidate.get());

分别是把当前用于构建新关键帧的图像帧放入关键帧队列以及传播逆深度。传播逆深度调用了深度建图中的函数createKeyFrame。

2.3.1 DepthMap::createKeyFrame

首先计算了新关键帧跟踪的关键帧到新关键帧的SE3变换

    SE3 oldToNew_SE3 = se3FromSim3(new_keyframe->pose->thisToParent_raw).inverse();

然后就调用深度图传播的函数

    propagateDepth(new_keyframe);`

之后设置当前激活的关键帧activeKeyFrame，这个变量就是当前关键帧的一个拷贝，用在深度图建图类的内部。

    activeKeyFrame = new_keyframe;activeKeyFramelock = activeKeyFrame->getActiveLock();activeKeyFrameImageData = new_keyframe->image(0);activeKeyFrameIsReactivated = false;

然后依次调用上面介绍的两个函数。

    regularizeDepthMap(true, VAL_SUM_MIN_FOR_KEEP);regularizeDepthMapFillHoles();regularizeDepthMap(false, VAL_SUM_MIN_FOR_KEEP);

最后就是对深度图做归一化处理

    float sumIdepth=0, numIdepth=0;for(DepthMapPixelHypothesis* source = currentDepthMap; source < currentDepthMap+width*height; source++){if(!source->isValid)continue;sumIdepth += source->idepth_smoothed;numIdepth++;}float rescaleFactor = numIdepth / sumIdepth;float rescaleFactor2 = rescaleFactor*rescaleFactor;for(DepthMapPixelHypothesis* source = currentDepthMap; source < currentDepthMap+width*height; source++){if(!source->isValid)continue;source->idepth *= rescaleFactor;source->idepth_smoothed *= rescaleFactor;source->idepth_var *= rescaleFactor2;source->idepth_var_smoothed *= rescaleFactor2;}activeKeyFrame->pose->thisToParent_raw = sim3FromSE3(oldToNew_SE3.inverse(), rescaleFactor);activeKeyFrame->pose->invalidateCache();

接下来我们重点看深度传播的实现部分

2.3.2 DepthMap::propagateDepth

在函数进来的部分，把当前深度图都设置为无效。这里的otherDepthMap是临时使用的变量，变量currentDepthMap是当前关键帧的深度图。　

    // wipe depthmapfor(DepthMapPixelHypothesis* pt = otherDepthMap+width*height-1; pt >= otherDepthMap; pt--){pt->isValid = false;pt->blacklisted = 0;}

在变量所有像素之前把需要用的变量都先获取。分别是新关键帧到当前关键帧的位姿变换，从当前关键帧跟踪到新关键帧的情况trackingWasGood，以及当前关键帧的灰度图以及新关键帧的梯度和灰度图。

    // re-usable values.SE3 oldToNew_SE3 = se3FromSim3(new_keyframe->pose->thisToParent_raw).inverse();Eigen::Vector3f trafoInv_t = oldToNew_SE3.translation().cast<float>();Eigen::Matrix3f trafoInv_R = oldToNew_SE3.rotationMatrix().matrix().cast<float>();const bool* trackingWasGood = new_keyframe->getTrackingParent() == activeKeyFrame ? new_keyframe->refPixelWasGoodNoCreate() : 0;const float* activeKFImageData = activeKeyFrame->image(0);const float* newKFMaxGrad = new_keyframe->maxGradients(0);const float* newKFImageData = new_keyframe->image(0);

然后就是变量当前关键帧深度图上每一个有效的点

    // go through all pixels of OLD image, propagating forwards.for(int y=0;y<height;y++)for(int x=0;x<width;x++){DepthMapPixelHypothesis* source = currentDepthMap + x + y*width;if(!source->isValid)continue;

把对应位置点变换到新关键帧的坐标系下，或得新的逆深度，并且得到对应的像素位置，判断是否被新关键帧观测到，跳过未观测到的点。

            Eigen::Vector3f pn = (trafoInv_R * Eigen::Vector3f(x*fxi + cxi,y*fyi + cyi,1.0f)) / source->idepth_smoothed + trafoInv_t;float new_idepth = 1.0f / pn[2];float u_new = pn[0]*new_idepth*fx + cx;float v_new = pn[1]*new_idepth*fy + cy;// check if still within image, if not: DROP.if(!(u_new > 2.1f && v_new > 2.1f && u_new < width-3.1f && v_new < height-3.1f)){if(enablePrintDebugInfo) runningStats.num_prop_removed_out_of_bounds++;continue;}

确保在新关键帧上的点是从当前参考帧成功跟踪的点。这里的trackingWasGood在之前赋值，是非空的。这里为新关键帧没有跟踪到当前关键帧的情况，用两帧图像对应位置的灰度差来判断当前点是否有效。

            int newIDX = (int)(u_new+0.5f) + ((int)(v_new+0.5f))*width;float destAbsGrad = newKFMaxGrad[newIDX];if(trackingWasGood != 0){if(!trackingWasGood[(x >> SE3TRACKING_MIN_LEVEL) + (width >> SE3TRACKING_MIN_LEVEL)*(y >> SE3TRACKING_MIN_LEVEL)]|| destAbsGrad < MIN_ABS_GRAD_DECREASE){if(enablePrintDebugInfo) runningStats.num_prop_removed_colorDiff++;continue;}}else{float sourceColor = activeKFImageData[x + y*width];float destColor = getInterpolatedElement(newKFImageData, u_new, v_new, width);float residual = destColor - sourceColor;if(residual*residual / (MAX_DIFF_CONSTANT + MAX_DIFF_GRAD_MULT*destAbsGrad*destAbsGrad) > 1.0f || destAbsGrad < MIN_ABS_GRAD_DECREASE){if(enablePrintDebugInfo) runningStats.num_prop_removed_colorDiff++;continue;}}

对满足条件的点进行逆深度构造，这里使用的是公式(15)(15)计算新的逆深度方差。

            DepthMapPixelHypothesis* targetBest = otherDepthMap +  newIDX;// large idepth = point is near = large increase in variance.// small idepth = point is far = small increase in variance.float idepth_ratio_4 = new_idepth / source->idepth_smoothed;idepth_ratio_4 *= idepth_ratio_4;idepth_ratio_4 *= idepth_ratio_4;float new_var =idepth_ratio_4*source->idepth_var;

如果该点已经是有效的，则检查新的估计和原有的先验的差是否大于方差。如果是大于方差，则分成两种情况：如果点变远了（逆深度变小），则舍弃新的估计；否则认为该点的先验是无效的。

疑问４：很奇怪，这里和论文讲的深度图传播部分是正好相反的。为什么把深度变大或变小，作为点是否无效的判断标准？

            // check for occlusionif(targetBest->isValid){// if they occlude one another, one gets removed.float diff = targetBest->idepth - new_idepth;if(DIFF_FAC_PROP_MERGE*diff*diff >new_var +targetBest->idepth_var){if(new_idepth < targetBest->idepth){if(enablePrintDebugInfo) runningStats.num_prop_occluded++;continue;}else{if(enablePrintDebugInfo) runningStats.num_prop_occluded++;targetBest->isValid = false;}}}

如果对应像素位置没有有效的逆深度假设，则构建新的逆深度假设，否则使用公式(14)(14)结合先验逆深度融合。

            if(!targetBest->isValid){if(enablePrintDebugInfo) runningStats.num_prop_created++;*targetBest = DepthMapPixelHypothesis(new_idepth,new_var,source->validity_counter);}else{if(enablePrintDebugInfo) runningStats.num_prop_merged++;// merge idepth ekf-stylefloat w = new_var / (targetBest->idepth_var + new_var);float merged_new_idepth = w*targetBest->idepth + (1.0f-w)*new_idepth;// merge validityint merged_validity = source->validity_counter + targetBest->validity_counter;if(merged_validity > VALIDITY_COUNTER_MAX+(VALIDITY_COUNTER_MAX_VARIABLE))merged_validity = VALIDITY_COUNTER_MAX+(VALIDITY_COUNTER_MAX_VARIABLE);*targetBest = DepthMapPixelHypothesis(merged_new_idepth,1.0f/(1.0f/targetBest->idepth_var + 1.0f/new_var),merged_validity);}

３. 总结

深度估计的这块内容内容比较多，很多地方也不太好理解。基本是论文代码反复看，才基本啃下来。这块部分有几点比较有意思的部分：

充分考虑了逆深度估计中的几何误差和光度误差。
把地图使用深度图的形式来表示，当前关键帧优先选择地图中已有的关键帧。这个和其他SLAM系统构建关键帧后，再删选的思路不同。
立体匹配的时候使用线段匹配的方式，不同于块匹配；并且使用“隔点采样”的方式，而不是优化迭代。总体来说效率高。
在更新深度图的时候，如果新的观测和先验相差过大，则舍弃新的观测，并且增大先验的不确定度。

在地图构建的过程中很多地方都认为旋转是很小的，感觉这个假设会使得建图精度容易受旋影响。实际在用自己摄像头跑LSD-SLAM的时候，对相机移动的要求还是很高的，深度图很容易就变得一团糟。

参考文献

Semi-Dense Visual Odometry for a Monocular Camera
LSD-SLAM: Large-Scale Direct Monocular SLAM
Covariance Propagation for Guided Matching
Probability Models in Engineering and Science
lsd-slam源码解读第五篇:DepthEstimation