11 建图

11.1 概述

（1）地图的几类用处：

• 定位：
• 导航：机器人在地图中进行路径规划；
• 避障
• 重建
• 交互：人与地图之间的互动

（2）几类地图

• 稀疏地图
• 稠密地图
• 语义地图

11.2 单目稠密重建

11.2.1 立体视觉

（1）稠密重建中，我们需要知道每个像素（或大部分像素）的距离，对此有以下几种方案：

• 使用单目相机，估计相机运动，并且三角化计算像素的距离；
• 使用双目相机，利用左右目的视差计算像素的距离；
• 使用 RGB-D 相机直接获取像素距离。

前两种方式称为立体视觉，在 RGB-D 相机无法很好应用的室外、大型场景中，仍有较好的表现。

11.2.2 极线搜索与块匹配

对于一个单目相机，假设我们观察测到了某个像素 $p_1$，显然，还是无法确定它的深度信息，但这个像素对应的空间点应该分布在某条射线上。从另一个视角，这条线的投影也形成了图像平面上的一条线，称为 极线。当知道两个相机之间的运动时，这条极线也是可以确定的。但问题是，极线上的哪个点才是 $p_1$ 对应的点呢？

在 $p_1$ 周围取 $w\times w$ 大小的像素块，在极线上也取相同大小的块，依次进行比较，直至找到 $p_2$，这就是所谓的 块匹配。当然这种方法的前提是 图像块灰度不变性，相较于像素灰度不变性，假设更强了。

把 $p_1$ 周围的像素块记为 $\mathbf{A}\in {\mathbb{R}}^{w\times w}$，把极线上的 $n$ 个小块记为 ${\mathbf{B}}_i，i=1,2,...,n$，计算二者之间的差异：

• SAD（Sum of Absolute Difference），两个小块的差的绝对值之和：

$$S(\mathbf{A},\mathbf{B}{)}_{\mathrm{SAD}}=\sum _{i,j}|\mathbf{A}(i,j)-\mathbf{B}(i,j)|\text{\hspace{1em}(11-1)}$$

• SSD（Sum of Squared Distance），即平方和：

$$S(\mathbf{A},\mathbf{B}{)}_{\mathrm{SSD}}=\sum _{i,j}(\mathbf{A}(i,j)-\mathbf{B}(i,j){)}^2\text{\hspace{1em}(11-2)}$$

• NCC（Normalized Cross Correlation），归一化互相关，计算两个小块的相关性：

$$S(\mathbf{A},\mathbf{B}{)}_{\mathrm{NCC}}=\frac{{\sum }_{i,j}\mathbf{A}(i,j)\mathbf{B}(i,j)}{\sqrt{{\sum }_{i,j}\mathbf{A}(i,j{)}^2{\sum }_{i,j}\mathbf{B}(i,j{)}^2}}\text{\hspace{1em}(11-3)}$$

相关性越接近于 0 表示越不相似，接近 1 表示相似。

现在，我们在极线上计算了 $\mathbf{A}$ 与每一个 ${\mathbf{B}}_i$ 的相似性度量。这里假设使用 NCC 进行度量，那么，将得到一个沿极线的 NCC分布。我们将使用概率分布描述深度值，而非某个单一数值。于是，问题转化为在不断对不同图像进行极线搜索时，我们估计的深度分布将发生怎样的变化———这就是所谓的 深度滤波器。

11.2.3 高斯分布的深度滤波器

像素点深度的估计，也是一个状态估计问题，于是有滤波器和非线性优化两种方式。但由于 SLAM 实时性要求和算力的限制，一般在建图时采用计算量较小的滤波器进行优化。

假设深度 $d$ 符合 高斯分布（也可能是其他分布）：

$$P(d)=N(\mu ,{\sigma }^2)\text{\hspace{1em}(11-4)}$$

每当新的数据到来，我们都会观测它的深度，假设它也符合高斯分布：

$$P(d_{\mathrm{obs}})=N({\mu }_{\mathrm{obs}},{\sigma }_{\mathrm{obs}}^2)\text{\hspace{1em}(11-5)}$$

根据观测到的数据更新原先 $d$ 的分布，也就是两个分布相乘，得到融合后的分布 $N({\mu }_{\mathrm{fuse}},{\sigma }_{\mathrm{fuse}}^2)$，即

$${\mu }_{\text{fuse }}=\frac{{\sigma }_{\mathrm{obs}}^2\mu +{\sigma }^2{\mu }_{\mathrm{obs}}}{{\sigma }^2+{\sigma }_{\mathrm{obs}}^2},{\sigma }_{\text{fuse }}^2=\frac{{\sigma }^2{\sigma }_{\mathrm{obs}}^2}{{\sigma }^2+{\sigma }_{\mathrm{obs}}^2}\text{\hspace{1em}(11-6)}$$

现在关键在于计算 ${\mu }_{\mathrm{obs}}$ 和 ${\sigma }_{\mathrm{obs}}^2$。假设我们找到了 ${\mathbf{p}}_1$ 对应的点 ${\mathbf{p}}_2$，从而观测到了 ${\mathbf{p}}_1$ 的深度，认为 ${\mathbf{p}}_1$ 对应的三维点为 $\mathbf{P}$。记 ${\mathbf{O}}_1\mathbf{P}$ 为 $\mathbf{p}$，${\mathbf{O}}_1{\mathbf{O}}_2$ 为相机平移 $\mathbf{t}$，${\mathbf{O}}_2\mathbf{P}$ 为 $\mathbf{a}$，两个夹角分别为 $\alpha$、$\beta$。现在考虑极线$l_2$ 上存在一个像素大小的误差，使点 ${\mathbf{p}}_2$ 变为 ${\mathbf{p}}_2^{\prime }$，使 $\beta$ 角变为 ${\beta }^{\prime }$。我们要考虑的是，这个像素误差会导致距离 $\mathbf{p}$ 和 ${\mathbf{p}}^{\prime }$ 产生多大差距。

根据几何关系，

$$\mathbf{a}=\mathbf{p}-\mathbf{t}$$

$$\alpha =\arccos ⟨\mathbf{p},\mathbf{t}⟩$$

$$\beta =\arccos ⟨\mathbf{a},-\mathbf{t}⟩\text{\hspace{1em}(11-7)}$$

对 ${\mathbf{p}}_2$ 扰动一个像素，使 $\beta$ 变为 ${\beta }^{\prime }$，根据几何关系

$${\beta }^{\prime }=\arccos ⟨{\mathbf{O}}_2{\mathbf{p}}_2^{\prime },-\mathbf{t}⟩$$

$$\gamma =\pi -\alpha -{\beta }^{\prime }\text{\hspace{1em}(11-8)}$$

由正弦定理

$$\Vert {\mathbf{p}}^{\prime }\Vert =\Vert \mathbf{t}\Vert \frac{\sin {\beta }^{\prime }}{\sin \gamma }\text{\hspace{1em}(11-9)}$$

这样，我们就确定了由单个像素的不确定引起的深度不确定性，如果认为极线搜索的块匹配仅有一个像素的误差，那么可以设

$${\sigma }_{\mathrm{obs}}=\Vert \mathbf{p}\Vert -\Vert {\mathbf{p}}^{\prime }\Vert \text{\hspace{1em}(11-10)}$$

当极线搜索的不确定性大于一个像素时，可按此推导放大这个不确定性。当不确定性小于某个阈值时，就可认为深度数据收敛。

因此，估计稠密深度的完整过程为：

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

需要注意的是，这里的深度是指 $O_{1}P$ 的长度，而针孔相机的深度是指像素的 $z$ 值。

11.3 实践：单目稠密重建

11.4 RGB-D 稠密建图

（1）点云：直接由 RGB-D 图像生成，不需要额外处理。

（2）网格/面片

（3）八叉树地图

（4）TSDF

11.4.1 八叉树地图

点云有几个明显的缺点：一是规模太大，有很多无效信息，占据大量空间；二是无法处理运动的物体。因此提出了一种灵活的、可压缩的、能随时更新的地图形式：八叉树地图（Octo-tree）。

左图显示了一个大立方体不断被均分成八块，直到变为最小的块为止。于是，可以将整个大方块看做根节点，最小的块看做叶子结点，当由下一层节点往上走一层时，地图体积就扩大八倍。当某个方块的所有子节点都被占据或都不被占据时，就没必要展开这个节点，例如地图开始为空白，就只需要一个根节点，而不需要完整的树，所以说八叉树比点云更节省空间。

用概率的形式来表达节点是否被占据，比如初始值为 0.5，如果不断观测到它被占据，则值不断增大；如果不断观测到它是空白，则不断减小。当然，如果这个值不断增大或减小，就可能超出 $[0,1]$ 之外，因此，我们采用 概率对数值 来描述。设 $y\in \mathbb{R}$ 为概率对数值，$x$ 为 0~1 的概率，定义

$$y=\mathrm{logit}(x)=\log (\frac{x}{1-x})\text{\hspace{1em}(11-11)}$$

其反变换为：

$$x={\mathrm{logit}}^{-1}(y)=\frac{\exp (y)}{\exp (y)+1}\text{\hspace{1em}(11-12)}$$

可以看出，当 $y$ 从 $-\infty$ 到 $+\infty$ 时，$x$ 范围为 0~1，当 $y$ 取 0 时，$x$ 取 0.5。因此，不妨用 $y$ 来表达节点是否被占据，再将其转换为概率 $x$ 即可。假设某节点 $n$，观测数据为 $z$，那么从开始到 $t$ 时刻该节点的概率对数值为 $L(n|z_{1:t})$，$t+1$ 时刻为

$$L(n|z_{1:t+1})=L(n|z_{1:t-1})+L(n|z_t)\text{\hspace{1em}(11-13)}$$

将其写成概率形式（也就是 $x$），

$$P\left(n\mid z_{1:T}\right)={\left[1+\frac{1-P\left(n\mid z_T\right)}{P\left(n\mid z_T\right)}\frac{1-P\left(n\mid z_{1:T-1}\right)}{P\left(n\mid z_{1:T-1}\right)}\frac{P(n)}{1-P(n)}\right]}^{-1}\text{\hspace{1em}(11-14)}$$

有了对数概率，就可根据 RGB-D 数据更新八叉树地图。假设在 RGB-D 图像中某个像素带有深度 $d$，就说明：在深度值对应的空间点上观察到了一个占据数据，并且，从相机光心出发到这个点的线段上应该是没有物体的（否则被遮挡）。

11.4.2 TSDF 地图和 Fusion 系列

实时三维重建