SLAM入门之建图

13 建图 Mapping

13.1 概述

在经典的 SLAM 模型中，地图即所有路标点的集合。SLAM 作为一种底层技术，往往是用来为上层应用提供信息的。应用层面对于“定位”的需求是相似的，他们希望 SLAM 提供相机或搭载相机的主体的空间位姿信息。而对于地图，则存在着许多不同的需求。

• 定位：定位是地图的一个基本功能。稀疏路标地图。
• 导航：在地图中进行路径规划，需要知道地图中哪些地方不可通过，而哪些地方是可以通过的。稠密地图。
• 避障：与导航类似，但更注重局部的、动态的障碍物的处理。稠密地图。
• 重建：稠密点云重建。稠密地图
• 交互：指人与地图之间的互动。语义地图。

稠密地图是指建模所有看到过的部分。对于同一个桌子，稀疏地图可能只建模了桌子的四个角，而稠密地图则会建模整个桌面。虽然从定位角度看，只有四个角的地图也可以用于对相机进行定位，但由于我们无法从四个角推断这几个点之间的空间结构，所以无法仅用四个角来完成导航、避障等需要稠密地图才能完成的工作。

13.2 单目稠密重建

13.2.1 立体视觉

相机，很久以来被认为是只有角度的传感器。单个图像中的像素，只能提供物体与相机成像平面的角
度以及物体采集到的亮度，而无法提供物体的距离（Range）。而在稠密重建，我们需要知道每一个像素点（或大部分像素点）的距离，那么大致上有以下几种解决方案：

1. 使用单目相机，利用移动相机之后进行三角化，测量像素的距离。
2. 使用双目相机，利用左右目的视差计算像素的距离（多目原理相同）。
3. 使用 RGB-D 相机直接获得像素距离。但有一些量程、应用范围和光照的限制。

• 1和2称为立体视觉（Stereo Vision），其中移动单目的又称为移动视角的立体视觉（Moving View Stereo）。 单目和双目对深度的获取计算量大，深度估计不怎么可靠。在目前 RGB-D还无法很好应用在室外、大场景场合中，仍能通过立体视觉估计深度信息。

• 单目稠密估计
  在给定相机轨迹的基础上，如何根据一段时间的视频序列，来估计某张图像的深度？
  假定有某一段视频序列，已知每一帧对应的轨迹。现以第一张图像为参考帧，计算参考帧中每一个
  像素的深度：

  • 首先确定第一张图的某像素出现在其他图里的位置，这需要用到极线搜索和块匹配技术。
  • 然后通过多次三角测量让深度估计收敛。这就是深度滤波器技术。

13.2.2 极线搜索与块匹配

• 极线：左边的相机观测到了某个像素 $p_1$，假设深度范围为 $(dmin,+\infty )$。因此，该像素对应的空间点就分布在某条线段（本例中是射线）上。在右侧相机看来，这条线段的投影也形成图像平面上的一条线，这称为极线。当我们知道两个相机间的运动时，这条极线也是能够确定的。
• 块匹配：需要在极线上找到$p_1$对应的点，若采用特征点法没有描述子，若直接法的单个像素亮度值不可靠且存在两点相似的问题。因此在 $p_1$ 周围取一个大小为 $w\times w$ 的小块，然后在极线上也取很多同样大小的小块进行比较，就可以一定程度上提高区分性。这就是块匹配。条件是图像块的灰度不变。
• 记 $p_1$ 周围小块为 $A\text{A}A\in {\mathbb{R}}^{w\times w}$，极线上的 $n$ 个小块 ${B\text{B}B}_i,i=1,\dots ,n$。计算小块与小块间的差异的方法：
  • SAD：两个小块的差的绝对值之和 $S(A\text{A}A,B\text{B}B{)}_{SAD}={\sum }_{i,j}|A\text{A}A(i,j)-B\text{B}B(i,j)|$
  • SSD：平方和 $S(A\text{A}A,B\text{B}B{)}_{SSD}={\sum }_{i,j}(A\text{A}A(i,j)-B\text{B}B(i,j){)}^2$
  • NCC：归一化互相关 $S(A\text{A}A,B\text{B}B{)}_{NCC}=\frac{{\sum }_{i,j}A\text{A}A(i,j)B\text{B}B(i,j)}{\sqrt{{\sum }_{i,j}A\text{A}A(i,j{)}^2{\sum }_{i,j}B\text{B}B(i,j{)}^2}}$
    相关性接近 0 表示两个图像不相似，而接近 1才表示相似。
  • 去均值的 SSD、去均值的 NCC 等：去掉均值之后，我们允许像“小块 B 比 A 整体上亮一些，但仍然很相似”这样的情况
    $NC{C}_z(A\text{A}A,B\text{B}B)=\frac{{\sum }_{i,j}(A\text{A}A(i,j)-\bar{A}\text{Aˉ}\bar{A}(i,j))(B\text{B}B(i,j)-\bar{B}\text{Bˉ}\bar{B}(i,j))}{\sqrt{{\sum }_{i,j}(A\text{A}A(i,j)-\bar{A}\text{Aˉ}\bar{A}(i,j){)}^2{\sum }_{i,j}(B\text{B}B(i,j)-\bar{B}\text{Bˉ}\bar{B}(i,j){)}^2}}$

计算了 $A\text{A}A$ 与每一个 ${B\text{B}B}_i$ 的相似性度量后得到一个沿着极线的 NCC 分布。这个分布的形状严重取决于图像本身的样子，如下图。在搜索距离较长的情况下，我们通常会得到一个非凸函数：这个分布存在着许多峰值，然而真实的对应点必定只有一个。

在这种情况下，我们会倾向于使用概率分布来描述深度值，而非用某个单一个的数值来描述深度。于是，我们的问题就转到了，在不断对不同图像进行极线搜索时，我们估计的深度分布将发生怎样的
变化——这就是深度滤波器。

13.2.3 高斯分布的深度滤波器

• 对像素点深度的估计，可以用滤波器和非线性优化两种思路。由于前端已经占用不少计算量了所以建图常采用计算量小的滤波器。

• 在简单的假设条件下，可以假设深度值服从高斯分布，得到一种类卡尔曼式的方法

  • 设某个像素点的深度 d 服从：$P(d)=N(\mu ,{\sigma }^2)$
  • 新的观测数据$d_{obs}$ 服从：$P(d_{obs})=N({\mu }_{obs},{\sigma }_{obs}^2)$
  • 使用观测的信息，更新原先 d 的分布。由高斯分布乘积仍是高斯分布得：${\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}$
  • 由于我们仅有观测方程而没有运动方程，所以这里深度仅用到了信息融合部分，而无须像完整的卡尔曼那样进行预测和更新。

• 确定观测的深度分布，即计算 ${\mu }_{obs},{\sigma }_{obs}$：（这里仅考虑几何不确定性，还有光度不确定性）

  • 在极线搜索后已知 ${p\text{p}p}_1$ 对应 ${p\text{p}p}_2$ ，观测到 ${p\text{p}p}_1$ 的深度值。${p\text{p}p}_1$ 对应 $P$ 点，记 $O_{1}P$ 为 ${p\text{p}p}_1$，$O_1{O}_2$ 为 相机的平移 $t\text{t}t$ ，$O_{2}P$ 记为 $a\text{a}a$。并且，把这个三角形的下面两个角记作 $\alpha ,\beta$。现在，考虑极线 $l_2$ 上存在着一个像素大小的误差，使得 $\beta$ 角变成了 ${\beta }^{\prime }$，而 $p\text{p}p$ 也变成了 ${p\text{p}p}^{\prime }$，并记上面那个角为 $\gamma$
  • 这一个像素的误差，会导致 ${p\text{p}p}^{\prime }$ 与 $p\text{p}p$ 产生多大的差距呢?
    • $a\text{a}a=p\text{p}p-t\text{t}t$
    • $\alpha =\arccos ⟨p\text{p}p,t\text{t}t⟩$
    • $\beta =\arccos ⟨a\text{a}a,-t\text{t}t⟩$
    • 对 ${p\text{p}p}_2$ 绕动一个像素， 将使得 $\beta$ 产生一个变化量 $\delta \beta$，由于相机焦距为 $f$
      • $\delta \beta =\arctan \frac{1}{f}$
      • ${\beta }^{\prime }=\beta +\delta \beta$
      • $\gamma =\pi -\alpha -\beta$
      • 由正弦定理：$||{p\text{p}p}^{\prime }||=||t\text{t}t||\frac{\sin {\beta }^{\prime }}{\sin \gamma }$
  • 由此，我们确定了由单个像素的不确定引起的深度不确定性。如果认为极线搜索的块匹配仅有一个像素的误差，那么就可以设：${\sigma }_{obs}=||p\text{p}p||-||{p\text{p}p}^{\prime }||$

• 估计稠密深度的完整的过程

1. 假设所有像素的深度满足某个初始的高斯分布；
2. 当新数据产生时，通过极线搜索和块匹配确定投影点位置；
3. 根据几何关系计算三角化后的深度以及不确定性；
4. 将当前观测融合进上一次的估计中。若收敛则停止计算，否则返回 2。

在实际工程中，当不确定性小于一定阈值之后，就可以认为深度数据已经收敛了

13.3 实验分析与讨论

• 像素梯度

  • 有明显梯度的小块将具有良好的区分度，不易引起误匹配。对于梯度不明显的像素，由于在块匹配时没有区分性，所以我们将难以有效地估计其深度。反之，像素梯度比较明显的地方，我们得到的深度信息也相对准确。这反映了立体视觉中一个非常常见的问题： 对物体纹理的依赖性。该问题在双目视觉中也极其常见，体现了立体视觉的重建质量，十分依赖于环境纹理。从某种角度来说，该问题是无法在现有的算法流程上加以改进并解决的——如果我们依然只关心某个像素周围的邻域（小块）的话。
  • 像素梯度和极线之间存在联系：当像素梯度与极线夹角较小时，极线匹配的不确定性大；而当夹角较大时，匹配的不确定性变小。
    

• 逆深度

  • 把像素深度假设成高斯分布，是否合适呢？这里关系到一个参数化的问题（Parameterization）
  • 用 $x,y,z$ 表示世界坐标是一种参数化形式，$u,v,d$ 也是一种参数化形式，这里认为 $u,v$ 比较确定且与 $d$ 近似独立。如果假设 $d$ 服从高斯分布，存在一些问题：
    • 我们实际想表达的是：这个场景深度大概是 5-10 米，可能有一些更远的点，但近处肯定不会小于相机焦距（或认为深度不会小于 0）。这个分布并不是像高斯分布那样，形成一个对称的形状。它的尾部可能稍长，而负数区域则为零。
    • 在一些室外应用中，可能存在距离非常远，乃至无穷远处的点。我们的初始值中难以涵盖这些点，并且用高斯分布描述它们会有一些数值计算上的困难。
  • 假设深度的倒数，即逆深度，为高斯分布是比较有效的。 $d$逆深度（Inverse depth）是近年 SLAM 研究中，出现的一种广泛使用的参数化。从正深度改成逆深度只要将 $d$ 改成逆深度 $d^{-1}$ 即可。

• 图像间的变换

  • 在块匹配之前，需要做一次图像到图像间的变换。这是因为当相机绕光心旋转时，一个下黑上白的图像可能会变成一个上黑下白的图像块，导致相关性直接变成了负数（尽管仍然是同样一个块）。通常需要在块匹配之前，把参考帧与当前帧之间的运动考虑进来。
  • 根据相机模型，记参考帧上的一个像素为 ${P\text{P}P}_R$，三维点世界坐标为 ${P\text{P}P}_W$，${P\text{P}P}_W$在当前帧上的投影 ${P\text{P}P}_C$，有如下关系：
    • $d_R{P\text{P}P}_R=K\text{K}K({R\text{R}R}_{RW}{P\text{P}P}_W+{t\text{t}t}_{RW})$
    • $d_C{P\text{P}P}_C=K\text{K}K({R\text{R}R}_{CW}{P\text{P}P}_W+{t\text{t}t}_{CW})$
    • 消去 ${P\text{P}P}_W$ 得：$d_C{P\text{P}P}_C=d_{R}K\text{K}K{R\text{R}R}_{CW}{K\text{K}K}^{-1}{P\text{P}P}_R+K\text{K}K{t\text{t}t}_{CW}-K\text{K}K{R\text{R}R}_{CW}{R\text{R}R}_{RW}^{T}K\text{K}K{t\text{t}t}_{RW}$
    • 当知道 $d_R,{P\text{P}P}_R$ 时，可算出 ${P\text{P}P}_C$ 的投影位置。此时，再给 ${P\text{P}P}_R$ 两个分量各一个增量 $du,dv$，就可以求得 PC 的增量 $d{u}_c,d{v}_c$。
    • 在局部范围内，参考帧和当前帧图像坐标变换的一个线性关系，构成仿射变换：$\left[\begin{array}{c}d{u}_c\\ d{v}_c\end{array}\right]=\left[\begin{array}{cc}\frac{d{u}_c}{du}& \frac{d{u}_c}{dv}\\ \frac{d{v}_c}{du}& \frac{d{v}_c}{dv}\end{array}\right]\left[\begin{array}{c}du\\ dv\end{array}\right]$
  • 当前帧（或参考帧）的像素进行变换后，再进行块匹配，以期获得对旋转更好的效果。

• 并行化：效率的问题
  稠密深度图的估计像素点此无关的！GPU 的并行计算架构非常适合这样的问题，在单双和双目的稠密重建中，经常看到利用 GPU 进行并行加速的方式。

• 其他的改进

  • 各像素独立计算时可能存在这个像素深度很小，边上一个又很大的情况。我们可以假设深度图中相邻的深度变化不会太大，从而给深度估计加上了空间正则项。这种做法会使得到的深度图更加平滑。
  • 由于遮挡、光照、运动模糊等各种因素的影响，不可能对每个像素都能保持成功的匹配。均匀——高斯混合分布下的深度滤波器，显式地将内点与外点进行区别并进行概率建模，能够较好的处理外点数据。

13.4 RGB-D 稠密建图

RGB-D 相机中可以完全通过传感器中硬件测量得到，无需消耗大量的计算资源来估计它们。并且RGB-D 的结构光或飞时原理，保证了深度数据对纹理的无关性。即使面对纯色的物体，只要它能够反射光，我们就能测量到它的深度。

• 根据估算的相机位姿，将 RGB-D 数据转化为点云（Point Cloud），然后进行拼接，最后得到一个由离散的点组成的点云地图（Point Cloud Map）。

  • RGB-D 相机提供了彩色图和深度图，根据相机内参来计算 RGB-D 点云。如果通过某种手段，得到了相机的位姿，那么只要直接把点云进行加和，就可以获得全局的点云。
  • 对点云进行处理时需要注意在生成每帧点云时，去掉深度值太大或无效的点。这主要是考虑到 Kinect 的有效量程，超过量程之后的深度值会有较大误差。
  • 利用统计滤波器方法去除孤立点。该滤波器统计每个点与它最近 N 个点的距离值的分布，去除距离均值过大的点。这样，我们保留了那些“粘在一起”的点，去掉了孤立的噪声点。
  • 利用体素滤波器（Voxel Filter）进行降采样。由于多个视角存在视野重叠，在重叠区域会存在大量的位置十分相近的点。这会无益地占用许多内存空间。体素滤波保证在某个一定大小的立方体（或称体素）内仅有一个点，相当于对三维空间进行了降采样，从而节省了很多存储空间。在体素滤波器中，分辨率0.01表示每立方厘米有一个点。

• 如果对外观有进一步的要求，希望估计物体的表面，可以使用三角网格（Mesh），面片（Surfel）进行建图。在 SfM 中常用的泊松重建方法，就能通过基本的点云重建物体网格地图，得到物体的表面信息。或是利用Surfel作为 表达物体表面的方式，以面元作为地图的基本单位，能够建立漂亮的可视化地图。它们都可以通过点云进行构建。

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

• 八叉树地图

  • 一种在导航中比较常用的，本身有较好的压缩性能的地图形式
  • 点云地图规模很大，提供了很多不必要的细节。无法处理运动物体。我们的做法里只有“添加点”，而没有“当点消失时把它移除”的做法。
  • 八叉树（Octo-map）
    • 三维空间可以建模为许多个小方块（或体素）：把一个小方块的每个面平均切成两片，那么这个小方块就会变成同样大小的八个小方块。这个步骤可以不断的重复，直到最后的方块大小达到建模的最高精度。在这个过程中，把“将一个小方块分成同样大小的八个”这件事，看成“从一个节点展开成八个子节点”，那么，整个从最大空间细分到最小空间的过程，就是一棵八叉树（Octo-tree）。
      
    • 在八叉树中，当我们由下一层节点往上走一层时，地图的体积就能扩大八倍。如果叶子节点的方块大小为 1 $c{m}^3$，那么当我们限制八叉树为 10 层时，总共能建模的体积大约为 $8^{10}=1073m^3$，这足够建模一间屋子。在八叉树中，节点存储它是否被占据的信息。与点云体素滤波器中一个体素只有一个点不同的是，当某个方块的所有子节点都被占据或都不被占据时，就没必要展开这个节点。例如，一开始地图为空白时，我们就只需一个根节点，而不需要完整的树。当在地图中添加信息时，由于实际的物体经常连在一起，空白的地方也会常常连在一起，所以大多数八叉树节点都无需展开到叶子层面。所以说，八叉树比点云节省了大量的存储空间。
    • 由于噪声的影响，选择用概率形式表达某节点是否被占据的事情。如，用一个浮点数 $x\in [0,1]$ 来表达。这个 $x$ 一开始取 0.5。如果不断观测到它被占据，那么让这个值不断增加；反之，如果不断观测到它是空白，那就让它不断减小即可。通过这种方式，我们动态地建模了地图中的障碍物信息。
    • 如果让 $x$ 不断增加或减小，它可能跑到 [0, 1] 区间之外，所以采用概率对数值（Log-odds）来描述。
      • 设 $y\in R$ 为概率对数值， $x$ 为 0 到 1 之间的概率，那么它们之间的变换由 $logit$ 变换描述：$y=logit(x)=\log (\frac{x}{1-x})$，$x=logi{t}^{-1}(y)=\frac{\exp (y)}{\exp (y)+1}$
      • 当 y 从 −1 变到 +1 时， x 相应地从 0 变到了 1。而当 y 取 0 时， x 取到 0.5。我存储 y 来表达节点是否被占据，当不断观测到“占据”时，让 y增加一个值；否则就让 y 减小一个值。当查询概率时，再用逆 logit 变换，将 y 转换至概率即可。
      • 设某节点为 n，观测数据为 z。那么从开始到 t 时刻某节点的概率对数值为 $L(n|z_{1:t})$，那么 t + 1 时刻为：$L(n|z_{1:t})=L(n|z_{1:t-1})+L(n|z_t)$
    • 假设我们在RGB-D 图像中观测到某个像素带有深度 d，这说明了一件事：我们在深度值对应的空间点上观察到了一个占据数据，并且，从相机光心出发，到这个点的线段上，应该是没有物体的（否则会被遮挡）。利用这个信息，可以很好地对八叉树地图进行更新，并且能处理运动的结构。

13.5 TSDF 地图和 Fusion 系列

• 该方向为实时三维重建，牵涉到 GPU 编程。
• 现有做法中，我们并没有对稠密地图进行优化，我们只根据了两张图像的位姿，把两处的点云进行叠加，地图中会出现物体的重影。我们希望重建结果是光滑的、完整的，因此出现了实时三维重建是一种以“建图”为主体，而定位居于次要地位的做法。
• 利用 RGB-D 图像进行实时重建形成了一个重要的发展方向，陆续出现了 Kinect Fusion，Dynamic Fusion ，Elastic Fusion，Fusion4D， Volumn Deform等等工作。