第13讲 建图
13.2 单目稠密重建
13.2.1 立体视觉
假定有一段视频序列，我们通过某种魔法得到了每一帧对应的轨迹（当然也很可能是由视觉里程计前端估计所得）。现在我们以第一幅图像为参考帧，计算参考帧中每一个像素的深度（或者说距离）。
首先，回忆在特征点部分我们是如何完成该过程的： 1.首先，我们对图像提取特征，并根据描述子计算了特征之间的匹配。换言之，通过特征，我们对某一个空间点进行了跟踪，知道了它在各个图像之间的位置。2.然后，由于我们无法仅用一幅图像确定特征点的位置，所以必须通过不同视角下的观测估计它的深度，原理即前面讲过的三角测量。
如何确定第一幅图的某像素出现在其他图里的位置呢？这需要用到极线搜索和块匹配技术。然后，当我们知道了某个像素在各个图中的位置，就能像特征点那样，利用三角测量确定它的深度。不过不同的是，在这里我们要使用很多次三角测量让深度估计收敛，而不仅是一次。我们希望深度估计能够随着测量的增加从一个非常不确定的量，逐渐收敛到一个稳定值。这就是深度滤波器技术。
13.2.2 极线搜索与块匹配
左边的相机观测到某像素点$p_1$。由于这是一个单目相机，我们无从知道它的深度，假设这个深度范围为$(d_{min},+∞)$。因此，该像素对应的空间点就分布在某条线段（本例中是射线）上。在另一个视角（右侧相机）看来，这条线段的投影也形成图像平面上的一条线，称为极线。当知道两部相机间的运动时，这条极线也是能够确定的。那么问题就是：极线上的哪一个点是我们刚才看到的$p_1$点呢？
单个像素的亮度没有区分性，于是比较像素块。我们在$p_1$周围取一个大小为$w\times w$的小块，然后在极线上也取很多同样大小的小块进行比较，就可以在一定程度上提高区分性。这就是所谓的块匹配。
计算小块与小块间的差异，有若干方法：
- SAD（Sum of Absolute Difference）差的绝对值之和
- SSD（Sum of Squared Distance）平方和
- NCC（Normalized Cross Correlation）归一化互相关
$ S_{SAD}=\sum_{i,j}|A(i,j)-B(i,j)| \tag{1} $
$ S_{SSD}=\sum_{i,j}(A(i,j)-B(i,j))^2 \tag{2} $
$ S_{NCC}=\sum_{i,j}\frac{A(i,j)B(i,j)}{\sqrt{\sum_{i,j}A(i,j)^2\sum_{i,j}B(i,j)^2}} \tag{3} $
NCC相关性接近0表示两幅图像不相似，而接近1才表示相似。前面两种距离则是反过来的，接近0表示相似，而大的数值表示不相似。
除了这些简单版本之外，可以先把每个小块的均值去掉 ，称为去均值的SSD、去均值的NCC等。去掉均值之后，我们允许像“小块B比A整体上亮一些，但仍然很相似”，因此比之前的更加可靠一些。
我们得到了一个沿着极线的NCC分布。我们会倾向于使用概率分布来描述深度值，而非用某个单一的数值来描述深度。于是，我们的问题就转到了在不断对不同图像进行极线搜索时，我们估计的深度分布将发生怎样的变化——这就是所谓的深度滤波器。
13.2.3 高斯分布的深度滤波器
设某个像素点的深度d服从高斯分布$P(d)=N(\mu,\sigma^2)$。每当新的数据到来，我们都会观测到它的深度，假设这次观测亦是一个高斯分布$P(d_{obs})=N(\mu_{obs},\sigma_{obs}^2)$。使用观测的信息更新原先d的分布，设融合后的d的分布为$N(\mu_{fuse},\sigma_{fuse}^2)$，根据高斯分布的乘积，两个高斯分布的乘积依然是一个高斯分布，有：
\[\begin{equation} \mu_{fuse}=\frac{\sigma_{obs}^2\mu+\sigma^2\mu_{obs}}{\sigma^2+\sigma^2_{obs}},\ \sigma_{fuse}=\frac{\sigma^2\sigma^2_{obs}}{\sigma^2+\sigma^2_{obs}} \tag{4} \end{equation}\]假设我们通过极线搜索和块匹配确定了参考帧某个像素在当前帧的投影位置。那么，这个位置对深度的不确定性有多大呢？
考虑某次极线搜索，我们找到了$p_1$对应的$p_2$点，从而观测到了$p_1$的深度值，认为$p_1$对应的三维点为$P$。从而，可记$O_1P$为$p$，$O_1O_2$为相机的平移$t$，$O_2P$记为$a$。并且，把这个三角形的下面两个角记作$\alpha,\beta$。现在，考虑极线$l_2$上存在着一个像素大小的误差，使得$\beta$角变成了$\beta’$，而$p$也变成了$p’$，并记上面那个角为$\gamma$。我们要问的是，这一个像素的误差会导致$p’$与$p$产生多大的差距呢？
这是一个典型的几何问题。我们来列写这些量之间的几何关系。显然有：
$ a=p-t,\ \ \alpha=\arccos<p,t>,\ \ \beta=\arccos<a,-t> \tag{5} $
对$p_2$扰动一个像素，将使得$\beta$产生一个变化量$\Delta \beta$，由于相机焦距为f，有（6）。所以（7）（8）。
$ \Delta \beta=\arctan\frac1f \tag{6} $
$ \beta’=\beta+\Delta\beta \tag{7} $
$ \gamma=\pi-\alpha-\beta’ \tag{8} $
于是，由正弦定理，$p’$的大小可以求得：
$ |p’|=|t|\frac{\sin\beta’}{\sin\gamma} \tag{9} $
由此，我们确定了由单个像素的不确定引起的深度不确定性。如果认为极线搜索的块匹配仅有一个像素的误差，那么就可以设：
$ \sigma_{obs}=|p|-|p’| \tag{10} $
在实际工程中，当不确定性小于一定阈值之后，就可以认为深度数据已经收敛了。
综上所述，我们给出了估计稠密深度的一个完整的过程：
- 假设所有像素的深度满足某个初始的高斯分布。
- 当新数据产生时，通过极线搜索和块匹配确定投影点位置。
- 根据几何关系计算三角化后的深度及不确定性。
- 将当前观测融合进上一次的估计中。若收敛则停止计算，否则返回第2步。
13.5.2 八叉树地图
在八叉树中，我们在节点中存储它是否被占据的信息，当某个方块的所有子节点都被占据或都不被占据时，就没必要展开这个节点 。所以八叉树比点云节省大量的存储空间。
13.6 TSDF地图和Fusion系列
在本讲的最后，我们介绍一个与SLAM非常相似但又有稍许不同的研究方向：实时三维重建。
TSDF是Truncated Signed Distance Function的缩写，不妨译作截断符号距离函数。与八叉树相似，TSDF地图也是一种网格式（或者说方块式）的地图。每个TSDF体素内，存储了该小块与距其最近的物体表面的距离。
如果小块在该物体表面的前方，它就有一个正的值；反之，如果该小块位于表面之后，那么就为负值。由于物体表面通常是很薄的一层，所以就把值太大的和太小的都取成1和- 1，这样就得到了截断之后的距离，也就是所谓的TSDF。那么按照定义，TSDF为0的地方就是表面本身——或者，由于数值误差的存在，TSDF由负号变成正号的地方就是表面本身。
由于TSDF没有颜色信息，意味着我们可以只使用深度图 ，不使用彩色图，就完成位姿估计，这在一定程度上摆脱了视觉里程计算法对光照和纹理的依赖性，使得RGB-D重建更加稳健。
