视觉SLAM-13

第十三章 稠密地图

地图的应用大致可分为：1）定位。2）导航：在这个过程中，需要知道地图中哪些地方可以通过，哪些地方不可通过，而要能实现这样的功能，则至少是稠密地图。3）避障：与导航类似，但更加关注局部的、动态的障碍物的处理，同样也需要至少是稠密的地图。4）重建：利用SLAM获取周围环境的重建效果，主要是用于向他人展示。并且我们对这种地图还有外观要求，所以这样地图也是稠密的。5）交互：主要指人与地图之间的互动。语义地图。

其中定位是稀疏路标地图；导航/避障/重建是稠密地图；交互是语义地图。

1. 单目稠密重建

在稠密重建中，我们需要知道每一个像素点（或大多数像素点）的距离，有三种解决方法：

• 1）使用单目相机，通过移动相机之后进行三角化测量像素的距离。

• 2）使用双目相机，利用左右目的视差计算像素的距离，与多目原理相同。

• 3）使用RGB-D相机直接获得像素距离。

前两种方法又称为立体视觉（Stereo Vision）：立体视觉的重建质量十分依赖于环境纹理。单目又称为移动视角的立体视觉。

这三种方法的优缺点对比：相比于RGB-D直接测量的深度，单目和双目对深度的获取往往是“费力不讨好“的：因为我们要花大量的时间用于计算，最后得到一些并不怎么可靠的深度估计。所以往往我们选择用RGB-D进行稠密重建。但是RGB-D有一些量程、应用范围和光照的限制。单目、双目的好处是：对于目前RGB-D无法很好应用的室外、大场景场合中，仍能通过视觉立体估计深度信息。

在稠密深度图估计中，我们不能把每个像素都当作特征点计算描述子，所以，这里我们采用极线搜索和块匹配技术来确定第一幅图的某像素出现在其他图里的位置。

当我们知道了某个像素在各个图中的位置，就能像特征点那样，利用三角测量确定它的深度。这里要注意的是我们要使用多次三角测量让深度估计收敛，而不仅仅是一次。用到的是深度滤波器技术（深度估计能够随着测量的增加从一个非常不确定的量逐渐收敛至一个稳定值）。

(1) 极线搜索与块匹配

如图所示，已知P位于图片1的p1，但是不知道距离，只知道P在射线O1p1上。在图片2上，P的位置在极线 l2 上，我们要在极线上找出P在图片2的对应像素p2。因为单个像素的亮度没有区别性，可能会找到很多个相似的点。所以这里比较像素周围的像素块：在p1周围取大小为w×w的像素块，在极线上的每个像素点周围也取同样大小的像素块进行比较，可以在一定程度上提高区分性，这就是块匹配。

假设p1周围的块记为A，极线上像素点的块记为Bi，计算小块之间的差异：

1. SAD，取两个小块的差的绝对值之和：

$$S(A,B{)}_{SAD}=\sum _{i,j}|A(i,j)-B(i,j)|$$

2. SSD，取两个小块的差的平方和：

   $$S(A,B{)}_{SSD}=\sum _{i,j}(A(i,j)-B(i,j){)}^2$$

3. NCC，归一化互相关，计算小块的相关性：
   $$S(A,B{)}_{NCC}=\frac{{\sum }_{i,j}A(i,j)B(i,j)}{\sqrt{{\sum }_{i,j}A(i,j{)}^2{\sum }_{i,j}B(i,j{)}^2}}$$

其中1,2是越小表示越相似，3是越接近1表示越相似。还可以先去掉小块的均值再进行计算，称为去均值的SSD，去均值的NCC。去掉均值后允许小块B比小块A整体更亮。

(2) 高斯分布的深度滤波器

例如使用NCC可以得到一个非凸函数，其中有许多的峰值，但是真实对应点只有一个，这种情况下得到的深度不准确，所以倾向于使用概率分布来描述深度值。一个空间点可以出现在不同的图像中，所以可以得到多个深度，对其进行概率分布，得到更准确的深度信息。

估计像素点的深度，可建模为一个状态估计问题。所以有滤波器和非线性优化两种求解思路。而SLAM实时性要求较高，前端已经占据了不少的计算量，所以在建图中，我们通常采用计算量较少的滤波器方式。

假设深值服从高斯分布，估计稠密深度的一个完整过程：（最初会初始化一个数学期望和方差）

（1）假设所有像素的深度满足某个初始的高斯分布

（2）当新数据产生时，通过极线搜索和块匹配确定投影点的位置

（3）根据几何关系计算三角化后的深度及不确定性

（4）将当前观测融合进上一次的估计中，若收敛则停止计算，否则返回第（2）步。

其中高斯分布的数学期望作为深度值，方差作为该深度的不确定性，通过计算并判断方差大小来确定该深度值是否可行。

方差和数学期望的更新：
$$\begin{gathered} {\mu }_{fuse}=\frac{{\sigma }_{obs}^2\mu +{\sigma }^2{\mu }_{obs}}{{\sigma }^2+{\sigma }_{obs}^2} \\ {\sigma }_{fuse}^2=\frac{{\sigma }^2{\sigma }_{obs}^2}{{\sigma }^2+{\sigma }_{obs}^2} \end{gathered}$$
以上不带角标的是已知量，存在深度地图和深度方差地图中。obs是观察量，就是当前图像的计算量，通过书本P326-327可以知道计算sigma的过程，mu是当前图像通过三角法计算出来的深度值。然后通过上述公式进行更新参数。用计算出来的量代替没有角标的量。

(3)分析

a. 对于梯度不明显的像素，由于块匹配没有区分性，所以很难有效估计深度。立体视觉一个非常常见的问题：对物体纹理的依赖性。

当像素梯度与极线夹角较大时，极线匹配的不确定性大；反之，不确定性小。如果极线与像素梯度垂直，所有的匹配程度都一样，也就无法得到有效的匹配，而当极线与梯度平行，就能精确地确定匹配度最高点出现在何处P340图13-7。

b. 逆深度就是指深度的倒数，把逆深度假设为服从高斯分布更为准确，并且逆深度也具有更好的数值稳定性。

c. 小块不变性是建立在相机不旋转基础上的，如果相机发生旋转，同一个小块在两个图像中就不同了。所以在块匹配前，要考虑图像间的变换。

d. 并行化-效率的问题。在进行稠密深度图的估计时，对几十万个像素点的深度估计是彼此无关的，所以可以利用并行化。

e. 如果使用并行化使得各像素独立计算，可能存在这个像素深度很小，边上一个又很大的情况。我们可以假设深度途中相邻的深度变化不会太大，从而给深度估计加上了空间正则项。这种做法会让深度图更加平滑。

f. 显示处理外点（Out lier)。处理错误匹配。

2. RGB-D稠密建图

(1) 点云地图

利用RGB-D进行稠密建图的好处：

1）利用RGB-D相机可以完全通过传感器中硬件测量得到深度，而无须像单目和双目中那样通过消耗大量的计算资源来估计。

2）RGB-D的结构光或飞时原理，保证了深度数据对纹理的无关性。

根据地图形式的不同，存在着若干种不同的主流建图方式：

1）最直观、最简单的方式：根据估算的相机位姿，将RGB-D数据转化为点云（Point Cloud），然后进行拼接，最后得到一个由离散的点组成的点云地图。

2）如果我们希望估计物体的表面，可以使用三角网格(Mesh)、面片(Surfel)进行建图。

3）如果我们想要知道地图的障碍物信息并在地图上导航，亦可通过体素（Voxel）建立占据网格地图（Occupancy Map）。

实践：点云地图，与第五章的相似，只不过加入了外点去除滤波器和降采样滤波器。

(2) 八叉树地图

点云地图明显的缺陷：

1）点云地图通常规模很大，所以得到的pcd文件也会很大，需要大量的存储空间。然而这种“大”并不是必须的，它存储了很多不必要的细节。由于这些空间的占用，除非降低分辨率（降低分辨率又会导致地图质量下降），否则在有限的内存中，无法建模较大规模的环境。

2）点云地图无法处理运动物体。因为在生成点云地图时，我们只有“添加点”，而没有“当点消除时把它移除”的做法。但是生命在于运动，现实中，运动的物体又普遍存在，这使得点云地图不怎么实用。

所以现在我们来考虑一种灵活的、压缩的、又能随时更新的地图形式——八叉树（Octo-tree）

将一个方块平均切成大小相同的八个方块（如下图所示），这个步骤可以不断重复，直到最后的方块大小达到建模的最高精度。整个从最大空间细分到最小空间的过程，就是一个八叉树。八叉树节省空间的原因：

• 在八叉树中我们只储存它是否被占用，使用0,1就可以；
• 当某个方块的所有子节点都被占用或不被占用时，就没必要展开这个节点。

由于噪声影响，一个小块可能一会为1,一会为0。为了消除噪声影响，我们选择用概率表达是否被占用。用x表示，范围是[0,1]，初始化为0.5，如果检测被占用，就增加这个值，如果是不被占用，就减小这个值。x小于0.5是不占用，大于0.5是被占用。由于一直增加可能会超出1，所以使用log函数来变化：
$$\begin{gathered} y=logit(x)=log(\frac{x}{1-x}) \\ x=logi{t}^{-1}(x)=\frac{exp(y)}{exp(y)+1} \end{gathered}$$
所以可以知道y从负无穷到正无穷，x就从0到1。初始化y为0，对y进行加减操作即可，通过变换得到x概率值。

假设我们在图像中观察到了某个像素点带有深度d，这说明深度值对应的空间点上观察到了一个占用数据，并且从相机光心出发到这个点的线段上，应该是没有物体的。

查看八叉树地图：octovis octomap.bt