第13-14讲建图

13.1 概述

在经典的SLAM模型中，我们所谓的地图，就是所有路标点的集合，一旦确定了路标点的位置，相当于我们完成了地图的创建。前面讨论的所有内容，都是基于这个地图来的。但是人与机器人对地图的需求不同，对人来说，只有路标点的地图是很难满足需求的，人需要的是地图的引申功能，关于地图的功能，我们主要认为有下面五条：
①定位
定位是地图的一个基本功能，前面我们都讨论了如何根据局部地图来定位，更进一步我们还可以存储地图，让机器人在下次开机时可以直接定位而不需要重新制图。
②导航
导航是指机器人能够在地图中进行路径规划，从任意两个地图点之间寻找路径，然后控制自己运动到目标点的过程，而这个过程中需要知道地图哪些点可通过哪些点不可通过，这种需求对于前面说过的只有路标点的地图来说显然有些力不从心，所以这时就需要使用稠密地图。
③避障
与导航类似，但是更加注重局部的、动态的障碍物的处理。
④重建
有时候人们希望利用SLAM获得周围环境的重建效果，并展示给其他人看，这种情况就需要展示比较美观、舒适的地图。这种地图同样是稠密的，而且除此之外还需要一些额外的要求。
⑤交互
交互主要是人与地图之间的互动。对于这种需求，机器人需要对地图有更高级层面的认识，将语义信息引入地图，这时候地图就称为语义地图。

前面关于SLAM的讨论主要都是集中在稀疏路标地图的环境下，因为定位、制图等内容根据路标点已经足够了，使用稠密地图显然会在运算速度、效率上存在不足。所谓稠密地图是相对于稀疏地图而言的，稀疏地图只建模感兴趣的部分，也就是特征点或者路标点，而稠密地图则是建模看见的全部内容。
对于同一个桌子，稀疏地图可能只建模四个边缘的角，而稠密地图可能会建模整个桌子，显然四个角已经可以用于一定程度的定位，但是导航、避障却需要稠密地图才能完成。

13.2 单目稠密重建

单个图像中的像素，只能提供物体与相机成像平面的角度以及物体采集到的亮度，而无法提供物体的距离，但是在稠密地图中，我们需要得到每个像素点的距离。根据相机类型，我们一般有下面三种解决方案：
①使用单目相机，移动相机之后进行三角化，恢复像素点的深度。
②使用双目相机，利用左右视差计算像素的距离。
③使用RGB-D相机，直接得到像素的距离。
这三种方法前两种被称为立体视觉，相比于使用深度相机，立体视觉有些费力不讨好，我们需要花费大量的计算，而得到的结果是一些不太可靠的深度估计，但是深度相机存在一些使用场景上的限制。

我们首先看一下立体视觉是怎么费力不讨好的。在前面使用特征点恢复深度的时候，我们需要两步，先对图像提取特征点，并根据特征对空间点进行跟踪或者匹配，得到同一个空间点在图像之间的位置。确定了匹配的特征点，我们就可以通过三角化恢复深度信息。
显然，这种利用特征点的方法并不是对于全部的点的方法，对于不是特征点的像素，我们无法恢复深度。在稠密地图估计中，我们无法将每个像素都当做特征点，因此在稠密深度估计问题中，匹配就成为很重要的一环，这里就需要用到极线搜索和块匹配技术，采用这两种方式去，去估计像素在图中的位置，不过不同于特征点的深度估计，这里我们需要使用很多次三角测量，让深度估计收敛到一个很准确的范围，而不是一次直接算出来，这种一点一点准确的过程就需要深度滤波器技术。

前面提到对于一个位姿下图像的观测，由于单目相机不知道深度，所以像素点在实际三维空间中的对应点，实际上是一个不确定的范围，也就是深度在一个区域之内，可以说是最小值到无穷远之间，也就是上图d的范围内，从另一个位姿来看，在我们不知道特征点再当前位姿下的位置时，由于空间点的深度不知道，所以在当前位姿的位置也不确定，但是根据三角关系，p2可能出现的位置在一条线上，这条线是将观测点O2和p1可能出现的空间位置点连线，连线与O2的相机平面的交点组成的一条线，这条线称为极线。所以说，我们知道相机之间的运动，就可以得到这条极线，但问题是我们没法找到极线上的哪一点才是对应点。

在特征点方法中，我们是根据描述子通过特征匹配来确定那个点是匹配点，但是在稠密的限制下，我们不可能对每个点保存描述子，我们只能在极线上找一个与p1长的很像的点，具体来说，我们要沿着极线l2，从一头跑到另一头，在这里面找一个很像的点，从某种程度来看这种方法和直接法有些相似。
确定了寻找的方法，我们需要一个标准去决定什么是相似。但是单个像素的灰度区分度太低，又没有描述子去描述特征，我们采用块匹配的方法，也就是取p1周围一个w×w的小块，在极线上也取很多的等大小的小块进行比较，就可以一定程度上提高区分性。这一点有一些想RandLANet里面用一个点代表周围一些点的方法。

所以现在问题变成了沿着极线，找许多小块，根据小块进行匹配，对于计算小块之间的差异，有很多种方法，下面是三种比较典型的方法：
①SAD
取两个小块对应位置上灰度值差的绝对值之和。

②SSD
取两个小块对应位置上灰度值差的平方和。

③NCC
计算两个小块之间的相关性。

和前面许多的情景一样，我们在计算差异的时候，存在一个效率-精度的矛盾。要高效就会在效果上有所欠缺，精度高的算法必然计算速度就会下降。我们一般需要根据工程需要进行选择。

现在我们根据极线搜索和块匹配，得到了极线上计算了相似性度量，假设我们使用的是NCC，那么我们可以得到一个沿着极线的NCC分布，也就是在极线上不同位置的匹配正确的概率值，分布的形状取决于图像本身的样子，在搜索距离较长的时候，我们通常会得到一个非凸函数，存在许多峰值，但是真实的对应点只有一个。也就是说对应位置是一个概率表示的位置，所以估计的深度也应该是一个概率值，所以问题就转换到了对不同图像进行极线搜索的时候，我们估计的深度分布会发生怎样一个变化，这就是深度滤波器。

对于像素点深度的估计，本身也可以看作是一个状态估计问题，自然就存在滤波器和非线性优化两种思路，在建图方面一般选择计算量较少的滤波器方法。我们一般假设深度服从高斯分布，得到一种类似于卡尔曼滤波的方法。

假设深度d服从一个正态分布：

每次有新的图像到来，我们都会观测到一个新的深度，假设这个深度服从另一个高斯分布：

所以现在我们相当于需要融合两个高斯分布来更新我们对深度的分布，于是有：

由于我们仅有观测方程而没有运动方程，所以深度只用到了信息融合，而没有使用完整的卡尔曼滤波那样进行预测和更新。

关于新估计的深度的两个高斯分布参数，存在许多种不同的处理方法，我们暂时只考虑几何关系带来的不确定性，也就是偏差值б的确定。
假设我们通过极线搜索和块匹配，确定了一个投影位置，这个投影位置会对深度产生一定的不确定性，也就是位置估计的偏移会让深度估计产生多大的偏移。

从图中可以看出，位置估计相差一个像素，会在深度估计上产生p-p’的误差，这是一个几何问题，几个重要的量的关系如下：

现在我们对p2增加一个扰动，大小为一个像素，这会让β产生一个变化量，相机焦距为f的情况下，会有：

所以

根据正弦定理，可以求的p’的值：

所以我们确定了单个像素的不确定性导致的深度不确定性：

之后就可以进行数据的融合，在实际使用时，我们一般认为当不确定性小于一定值的时候，数据的融合就已经收敛了。
所以，估计稠密深度的过程可以总结为：

做一个小总结，由于我们没有特征点之间的匹配关系，所以我们采用在极线上找最优匹配点的方法，得到的结果也不是一个确定的值，而是一个可能的概率，所以我们在此基础上得到的深度也是一个概率值，我们希望分布尽可能准确，所以需要多次融合概率分布，每次都需要计算深度和不确定性，其中不确定性对应的就是高斯分布中的离散程度，不确定性是用像素偏移对深度偏移的影响来衡量的。重复迭代直到收敛，此时得到的就是比较靠谱的深度估计。

对于上面的极线搜索和块匹配，其实存在一些不足，下面从几个方面来看一下。
首先，在块匹配的过程中，匹配的结果十分依赖图像是否有区分度，在缺少有效信息的情况下，计算的NCC值会完全不可靠。所以在进行块匹配的时候，我们必须要假设小块不变，这里和光流法里面假设灰度不变性一个道理，小块不变的情况下才可以与其他小块进行匹配。这种情况下，有明显梯度的小块将具有良好的区分度，对于梯度不明显的像素，块匹配时就没有区分性，所以将很难有效估计其深度。因此，立体视觉实际上是对物体纹理有很大的依赖性，这个问题在双目视觉中也极其常见，体现了立体视觉的重建质量，十分依赖于环境纹理。所以在实际应用中，对于墙面、光滑物体表面这种亮度均匀的地方，深度估计会是一个很大的问题。

上图展示了一个很好的例子，这是两种比较极端的情况：像素梯度垂直于极线方向以及平行于极线方向。在平行的例子里，即使小块有明显梯度，但是当我们沿着极线去做块匹配时，会发现匹配程度都是一样的，因此得不到有效的匹配。反之，在垂直的例子里，我们能够精确地确定匹配度最高点出现在何处。实际情况中这两个极端场景不会经常出现，一般都是位于二者之间，这时我们认为，当像素梯度与极线夹角较小时，极线匹配的不确定性大；而当夹角较大时，匹配的不确定性变小。

其次，我们可以考虑一下，将深度的分布看做高斯分布是不是合理。其实笼统地将所有场景下的深度分布看作高斯分布是不合理的，一方面，场景的深度是从一个最小值到一个很大的值，这个分布很可能形成不了一个对称的形状，应该是尾部稍长，负数区域为0的形状。另一方面，在一些室外的场景下，可能存在距离很远的点，甚至无穷远的点，利用高斯分布显然很难表述这种点。于是就诞生了逆梯度，即认为深度的倒数为高斯分布。这种方式改进原来深度的分布，实际应用中一般采用这种方法，它具有更好的数值稳定性。

此外，在块匹配之前做一次图像到图像的变换是一种常用的预处理方法，前面我们假设了小块不变性，这个假设在平移的时候能够保持成立，但是发生明显旋转的时候却很难保证。为了防止这种情况的出现，我们需要在块匹配前，将参考帧和当前帧之间的运动考虑进来。
根据相机模型，参考帧的像素PR与真实三维点OW应该有如下的关系：

类似的，对于当前帧，它亦有PW在它上边的投影，记作PC：

代入并消去PW，即得两个图像之间的像素关系：

通过这种方式，算出在局部范围内，参考帧和当前帧图像坐标变换的一个线性关系。根据这个变换关系，我们可以将当前帧的像素进行变换后再进行块匹配，从而实现对旋转更好的判断效果。

13.5 RGB-D稠密建图

在RGB-D相机中可以完全通过传感器中硬件测量得到，无需消耗大量的计算资源来估计它们。并且，RGB-D的结构光或子弹时间原理，保证了深度数据对纹理的无关性。即使面对纯色的物体，只要它能够反射光，我们就能测量到它的深度。这亦是RGB-D传感器的一大优势。利用深度相机进行稠密建图是相对简单的，最简单的方法就是根据估算的相机位姿，将深度相机数据转换为点云，拼接之后就得到了一个由离散点组成的点云地图。点云地图为我们提供了比较基本的可视化地图，让我们能够大致了解环境的样子。它以三维方式存储，使得我们能够快速地浏览场景的各个角落，乃至在场景中进行漫游。点云的一大优势是可以直接由RGB-D图像高效地生成，不需要额外的处理。它的滤波操作也非常直观，且处理效率尚能接受。
点云地图在一些前端处理方式下可以满足定位需求，无法直接用于导航和避障，具有基本的可视化和交互能力。我们说点云地图是“基础”的或“初级的”，是指它更接近于传感器读取的原始数据。它具有一些基本的功能，但通常用于调试和基本的显示，不便直接用于应用程序。如果我们希望地图有更高级的功能，点云地图是一个不错的出发点。

点云地图的存储是个很大的问题，点云规模大，而且除非降低分辨率，否则在有限的内存中无法建模较大的环境。而且点云无法处理运动物体，因为我们的做法里只有“添加点”，而没有“当点消失时把它移除”的做法。而在实际环境中，运动物体的普遍存在，使得点云地图变得不够实用。
对于这个问题，我们采用八叉树这个结构来改进，它可以是一种灵活的、压缩的、可以随时更新的地图形式。

可以看出，这个结构就是对空间进行划分，每次划分是对空间的三个方向各划一刀，于是会产生八个块，对应到树结构上就是一层产生八个儿子节点，也就是八叉树。整个大方块可以看成是根节点，而最小的块可以看作是“叶子节点”。于是，在八叉树中，当我们由下一层节点往上走一层时，地图的体积就能扩大八倍。体积与深度成指数关系，所以当我们用更大的深度时，建模的体积会增长的非常快。
我们认为八叉树可以节省存储，主要是在于当某个方块的所有子节点都被占据或都不被占据时，就没必要展开这个节点。例如，一开始地图为空白时，我们就只需一个根节点，而不需要完整的树。当在地图中添加信息时，由于实际的物体经常连在一起，空白的地方也会常常连在一起，所以大多数八叉树节点都无需展开到叶子层面。所以说，八叉树比点云节省了大量的存储空间。

另外，在八叉树的具体存储上，我们一般用一个概率值来表示节点是否被占用这件事，比方说，用一个浮点数 x ∈ [0, 1] 来表达。这个 x 一开始取0.5。如果不断观测到它被占据，那么让这个值不断增加；反之，如果不断观测到它是空白，那就让它不断减小即可。通过这种方式，我们动态地建模了地图中的障碍物信息。
当然，一直减小或者增加必然会溢出合法范围，所以这里实际上是用概率对数值来描述：

当y从−∞变到+∞时，x相应地从0变到了1。而当y取0时，x取到0.5。用数学形式来说，设某节点为n，观测数据为z。那么从开始到t时刻某节点的概率对数值为L(n|z1:t)，那么t+1时刻为：

如果写成概率形式而不是概率对数形式，就会有一点复杂：

有了对数概率，我们就可以根据 RGB-D 数据，更新整个八叉树地图了。假设我们在RGB-D图像中观测到某个像素带有深度d，这说明了一件事：我们在深度值对应的空间点上观察到了一个占据数据，并且，从相机光心出发，到这个点的线段上，应该是没有物体的（否则会被遮挡）。利用这个信息，可以很好地对八叉树地图进行更新，并且能处理运动的结构。