# Study notes for Fourteen Lectures of Visual SLAM From Theory to Practice (Practical application P3)


<!--more-->

PDF [视觉SLAM十四讲从理论到实践 第2版](https://shangzg.top/files/2022-10-19-视觉SLAM十四讲从理论到实践第2版学习笔记(数学基础)/视觉SLAM十四讲从理论到实践第2版.pdf)

前6讲（数学基础）知识笔记见 [Study notes for Fourteen Lectures of Visual SLAM From Theory to Practice (Mathematical basis)](https://shangzg.top/2022-10-19-study-notes-for-fourteen-lectures-of-visual-slam-from-theory-to-practice-mathematical-basis/)

7-8讲（视觉里程计）知识笔记见 [Study notes for Fourteen Lectures of Visual SLAM From Theory to Practice (Practical application P1)](https://shangzg.top/zh-cn/2022-10-19-study-notes-for-fourteen-lectures-of-visual-slam-from-theory-to-practice-practical-application-p1/)

9-10讲（后端）知识笔记见 [Study notes for Fourteen Lectures of Visual SLAM From Theory to Practice (Practical application P2)](https://shangzg.top/zh-cn/2022-11-11-study-notes-for-fourteen-lectures-of-visual-slam-from-theory-to-practice-practical-application-p2/)



# 第二部分 实践应用

## 第11讲 回环检测

### 概述

#### 回环检测的意义

前端提供特征点的提取和轨迹、地图的初值，而后端负责对所有这些数据进行优化。

然而，如果像视觉里程计那样仅考虑相邻时间上的关键帧，那么，之前产生的误差将不可避免地累积到下一个时刻，使得整个SLAM出现**累积误差**，长期估计的结果将不可靠，或者说，无法构建**全局一致**的轨迹和地图。

举个简单的例子：在自动驾驶的建图阶段，通常会指定采集车在某个给定区域绕若干圈以覆盖所有采集范围。

假设在前端提取了特征，然后忽略特征点，在后端使用位姿图优化整个轨迹，如下图（a）所示。前端给出的只是局部的位姿间约束，例如，可能是 $\boldsymbol x_1-\boldsymbol x_2,\boldsymbol x_2-\boldsymbol x_3$ 等等。

但是，由于 $\boldsymbol x_1$ 的估计存在误差，而 $\boldsymbol x_2$ 是根据 $\boldsymbol x_1$ 决定的，$\boldsymbol x_3$ 又是由 $\boldsymbol x_2$ 决定的。依此类推，误差就会被累积起来，使得后端优化的结果如下图（b）所示，慢慢地趋向不准确。

在这种应用场景下，应该保证，优化的轨迹和实际地点一致。当实际经过同一个地点时，估计轨迹也必定经过同一点。

![image-20221129175647736](/images/2022-11-29-视觉SLAM十四讲从理论到实践第2版学习笔记(实践应用P3)/1)

虽然后端能够估计最大后验误差，但所谓“好模型架不住烂数据”，只有相邻关键帧数据时，能做的事情并不多，也无从消除累积误差。

但是，回环检测模块能够给出除了相邻帧的一些**时隔更加久远**的约束：例如 $\boldsymbol x_1 \sim \boldsymbol x_{100}$ 之间的位姿变换。

产生约束是因为察觉到相机**经过了同一个地方，采集到了相似的数据**。

而回环检测的关键，就是如何有效地检测出**相机经过同一个地方**。

如果能够成功地检测到这件事，就可以为后端的位姿图提供更多的有效数据，使之得到更好的估计，特别是得到一个**全局一致**的估计。

回环检测对于SLAM系统意义重大。

一方面，它关系到估计的轨迹和地图在**长时间**下的正确性。

另一方面，由于回环检测提供了当前数据与所有历史数据的关联，还可以利用回环检测进行**重定位**。

重定位的用处就更多一些。例如，如果事先对某个场景录制了一条轨迹并建立了地图，那么之后在该场景中就可以一直跟随这条轨迹进行导航，而重定位可以帮助确定自身在这条轨迹上的位置。

因此，回环检测对整个SLAM系统精度与稳健性的提升是非常明显的。甚至在某些时候，把仅有前端和局部后端的系统称为视觉里程计，而把带有回环检测和全局后端的系统称为SLAM。

#### 回环检测的方法

最简单的方式就是对任意两幅图像都做一遍特征匹配，根据正确匹配的数量确定哪两幅图像存在关联——这确实是一种朴素且有效的思想。

缺点在于盲目地假设了“任意两幅图像都可能存在回环”，使得要检测的数量实在太大：对于 $N$ 个可能的回环，要检测 $C_{N}^{2}$ 那么多次，这是 $O(N^2)$ 的复杂度，随着轨迹变长增长太快，在大多数实时系统中是不实用的。

另一种朴素的方式是，随机抽取历史数据并进行回环检测，例如在 $n$ 帧中随机抽 $5$ 帧与当前帧比较。这种做法能够维持常数时间的运算量，但是这种盲目试探方法在帧数 $N$ 增长时，抽到回环的概率又大幅下降，使得检测效率不高。

上面说的朴素思路都过于粗糙。尽管随机检测在有些实现中确实有用，但我们至少希望有一个“**哪处可能出现回环**”的预计，才好不那么盲目地去检测。

这样的方式大体有两种思路：**基于里程计**（Odometry based）的几何关系，或**基于外观**（Appearance based）的几何关系。

基于里程计的几何关系是说，当发现当前相机运动到了之前的某个位置附近时，检测它们有没有回环关系——这自然是一种直观的想法，但是由于累积误差的存在，往往没法正确地发现“运动到了之前的某个位置附近”这件事实，回环检测也无从谈起。

因此，这种做法在逻辑上存在一点问题，因为回环检测的目标在于发现“相机回到之前位置”的事实，从而消除累积误差。而基于里程计的几何关系的做法假设了“相机回到之前位置附近”，这样才能检测回环。这是有倒果为因的嫌疑的，因而也无法在累积误差较大时工作。

另一种方法是基于外观的。它和前端、后端的估计都无关，仅根据两幅图像的相似性确定回环检测关系。

这种做法摆脱了累积误差，使回环检测模块成为SLAM系统中一个相对独立的模块（当然前端可以为它提供特征点）。

自21世纪初被提出以来，基于外观的回环检测方式能够有效地在不同场景下工作，成了视觉SLAM中主流的做法，并被应用于实际的系统中。

除此之外，从工程角度也能提出一些解决回环检测的办法。

例如，室外的无人车通常会配备 GPS，可以提供全局的位置信息。利用 GPS 信息可以很轻松地判断汽车是否回到某个经过的点，但这类方法在室内就不怎么好用。

在基于外观的回环检测算法中，核心问题是如何计算图像间的相似性。

例如，对于图像 $\boldsymbol A$ 和图像 $\boldsymbol B$，要设计一种方法，计算它们之间的相似性评分：$s(\boldsymbol A,\boldsymbol B)$。

这个评分会在某个区间内取值，当它大于一定量后认为出现了一个回环。

直观上看，图像能够表示成矩阵，那么直接让两幅图像相减，然后取某种范数行不行呢?

{{< raw >}}
$$
s(\boldsymbol{A}, \boldsymbol{B})=\|\boldsymbol{A}-\boldsymbol{B}\| .
$$
{{< /raw >}}

为什么不这样做？

1. 像素灰度是一种不稳定的测量值，它严重地受环境光照和相机曝光的影响。假设相机未动，打开了一支电灯，那么图像会整体变亮。这样，即使对于同样的数据，也会得到一个很大的差异值。
2. 当相机视角发生少量变化时，即使每个物体的光度不变，它们的像素也会在图像中发生位移，造成一个很大的差异值。

由于这两种情况的存在实际中，即使对于非常相似的图像，$\boldsymbol{A}-\boldsymbol{B}$ 也会经常得到一个（不符合实际的）很大的值。

所以这个函数不能很好地反映图像间的相似关系。

#### 准确率和召回率

回环检测的结果分类：

| 算法/事实 | 是回环 | 不是回环 |
| :-------: | :----: | :------: |
|  是回环   | 真阳性 |  假阳性  |
| 不是回环  | 假阴性 |  真阴性  |

假阳性（False Positive）又称为感知偏差，而假阴性（False Negative）称为感知变异。

为方便书写，用缩写 **TP** 代表True Positive（真阳性），用 **TN** 代表True Negative（真阴性），其余类推。

由于希望算法和人类的判断、事实一致，所以希望 **TP** 和 **TN** 尽量高，而 **FP** 和 **FN** 尽可能低。

所以，对于某种特定算法，可以统计它在某个数据集上的 **TP**、**TN**、**FP**、**FN** 的出现次数，并计算两个统计量：**准确率和召回率（Precision & Recall）**

{{< raw >}}
$$
\text { Precision }=\mathrm{TP} /(\mathrm{TP}+\mathrm{FP}), \quad \text { Recall }=\mathrm{TP} /(\mathrm{TP}+\mathrm{FN})
$$
{{< /raw >}}

从公式字面意义上看，准确率描述的是算法提取的所有回环中确实是真实回环的概率。

而召回率则是指，在所有真实回环中被正确检测出来的概率。

一个算法往往有许多的设置参数。例如，当提高某个阈值时,算法可能变得更加“严格”——它检出更少的回环，使准确率得以提高。

同时，由于检出的数量变少了，许多原本是回环的地方就可能被漏掉，导致召回率下降。

反之，如果我们选择更加宽松的配置，那么检出的回环数量将增加，得到更高的召回率，但其中可能混杂一些不是回环的情况，于是准确率下降。

为了评价算法的好坏，会测试它在各种配置下的 $P$ 和 $R$ 值，然后做 Precision-Recall 曲线。

![image-20221129195159574](/images/2022-11-29-视觉SLAM十四讲从理论到实践第2版学习笔记(实践应用P3)/2)

当用召回率为横轴，用准确率为纵轴时，会关心整条曲线偏向右上方的程度、100%准确率下的召回率或者50%召回率时的准确率，作为评价算法的指标。

在SLAM中，对准确率的要求更高，而对召回率则相对宽容一些。

由于假阳性的（检测结果是而实际不是的）回环将在后端的位姿图中添加根本错误的边，有些时候会导致优化算法给出完全错误的结果。所以在选择回环检测算法时，更倾向于把参数设置得更严格，或者在检测之后再加上**回环验证**的步骤。

### 词袋模型

直接用两张图像相减的方式不够好，需要一种更可靠的方式。结合前面几讲的内容，一种思路是：像视觉里程计那样使用特征点来做回环检测。

和视觉里程计一样，对两幅图像的特征点进行匹配，只要匹配数量大于一定值，就认为出现了回环。

根据特征点匹配，还能计算出这两幅图像之间的运动关系。

当然这种做法存在一些问题，例如，特征的匹配会比较费时、当光照变化时特征描述可能不稳定等。

词袋，也就是 Bag-of-Words（BoW），目的是用“图像上有哪几种特征”来描述一幅图像。

例如，说某张照片中有一个人、一辆车；而另一张中有两个人、一只狗。

根据这样的描述，就可以度量这两幅图像的相似性。再具体一些，要做以下三步：

1. 确定“人”、“车”、“狗”等概念——对应于BoW中的“**单词**”（Word），许多单词放在一起，组成了“**字典**”（Dictionary）。
2. 确定一幅图像中出现了哪些在字典中定义的概念——用单词出现的情况（或直方图）描述整幅图像。这就把一幅图像转换成了一个向量的描述。
3. 比较上一步中的描述的相似程度。

以上面举的例子来说，首先通过某种方式得到了一本“字典”。

字典上记录了许多单词，每个单词都有一定意义，例如“人”、“车”、“狗”都是记录在字典中的单词，不妨记为 $w_1, w_2, w_3$。

然后，对于任意图像 $\boldsymbol A$，根据含有的单词，可记为：

{{< raw >}}
$$
\boldsymbol A=1 \cdot w_{1}+1 \cdot w_{2}+0 \cdot w_{3}
$$
{{< /raw >}}

字典是固定的，所以只要用 $[1,1,0]^T$ 这个向量就可以表达 $\boldsymbol A$ 的意义。

通过字典和单词，只需一个向量就可以描述整幅图像。

该向量描述的是“图像是否含有某类特征”的信息，比单纯的灰度值更稳定。

又因为描述向量说的是“是否出现”，而不管它们“在哪儿出现”，所以与物体的空间位置和排列顺序无关，因此在相机发生少量运动时，只要物体仍在视野中出现，就仍然保证描述向量不发生变化。

基于这种特性，称它为 Bag-of-Words 而不是 List-of-Words，强调的是 Words 的有无，而无关其顺序。

因此，可以说字典类似于单词的一个集合。

回到上面的例子，同理，用 $[2,0,1]^T$ 可以描述图像B。如果只考虑“是否出现”而不考虑数量，也可以是 $[1,0,1]^T$，这时候这个向量就是二值的。

于是，根据这两个向量，设计一定的计算方式，就能确定图像间的相似性。

当然，对两个向量求差仍然有一些不同的做法，例如对于 $\boldsymbol{a}, \boldsymbol{b} \in \mathbb{R}^{W}$，可以计算：

{{< raw >}}
$$
s(\boldsymbol{a}, \boldsymbol{b})=1-\frac{1}{W}\|\boldsymbol{a}-\boldsymbol{b}\|_{1}
$$
{{< /raw >}}

其中范数取 $L_1$ 范数，即各元素绝对值之和。

在两个向量完全一样时，将得到 $1$；完全相反时（$\boldsymbol a$ 为 $0$ 的地方 $\boldsymbol b$ 为 $1$）得到 $0$。

这样就定义了两个描述向量的相似性，也就定义了图像之间的相似程度。
### 字典

#### 字典的结构

字典由很多单词组成，而每一个单词代表了一个概念。

一个单词与一个单独的特征点不同，它不是从单幅图像上提取出来的，而是某一类特征的组合。

所以，字典生成问题类似于一个**聚类（Clustering）** 问题。

聚类问题在无监督机器学习（Unsupervised ML）中特别常见，用于让机器自行寻找数据中的规律。

BoW的字典生成问题也属于其中之一。

首先，假设对大量的图像提取了特征点，例如有 $N$ 个。

现在，想找一个有 $k$ 个单词的字典，每个单词可以看作局部相邻特征点的集合，可以用经典的 K-means（K均值）算法解决。

K-means 是一个非常简单有效的方法，因此在无监督学习中广为使用。

简单的说，当有 $N$ 个数据，想要归成 $k$ 个类，那么用 K-means 来做主要包括如下步骤：

1. 随机选取 $k$ 个中心点：$c_1,\cdots, c_k$
2. 对每一个样本，计算它与每个中心点之间的距离，取最小的作为它的归类。
3. 重新计算每个类的中心点。
4. 如果每个中心点都变化很小，则算法收敛，退出；否则返回第2步。

K-means 的做法是朴素且简单有效的，不过也存在一些问题，例如，需要指定聚类数量、随机选取中心点使得每次聚类结果都不相同、以及一些效率上的问题。

随后，研究者们也开发出了层次聚类法、K-means++ 等算法以弥补它的不足。

总之，根据 K-means，可以把已经提取的大量特征点聚类成一个含有 $k$ 个单词的字典。

现在的问题变成了如何根据图像中某个特征点查找字典中相应的单词。

仍然有朴素的思想：只要和每个单词进行比对，取最相似的那个就可以了——这当然是简单有效的做法。

然而，考虑到字典的通用性，通常会使用一个较大规模的字典，以保证当前使用环境中的图像特征都曾在字典里出现，或至少有相近的表达。

这种 $O(n)$ 的查找算法显然不行的。

如果字典排过序，那么二分查找显然可以提升查找效率，达到对数级别的复杂度。

而实践中，可能会用更复杂的数据结构，例如 Fabmap 中的 Chou-Liu tree 等。

可以用一种较为简单实用的树结构，使用一种 $k$ 叉树来表达字典。

思路很简单，类似于层次聚类，是 K-means 的直接扩展。

假定有 $N$ 个特征点，希望构建一个深度为 $d$、每次分叉为 $k$ 的树，做法如下：

1. 在根节点，用 K-means 把所有样本聚成 $k$ 类（实际中为保证聚类均匀性会使用
   K-means++）。这样就得到了第一层。
2. 对第一层的每个节点，把属于该节点的样本再聚成 $k$ 类，得到下一层。
3. 依此类推，最后得到叶子层。叶子层即为所谓的 Words。

![image-20221130105803807](/images/2022-11-29-视觉SLAM十四讲从理论到实践第2版学习笔记(实践应用P3)/3)

实际上，最终仍在叶子层构建了单词，而树结构中的中间节点仅供快速查找时使用。

这样一个 $k$ 分支、深度为 $d$ 的树，可以容纳 $k^d$ 个单词。

另外，在查找某个给定特征对应的单词时，只需将它与每个中间节点的聚类中心比较（一共 $d$ 次），即可找到最后的单词，保证了对数级别的查找效率。

### 相似度计算

有了字典之后，给定任意特征 $f_i$，只要在字典树中逐层查找，最后都能找到与之对应的单词 $w_j$——当字典足够大时，可以认为 $f_i$ 和 $w_j$ 来自同一类物体（尽管没有理论上的保证，仅是在聚类意义下这样说）。

那么，假设从一幅图像中提取了 $N$ 个特征，找到这 $N$ 个特征对应的单词之后，就相当于拥有了该图像在单词列表中的分布，或者直方图。

根据 BoW 的说法，不妨认为这是一个 Bag。

注意，这种做法中对所有单词都是“一视同仁”的——有就是有，没有就是没有。

考虑部分单词具有更强区分性这一因素。

例如，“的”“是”这样的字可能在许许多多的句子中出现，无法根据它们判别句子的类型；但如果有“文档”“足球”这样的单词，对判别句子的作用就大一些，可以说它们提供了更多信息。

所以概括起来，希望对单词的区分性或重要性加以评估，给它们不同的权值以起到更好的效果。

TF-IDF（Term Frequency-Inverse Document Frequency），或译频率–逆文档频率，是文本检索中常用的一种加权方式，也用于 BoW 模型中。

TF 部分的思想是，某单词在一幅图像中经常出现，它的区分度就高。另外，IDF的思想是，某单词在字典中出现的频率越低，分类图像时区分度越高。

可以在建立字典时计算 IDF：统计某个叶子节点 $w_i$ 中的特征数量相对于所有特征数量的比例，作为 IDF 部分。

假设所有特征数量为 $n$，$w_i$ 数量为 $n_i$，那么该单词的 IDF 为：

{{< raw >}}
$$
\mathrm{IDF}_{i}=\log \frac{n}{n_{i}}
$$
{{< /raw >}}

TF 部分则是指某个特征在单幅图像中出现的频率。

假设图像 $\boldsymbol A$ 中单词 $w_i$ 出现了 $n_i$ 次，而一共出现的单词次数为 $n$，那么 TF 为：

{{< raw >}}
$$
\mathrm{TF}_{i}=\frac{n_i}{n}
$$
{{< /raw >}}

于是，$w_i$ 的权重等于 TF 乘 IDF 之积：

 {{< raw >}}
$$
\eta_{i}=\mathrm{TF}_{i}\times\mathrm{IDF}_{i}
$$
{{< /raw >}}

考虑权重以后，对于某幅图像 $\boldsymbol A$，它的特征点可对应到许多个单词，组成它的 BoW：

 {{< raw >}}
$$
\boldsymbol A=\left\{\left(w_{1}, \eta_{1}\right),\left(w_{2}, \eta_{2}\right), \ldots,\left(w_{N}, \eta_{N}\right)\right\} \stackrel{\text { def }}{=} \boldsymbol{v}_{A}
$$
{{< /raw >}}

由于相似的特征可能落到同一个类中，因此实际的 $\boldsymbol{v}_{A}$ 中会存在大量的零。

无论如何，通过词袋用单个向量 $\boldsymbol{v}_{A}$ 描述了一幅图像 $\boldsymbol A$。

这个向量 $\boldsymbol{v}_{A}$ 是一个稀疏的向量，它的非零部分指示了图像 $\boldsymbol A$ 中含有哪些单词，而这些部分的值为 TF-IDF 的值。

给定  {{< raw >}}$\boldsymbol{v}_{A}$ 和 $\boldsymbol{v}_{B}$，计算它们的差异，和范数定义的方式一样，存在若干种解决方式，例如 $L_1$ {{< /raw >}} 范数形式：

 {{< raw >}}
$$
s\left(\boldsymbol{v}_{A}-\boldsymbol{v}_{B}\right)=2 \sum_{i=1}^{N}\left|\boldsymbol{v}_{A i}\right|+\left|\boldsymbol{v}_{B i}\right|-\left|\boldsymbol{v}_{A i}-\boldsymbol{v}_{B i}\right|
$$
{{< /raw >}}

#### 相似性评分的处理

对任意两幅图像，都能给出一个相似性评分，但是只利用这个分值的绝对大小并不一定有很好的帮助。

例如，有些环境的外观本来就很相似，像办公室往往有很多同款式的桌椅一样；另一些环境则各个地方都有很大的不同。

考虑到这种情况，一般会取一个**先验相似度** $s(v_t, v_{t-\Delta t})$，它表示某时刻关键帧图像与上一时刻的关键帧的相似性。

然后，其他的分值都参照这个值进行归一化：

 {{< raw >}}
$$
s\left(\boldsymbol{v}_{t}, \boldsymbol{v}_{t_{j}}\right)^{\prime}=s\left(\boldsymbol{v}_{t}, \boldsymbol{v}_{t_{j}}\right) / s\left(\boldsymbol{v}_{t}, \boldsymbol{v}_{t-\Delta t}\right)
$$
{{< /raw >}}

站在这个角度上，如果当前帧与之前某关键帧的相似度超过当前帧与上一个关键帧相似度的 $3$ 倍，就认为可能存在回环。

这个步骤避免了引入绝对的相似性阈值，使得算法能够适应更多的环境。

#### 关键帧的处理

在检测回环时，必须考虑到关键帧的选取。如果关键帧选得太近，那么将导致两个关键帧之间的相似性过高，相比之下不容易检测出历史数据中的回环。

例如，检测结果经常是第 $n$ 帧和第 $n -2$ 帧、第 $n-3$ 帧最为相似，这种结果似乎太平凡了，意义不大。

所以从实践上说，用于回环检测的帧最好稀疏一些，彼此之间不太相同，又能涵盖整个环境。

另外，如果成功检测到了回环，例如，回环出现在第 $1$ 帧和第 $n$ 帧。那么很可能第 $n+1$ 帧、第 $n+2$ 帧都会和第 $1$ 帧构成回环。

确认第 $1$ 帧和第 $n$ 帧之间存在回环对轨迹优化是有帮助的，而接下去的第 $n+1$ 帧、第 $n+2$ 帧都会和第 $1$ 帧构成回环产生的帮助就没那么大了，因为已经用之前的信息消除了累积误差，更多的回环并不会带来更多的信息。

所以，会把“相近”的回环聚成一类，使算法不要反复地检测同一类的回环。

#### 检测之后的验证

词袋的回环检测算法完全依赖于外观而没有利用任何的几何信息，这导致外观相似的图像容易被当成回环。

并且，由于词袋不在乎单词顺序，只在意单词有无的表达方式，更容易引发感知偏差。

所以，在回环检测之后，通常还会有一个验证步骤。

一个方法是设立回环的缓存机制，认为单次检测到的回环并不足以构成良好的约束，而在一段时间中一直检测到的回环，才是正确的回环。

这可以看成时间上的一致性检测。

另一个方法是空间上的一致性检测，即对回环检测到的两个帧进行特征匹配，估计相机的运动。

然后，把运动放到之前的位姿图中，检查与之前的估计是否有很大的出入。

总之，验证部分通常是必需的，但如何实现却是见仁见智的问题。



## 第12讲 建图

### 概述

在经典的SLAM模型中，所谓的地图，即所有路标点的集合。一旦确定了路标点的位置，就可以说完成了建图。

于是，前面说的视觉里程计也好，BA也好，事实上都建模了路标点的位置，并对它们进行了优化。从这个角度上说，已经探讨了建图问题。

但人们对建图的需求不同。

SLAM作为一种底层技术，往往是用来为上层应用提供信息的。

如果上层是机器人，那么应用层的开发者可能希望使用SLAM做全局的定位，并且让机器人在地图中导航——例如扫地机需要完成扫地工作，希望计算一条能够覆盖整张地图的路径。

或者，如果上层是一个增强现实设备，那么开发者可能希望将虚拟物体叠加在现实物体之中，特别地、还可能需要处理虚拟物体和真实物体的遮挡关系。

应用层面对于“定位”的需求是相似的，希望SLAM提供相机或搭载相机的主体的空间位姿信息。

而对于地图，则存在着许多不同的需求。

从视觉SLAM的角度看，“建图”是服务于“定位”的；但是从应用层面看，“建图”明显带有许多其他的需求。

关于地图的用处，大致归纳如下:

1. **定位**。定位是地图的一项基本功能。在前面的视觉里程计部分，讨论了如何利用局部地图实现定位。在回环检测部分，只要有全局的描述子信息，也能通过回环检测确定机器人的位置。我们还希望能够把地图保存下来，让机器人在下次开机后依然能在地图中定位，这样只需对地图进行一次建模，而不是每次启动机器人都重新做一次完整的SLAM。
2. **导航**。导航是指机器人能够在地图中进行路径规划，在任意两个地图点间寻找路径，然后控制自己运动到目标点的过程。在该过程中，需要知道**地图中哪些地方不可通过，而哪些地方是可以通过的**。这就超出了稀疏特征点地图的能力范围，必须有另外的地图形式，这至少得是一种**稠密**的地图。
3. **避障**。避障也是机器人经常碰到的一个问题。它与导航类似，但更注重局部的、动态的障碍物的处理。同样，仅有特征点，无法判断某个特征点是否为障碍物，所以需要**稠密**地图。
4. **重建**。有时，希望利用SLAM获得周围环境的重建效果。这种地图主要用于向人展示，所以希望它看上去比较舒服、美观。或者，也可以把该地图用于通信，使其他人能够远程观看重建得到的三维物体或场景——例如三维的视频通话或者网上购物等。这种地图亦是**稠密**的，并且还对它的外观有一些要求。我们可能不满足于稠密点云重建，更希望能够构建带纹理的平面，就像电子游戏中的三维场景那样。
5. **交互**。交互主要指人与地图之间的互动。例如，在增强现实中，我们会在房间里放置虚拟的物体，并与这些虚拟物体之间有一些互动——例如我们会点击墙面上放着的虚拟网页浏览器来观看视频，或者向墙面投掷物体,希望它们有（虚拟的）物理碰撞。另外，机器人应用中也会有与人、与地图之间的交互。例如，机器人可能会收到命令“取桌子上的报纸”，那么，除了有环境地图，机器人还需要知道哪一块地图是“桌子”，什么叫作“之上”，什么叫作“报纸”。这就需要机器人对地图有更高层面的认知也称为**语义地图**。

![image-20221130120009909](/images/2022-11-29-视觉SLAM十四讲从理论到实践第2版学习笔记(实践应用P3)/4)

之前的讨论，基本集中于“稀疏路标地图”部分，还没有探讨稠密地图。

所谓稠密地图是相对于稀疏地图而言的。

稀疏地图只建模感兴趣的部分，也就是前面说了很久的特征点（路标点）。

而稠密地图是指建模所有看到过的部分。

对于同一张桌子，稀疏地图可能只建模了桌子的四个角，而稠密地图则会建模整个桌面。

虽然从定位角度看，只有四个角的地图也可以用于对相机进行定位，但由于无法从四个角推断这几个点之间的空间结构，所以无法仅用四个角完成导航、避障等需要稠密地图才能完成的工作。

### 稠密重建

相机，被认为是只有角度的传感器（Bearing only）。

单幅图像中的像素，只能提供物体与相机成像平面的角度及物体采集到的亮度，而无法提供物体的距离（Range）。

而在稠密重建中，需要知道每一个像素点（或大部分像素点）的距离，对此大致上有如下解决方案：

1. 使用单目相机，估计相机运动，并且三角化计算像素的距离。
2. 使用双目相机，利用左右目的视差计算像素的距离（多目原理相同）。
3. 使用RGB-D相机直接获得像素距离。

前两种方式称为立体视觉（Stereo Vision），其中移动单目相机的又称为移动视角的立体视觉（Moving View Stereo，MVS）。

相比于 RGB-D 直接测量的深度，使用单目和双目的方式对深度获取往往是“费力不讨好”的——计算量巨大，最后得到一些不怎么可靠的深度估计。

当然，RGB-D 也有一些量程、应用范围和光照的限制，不过相比于单目和双目的结果，使用RGB-D进行稠密重建往往是更常见的选择。

而单目、双目的好处是，在目前 RGB-D 还无法被很好地应用的室外、大场景场合中，仍能通过立体视觉估计深度信息。

### 单目稠密重建

假定有一段视频序列，得到了每一帧对应的轨迹。

现在以第一幅图像为参考帧，计算参考帧中每个像素的深度（或者距离)。

首先，请回忆在特征点部分是如何完成该过程的：

1. 对图像提取特征，并根据描述子计算特征之间的匹配。换言之，通过特征，对某一个空间点进行了跟踪，知道了它在各个图像之间的位置。
2. 由于无法仅用一幅图像确定特征点的位置，所以必须通过不同视角下的观测估计它的深
   度，原理即前面讲过的三角测量。

在稠密深度图估计中，不同之处在于，无法把每个像素都当作特征点计算描述子。

因此，稠密深度估计问题中，匹配就成为很重要的一环：确定第一幅图的某像素出现在其他图里的位置，这需要用到**极线搜索**和**块匹配技术**。

当知道了某个像素在各个图中的位置，就能像特征点那样，利用三角测量法确定它的深度。

不过不同的是，在这里要使用很多次三角测量法让深度估计收敛，而不仅使用一次。我们希望深度估计能够随着测量的增加从一个非常不确定的量，逐渐收敛到一个稳定值。这就是**深度滤波器技术**。

#### 极限搜索与块匹配

先来探讨不同视角下观察同一个点产生的几何关系。

这非常像在第7讲讨论的对极几何关系。

如下图所示，左边的相机观测到了某个像素 $p_1$。由于这是一个单目相机，无从知道它的深度，所以假设这个深度可能在某个区域之内，不妨说是某最小值到无穷远之间 $(d_{min},+\infty)$。

![image-20221130142935389](/images/2022-11-29-视觉SLAM十四讲从理论到实践第2版学习笔记(实践应用P3)/5)

因此，该像素对应的空间点就分布在某条线段（本例中是射线）上。

从另一个视角（右侧相机）看，这条线段的投影也形成图像平面上的一条线，这称为**极线**。

当知道两部相机间的运动时，这条极线也是能够确定的。反之，如果不知道两部相机间的运动，那极线也无法确定。

那么问题就是：极线上的哪一个点是刚才看到的 $p_1$ 点呢？

在特征点方法中，通过特征匹配找到了 $p_2$ 的位置。然而现在没有描述子，所以只能在极线上搜索和 $p_1$ 长得比较相似的点。

再具体地说，可能沿着第二幅图像中的极线的某一头走到另一头，逐个比较每个像素与 $p_1$ 的相似程度。从直接比较像素的角度来看，这种做法和直接法有异曲同工之妙。

在直接法的讨论中，比较单个像素的亮度值并不一定稳定可靠。

一件很明显的事情就是：万一极线上有很多和 $p_1$ 相似的点，怎么确定哪一个是真实的呢？

这似乎回到了在回环检测中说到的问题：如何确定两幅图像（或两个点）的相似性？

回环检测是通过词袋来解决的，但这里由于没有特征，所以只好寻求另外的解决途径。

一种直观的想法是：既然单个像素的亮度没有区分性，是否可以比较像素块呢？

在 $p_1$ 周围取一个大小为 $w × w$ 的小块，然后在极线上也取很多同样大小的小块进行比较，就可以在一定程度上提高区分性。这就是所谓的**块匹配**。

在这个过程中，只有假设在不同图像间整个小块的灰度值不变，这种比较才有意义。

所以算法的假设，从像素的灰度不变性变成了图像块的灰度不变性——在一定程度上变得更强了。

现在取了 $p_1$ 周围的小块，并且在极线上也取了很多个小块。

不妨把 $p_1$ 周围的小块记成 $\boldsymbol{A} \in \mathbb{R}^{w \times w}$，把极线上的 $n$ 个小块记成 $\boldsymbol B_i,i = 1,\cdots,n$。

计算小块与小块间的差异有若干种不同的计算方法：

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

    {{< raw >}}
   $$
   S(\boldsymbol{A}, \boldsymbol{B})_{\mathrm{SAD}}=\sum_{i, j}|\boldsymbol{A}(i, j)-\boldsymbol{B}(i, j)|
   $$
   {{< /raw >}}

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

    {{< raw >}}
   $$
   S(\boldsymbol{A}, \boldsymbol{B})_{\mathrm{SSD}}=\sum_{i, j}(\boldsymbol{A}(i, j)-\boldsymbol{B}(i, j))^{2}
   $$
   {{< /raw >}}

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

    {{< raw >}}
   $$
   S(\boldsymbol{A}, \boldsymbol{B})_{\mathrm{NCC}}=\frac{\sum_{i, j} \boldsymbol{A}(i, j) \boldsymbol{B}(i, j)}{\sqrt{\sum_{i, j} \boldsymbol{A}(i, j)^{2} \sum_{i, j} \boldsymbol{B}(i, j)^{2}}} .
   $$
   {{< /raw >}}

   请注意，由于这里用的是相关性，所以相关性接近 $0$ 表示两幅图像不相似，接近 $1$ 表示相似。前面两种距离则是反过来的，接近 $0$ 表示相似，而大的数值表示不相似。

这些计算方式往往存在一个精度-效率之间的矛盾。精度好的方法往往需要复杂的计算，而简单的快速算法又往往效果不佳。这需要在实际工程中进行取舍。

另外，除了这些简单版本，可以**先把每个小块的均值去掉**，称为去均值的 SSD、去均值的 NCC，等等。

去掉均值之后，允许像“小块 $\boldsymbol B$ 比 $\boldsymbol A$ 整体上亮一些，但仍然很相似”这样的情况，因此比之前的更可靠。

在极线上计算 $\boldsymbol A$ 与每一个 $\boldsymbol B$ 的相似性度量。为了方便叙述，假设用了 NCC，那么，将得到一个沿着极线的 NCC分布。

这个分布的形状取决于图像数据。

![image-20221130144714750](/images/2022-11-29-视觉SLAM十四讲从理论到实践第2版学习笔记(实践应用P3)/6)

在搜索距离较长的情况下，通常会得到一个非凸函数：这个分布存在许多峰值，然而真实的对应点必定只有一个。

在这种情况下，会倾向于使用概率分布描述深度值，而非用某个单一的数值来描述深度。

于是，问题就转到了在不断对不同图像进行极线搜索时，估计的深度分布将发生怎样的变化——这就是所谓的**深度滤波器**。

#### 高斯分布的深度滤波器

对像素点深度的估计，本身也可建模为一个状态估计问题，于是就自然存在滤波器与非线性优化两种求解思路。

虽然非线性优化效果较好，但是在SLAM这种实时性要求较强的场合，考虑到前端已经占据了不少的计算量，建图方面则通常采用计算量较少的滤波器方式。

对深度的分布假设存在若干种不同的做法。

一方面，在比较简单的假设条件下，可以假设深度值服从高斯分布，得到一种类卡尔曼式的方法（但实际上只是归一化积）。

另一方面，可采用均匀-高斯混合分布的假设，推导了另一种形式更为复杂的深度滤波器。

高斯分布假设下的深度滤波器：

设某个像素点的深度 $d$ 服从：

 {{< raw >}}
$$
P(d)=N\left(\mu, \sigma^{2}\right)
$$
{{< /raw >}}

每当新的数据到来，都会观测到它的深度。同样，假设这次观测也是一个高斯分布：

 {{< raw >}}
$$
P\left(d_{\text {obs }}\right)=N\left(\mu_{\text {obs }}, \sigma_{\text {obs }}^{2}\right)
$$
{{< /raw >}}

于是，问题是，如何使用观测的信息更新原先 $d$ 的分布。

这正是一个信息融合问题。

两个高斯分布的乘积依然是一个高斯分布。

设融合后的 $d$ 的分布为 $N\left(\mu_{\text {fuse }}, \sigma_{\text {fuse }}^{2}\right)$，那么根据高斯分布的乘积，有：

 {{< raw >}}
$$
\mu_{\text {fuse }}=\frac{\sigma_{\text {obs }}^{2} \mu+\sigma^{2} \mu_{\text {obs }}}{\sigma^{2}+\sigma_{\text {obs }}^{2}}, \quad \sigma_{\text {fuse }}^{2}=\frac{\sigma^{2} \sigma_{\text {obs }}^{2}}{\sigma^{2}+\sigma_{\text {obs }}^{2}} .
$$
{{< /raw >}}

由于仅有观测方程没有运动方程，所以这里深度仅用到了信息融合部分，而无须像完整的卡尔曼那样进行预测和更新（当然，可以把它看成“运动方程为深度值固定不动”的卡尔曼滤波器）。

可以看到融合的方程确实浅显易懂，不过问题仍然存在：如何确定观测到深度的分布呢？即如何计算 $\mu_{\text {obs }},\sigma_{\text {obs }}$ 呢？

关于 $\mu_{\text {obs }},\sigma_{\text {obs }}$，也存在一些不同的处理方式。

例如，考虑了几何不确定性和光度不确定性二者之和，或仅考虑几何不确定性。

这里暂时只考虑由几何关系带来的不确定性。

现在，假设通过极线搜索和块匹配确定了参考帧某个像素在当前帧的投影位置。那么，这个位置对深度的不确定性有多大呢？

以下图为例。考虑某次极线搜索，找到了 $p_1$ 对应的 $p_2$ 点，从而观测到了 $p_1$ 的深度值，认为 $p_1$ 对应的三维点为 $\boldsymbol P$。

![image-20221130145905712](/images/2022-11-29-视觉SLAM十四讲从理论到实践第2版学习笔记(实践应用P3)/7)

从而，可记 $\boldsymbol O_1 \boldsymbol P$ 为 $\boldsymbol p$，$\boldsymbol O_1 \boldsymbol O_2$ 为相机的平移 $\boldsymbol t$，$\boldsymbol O_2 \boldsymbol P$ 记为 $\boldsymbol a$。

并且，把这个三角形的下面两个角记作 $α,β$。

现在，考虑极线 $l_2$ 上存在一个像素大小的误差，使得 $β$ 角变成了 $β^{\prime}$，而 $p_2$ 也变成了 $p^{\prime}_2$，并记上面那个角为 $\gamma $。

这个像素的误差会导致 $\boldsymbol p^{\prime}$ 与 $\boldsymbol p$ 产生多大的差距呢？

先列出这些量的几何关系，显然有：

 {{< raw >}}
$$
\begin{array}{l}
\boldsymbol{a}=\boldsymbol{p}-\boldsymbol{t} \\
\alpha=\arccos \langle\boldsymbol{p}, \boldsymbol{t}\rangle \\
\beta=\arccos \langle\boldsymbol{a},-\boldsymbol{t}\rangle
\end{array}
$$
{{< /raw >}}

对 $p_2$ 扰动一个像素，使得 $\beta$ 产生一个变化量，变成 $\beta^{\prime}$。根据几何关系，有：

 {{< raw >}}
$$
\begin{aligned}
\beta^{\prime} &=\arccos \left\langle\boldsymbol{O}_{2} \boldsymbol{p}_{2}^{\prime},-\boldsymbol{t}\right\rangle \\
\gamma &=\pi-\alpha-\beta^{\prime} .
\end{aligned}
$$
{{< /raw >}}

于是，由正弦定理，$\boldsymbol p^{\prime}$ 的大小可以求得：

 {{< raw >}}
$$
\left\|\boldsymbol{p}^{\prime}\right\|=\|\boldsymbol{t}\| \frac{\sin \beta^{\prime}}{\sin \gamma}
$$
{{< /raw >}}

由此，确定了由单个像素的不确定引起的深度不确定性。

如果认为极线搜索的块匹配仅有一个像素的误差，那么就可以设：

 {{< raw >}}
$$
\sigma_{\text {obs }}=\|\boldsymbol{p}\|-\left\|\boldsymbol{p}^{\prime}\right\|
$$
{{< /raw >}}

当然，如果极线搜索的不确定性大于一个像素，则可按照此推导放大这个不确定性。

接下来的深度数据融合已经在前面介绍过了。

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

综上所述，给出了估计稠密深度的一个完整的过程：

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

#### 像素梯度的问题

块匹配的正确与否依赖于图像块是否具有区分度。

显然，如果图像块仅是一片黑或者一片白，缺少有效的信息，那么在 NCC 计算中就很可能错误地将它与周围的某块像素匹配。

这里牵涉一个问题，该问题在直接法中已经见过一次。在进行块匹配（和NCC的计算）时，必须假设小块不变，然后将该小块与其他小块进行对比。

这时，有**明显梯度**的小块将具有良好的区分度，不易引起误匹配。

对于**梯度不明显**的像素，由于在块匹配时没有区分性，将难以有效地估计其深度。

反之，像素梯度比较明显的地方，得到的深度信息也相对准确，例如桌面上的杂志、电话等具有明显纹理的物体。

因此，立体视觉中一个非常常见的问题：**对物体纹理的依赖性**。该问题在双目视觉中也极其常见，体现了立体视觉的重建质量十分依赖于环境纹理。

进一步讨论像素梯度问题，还会发现像素梯度和极线之间的联系。

以下图为例，举两种比较极端的情况：像素梯度平行于极线方向，以及垂直于极线方向。

![image-20221130152315617](/images/2022-11-29-视觉SLAM十四讲从理论到实践第2版学习笔记(实践应用P3)/8)

先来看垂直的情况。在垂直的例子里，即使小块有明显梯度，当沿着极线做块匹配时，会发现匹配程度都是一样的，因此得不到有效的匹配。

反之，在平行的例子里，能够精确地确定匹配度最高点出现在何处。

而实际中，梯度与极线的情况很可能介于二者之间：既不是完全垂直也不是完全平行。

这时，当像素梯度与极线夹角较大时，极线匹配的不确定性大；而当夹角较小时，匹配的不确定性变小。

#### 逆深度

从另一个角度看，把像素深度假设成高斯分布是否合适呢？这里关系到一个参数化的问题。

在前面的内容中，我们经常用一个点的世界坐标 $x, y, z$ 三个量来描述它，这是一种参数化形式。

我们认为 $x,y,z$ 三个量都是随机的，它们服从（三维的）高斯分布。

然而，本讲使用了图像坐标 $u,v$ 和深度值 $d$ 来描述某个空间点（即稠密建图）。认为 $u, v$ 不动，而 $d$ 服从（一维的）高斯分布，这是另一种参数化形式。

那么我们要问：这两种参数化形式有什么不同吗？是否也能假设 $u, v$ 服从高斯分布，从而形成另一种参数化形式呢？

不同的参数化形式，实际都描述了同一个量，也就是某个三维空间点。

考虑到当在相机中看到某个点时，它的图像坐标 $u,v$ 是比较确定的，而深度值 $d$ 则是非常不确定的。

此时，若用世界坐标 $x,y,z$ 描述这个点，根据相机当前的位姿，$x, y,z$ 三个量之间可能存在明显的相关性。

反映在协方差矩阵中，表现为非对角元素不为零。而如果用 $u, v, d$ 参数化一个点，那么它的 $u, v$ 和 $d$ 至少是近似独立的，甚至还能认为 $u, v$ 也是独立的——从而它的协方差矩阵近似为对角阵，更为简洁。

逆深度（Inverse depth）是近年来SLAM研究中出现的一种广泛使用的参数化技巧，之前我们假设深度值满足高斯分布 $d\sim N(\mu,\sigma^2)$。然而这样做合不合理呢？深度真的近似于一个高斯分布吗？

深度的正态分布确实存在一些问题：

1. 这个场景深度大概是5~10米，可能有一些更远的点，但近处肯定不会小于相机焦距（或认为深度不会小于0）。这个分布并不是像高斯分布那样，形成一个对称的形状。它的尾部可能稍长，而负数区域则为零。
2. 在一些室外应用中，可能存在距离非常远，乃至无穷远处的点。我们的初始值中难以涵盖这些点、并且用高斯分布描述它们会有一些数值计算上的困难。

于是，逆深度应运而生。人们在仿真中发现，假设深度的倒数，也就是**逆深度**，为高斯分布是比较有效的。

随后，在实际应用中，逆深度也具有更好的数值稳定性，从而逐渐成为一种通用的技巧，存在于现有SLAM方案中的标准做法中。

#### 图像间的变换

在块匹配之前，做一次图像到图像间的变换是一种常见的预处理方式。

这是因为，我们假设了图像小块在相机运动时保持不变，而这个假设在相机平移时能够保持成立，但当相机发生明显的旋转时，就难以继续保持了。

特别地，当相机绕光心旋转时，一块下黑上白的图像可能会变成一个上黑下白的图像块，导致相关性直接变成了负数（尽管仍然是同一个块）。

为了防止这种情况的出现，通常需要在块匹配之前，把参考帧与当前帧之间的运动考虑进来。

根据相机模型，参考帧上的一个像素 $\boldsymbol P_R$ 与真实的三维点世界坐标 $\boldsymbol P_W$ 有以下关系：

 {{< raw >}}
$$
d_{\mathrm{R}} \boldsymbol{P}_{\mathrm{R}}=\boldsymbol{K}\left(\boldsymbol{R}_{\mathrm{RW}} \boldsymbol{P}_{\mathrm{W}}+\boldsymbol{t}_{\mathrm{RW}}\right)
$$
{{< /raw >}}

类似地，对于当前帧，亦有 $\boldsymbol P_W$ 在它上边的投影，记作 $\boldsymbol P_C$：

{{< raw >}}
$$
d_{\mathrm{C}} \boldsymbol{P}_{\mathrm{C}}=\boldsymbol{K}\left(\boldsymbol{R}_{\mathrm{CW}} \boldsymbol{P}_{\mathrm{W}}+\boldsymbol{t}_{\mathrm{CW}}\right) .
$$
{{< /raw >}}

代入并消去 $\boldsymbol P_W$，即得两幅图像之间的像素关系：

{{< raw >}}
$$
d_{\mathrm{C}} \boldsymbol{P}_{\mathrm{C}}=d_{\mathrm{R}} \boldsymbol{K} \boldsymbol{R}_{\mathrm{CW}} \boldsymbol{R}_{\mathrm{RW}}^{\mathrm{T}} \boldsymbol{K}^{-1} \boldsymbol{P}_{\mathrm{R}}+\boldsymbol{K} t_{\mathrm{CW}}-\boldsymbol{K} \boldsymbol{R}_{\mathrm{CW}} \boldsymbol{R}_{\mathrm{RW}}^{\mathrm{T}} \boldsymbol{K} t_{\mathrm{RW}} .
$$
{{< /raw >}}

当知道 {{< raw >}}$d_{\mathrm{R}} ,\boldsymbol{P}_{\mathrm{R}}$ 时，可以计算出 $\boldsymbol{P}_{\mathrm{C}}${{< /raw >}} 的投影位置。

此时，再给 {{< raw >}}$\boldsymbol{P}_{\mathrm{R}}$ 两个分量各一个增量 $\mathrm du, \mathrm dv$，就可以求得 $\boldsymbol{P}_{\mathrm{C}}$ 的增量 $\mathrm du_c, \mathrm dv_c${{< /raw >}}。

通过这种方式，算出在局部范围内参考帧和当前帧图像坐标变换的一个线性关系构成仿射变换：

{{< raw >}}
$$
\left[\begin{array}{l}
\mathrm{d} u_{\mathrm{c}} \\
\mathrm{d} v_{\mathrm{c}}
\end{array}\right]=\left[\begin{array}{ll}
\frac{\mathrm{d} u_{\mathrm{c}}}{\mathrm{d} u} & \frac{\mathrm{d} u_{\mathrm{c}}}{\mathrm{d} v} \\
\frac{\mathrm{d} v_{\mathrm{c}}}{\mathrm{d} u} & \frac{\mathrm{d} v_{\mathrm{c}}}{\mathrm{d} v}
\end{array}\right]\left[\begin{array}{l}
\mathrm{d} u \\
\mathrm{~d} v
\end{array}\right]
$$
{{< /raw >}}

根据仿射变换矩阵可以将当前帧（或参考帧）的像素进行变换，再进行块匹配，以期获得对旋转更好的效果。

### RGB-D 稠密建图

除了使用单目和双目相机进行稠密重建，在适用范围内，RGB-D 相机是一种更好的选择。

在第11讲中详细讨论的深度估计问题，在 RGB-D 相机中可以完全通过传感器中硬件的测量得到，无须消耗大量的计算资源来估计。

并且，RGB-D 的结构光或飞时原理，保证了深度数据对纹理的无关性。即使面对纯色的物体，只要它能够反射光，就能测量到它的深度。这也是 RGB-D 传感器的一大优势。

利用 RGB-D 进行稠密建图是相对容易的。

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

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

在此基础上，如果对外观有进一步的要求，希望估计物体的表面，则可以使用三角网格（Mesh）、面片（Surfel）进行建图。

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

#### 点云地图

所谓点云，就是由一组离散的点表示的地图。

最基本的点包含 $x, y,z$ 三维坐标，也可以带有 $r,g,b$ 的彩色信息。

RGB-D 相机提供了彩色图和深度图，因此很容易根据相机内参来计算 RGB-D 点云。

 如果通过某种手段得到了相机的位姿，那么只要直接把点云 进行加和，就可以获得全局的点云。

点云地图提供了比较基本的可视化地图，让我们能够大致了解环境的样子。

它以三维方式存储，使我们能够快速地浏览场景的各个角落，乃至在场景中进行漫游。

点云的一大优势是可以直接由 RGB-D 图像高效地生成，不需要额外处理。

它的滤波操作也非常直观，且处理效率尚能接受。

不过，使用点云表达地图仍然是十分初级的，不妨按照之前提的对地图的需求，看看点云地图是否能满足这些需求。

1. 定位需求：取决于前端视觉里程计的处理方式。如果是基于特征点的视觉里程计，由于点云中没有存储特征点信息，则无法用于基于特征点的定位方法。如果前端是点云的ICP，那么可以考虑将局部点云对全局点云进行ICP以估计位姿。然而，这要求全局点云具有较好的精度。我们处理点云的方式并没有对点云本身进行优化,所以是不够的。
2. 导航与避障的需求：无法直接用于导航和避障。纯粹的点云无法表示“是否有障碍物”的信息，也无法在点云中做“任意空间点是否被占据”这样的查询，而这是导航和避障的基本需要。不过，可以在点云基础上进行加工，得到更适合导航与避障的地图形式。
3. 可视化和交互：具有基本的可视化与交互能力。我们能够看到场景的外观，也能在场景里漫游。从可视化角度来说，由于点云只含有离散的点，没有物体表面信息（例如法线），所以不太符合人们的可视化习惯。例如，从正面和背面看点云地图的物体是一样的，而且还能透过物体看到它背后的东西：这些都不太符合我们日常的经验。

综上所述，点云地图是“基础的”或“初级的”，是指它更接近传感器读取的原始数据。它具有一些基本的功能，但通常用于调试和基本的显示，不便直接用于应用程序。如果我们希望地图有更高级的功能，那么点云地图是一个不错的出发点。

例如，针对导航功能，可以从点云出发，构建占据网格（Occupancy Grid）地图，以供导航算法查询某点是否可以通过。

再如，SfM 中常用的泊松重建方法，就能通过基本的点云重建物体网格地图，得到物体的表面信息。

除了泊松重建，Surfel 也是一种表达物体表面的方式，以面元作为地图的基本单位，能够建立漂亮的可视化地图。
#### 八叉树地图

一种在导航中比较常用的、本身有较好的压缩性能的地图形式：**八叉树地图**（Octo-map）。

在点云地图中，虽然有了三维结构，也进行了体素滤波以调整分辨率，但是点云有几个明显的缺陷：

1. 点云地图通常规模很大，所以 *pcd* 文件也会很大。一幅 $640$ 像素 × $480$ 像素的图像，会产生 $30$ 万个空间点，需要大量的存储空间。即使经过一些滤波后 *pcd* 文件也是很大的。而且讨厌之处在于，它的“大”并不是必需的。点云地图提供了很多不必要的细节。我们并不特别关心地毯上的褶皱、阴暗处的影子这类东西，把它们放在地图里是在浪费空间。由于这些空间的占用，除非我们降低分辨率，否则在有限的内存中无法建模较大的环境，然而降低分辨率会导致地图质量下降。
2. 点云地图无法处理运动物体。因为我们的做法里只有“添加点”，而没有“当点消失时把它移除”的做法。而在实际环境中，运动物体的普遍存在，使得点云地图变得不够实用。

把三维空间建模为许多个小方块（或体素）是一种常见的做法。

如果把一个小方块的每个面平均切成两片，那么这个小方块就会变成同样大小的八个小方块。这个步骤可以不断地重复，直到最后的方块大小达到建模的最高精度。

在这个过程中，把“将一个小方块分成同样大小的八个”这件事，看成“从一个节点展开成八个子节点”，那么，整个从最大空间细分到最小空间的过程，就是一棵**八叉树**。

如下图所示，左侧显示了一个大立方体不断地均匀分成八块，直到变成最小的方块为止。

![image-20221130160555329](/images/2022-11-29-视觉SLAM十四讲从理论到实践第2版学习笔记(实践应用P3)/9)

于是，整个大方块可以看作根节点，而最小的块可以看作“叶子节点”。

于是，在八叉树中，当由下一层节点往上走一层时，地图的体积就能扩大为原来的八倍。

如果叶子节点的方块大小为 $1 \mathrm {cm^3}$，那么当限制八叉树为 $10$ 层时，总共能建模的体积大约为 $8^{10} \mathrm{cm^3} = 1073 \mathrm{m^3}$，这足够建模一间屋子了。

由于体积与深度呈指数关系，所以当用更大的深度时，建模的体积会增长得非常快。

在八叉树中，在节点中存储它是否被占据的信息。当某个方块的所有子节点都被占据或都不被占据时，就没必要展开这个节点。

例如，一开始地图为空白时，只需一个根节点，不需要完整的树。当向地图中添加信息时，由于实际的物体经常连在一起，空白的地方也会常常连在一起，所以大多数八叉树节点无须展开到叶子层面。

所以说，八叉树比点云节省大量的存储空间。

前面说八叉树的节点存储了它是否被占据的信息。从点云层面来讲，自然可以用 $0$ 表示空白，$1$ 表示被占据。

这种 $0-1$ 的表示可以用一个比特来存储，节省空间，不过显得有些过于简单了。

由于噪声的影响，可能会看到某个点一会儿为 $0$，一会儿为 $1$；或者多数时刻为 $0$，少数时刻为 $1$；或者除了“是”“否”两种情况，还有一个“未知”的状态。

能否更精细地描述这件事呢？我们会选择用**概率**形式表达某节点是否被占据的事情。

例如，用一个浮点数 $x \in [0,1]$ 来表达。这个 $x$ 一开始取 $0.5$。如果不断观测到它被占据，那么让这个值不断增加；反之，如果不断观测到它是空白，那就让它不断减小即可。

通过这种方式，动态地建模了地图中的障碍物信息。不过，现在的方式有一点小问题：

如果让 $x$ 不断增加或减小，它可能跑到 $[0,1]$ 区间之外，带来处理上的不便。

所以不是直接用概率来描述某节点被占据，而是用概率对数值（Log-odds）来描述。

设 $y \in \mathbb R$ 为概率对数值，$x$ 为 $0\sim1$ 的概率，那么它们之间的变换由 logit 变换描述：

{{< raw >}}
$$
y=\operatorname{logit}(x)=\log \left(\frac{x}{1-x}\right)
$$
{{< /raw >}}

其反变换为：

{{< raw >}}
$$
x=\operatorname{logit}^{-1}(y)=\frac{\exp (y)}{\exp (y)+1} .
$$
{{< /raw >}}

可以看到，当 $y$ 从 $-\infty$ 变到 $+\infty$ 时，$x$ 相应地从 $0$ 变到了 $1$。而当 $y$ 取 $0$ 时，$x$ 取 $0.5$。

因此，不妨存储 $y$ 来表达节点是否被占据。当不断观测到“占据”时，让 $y$ 增加一个值；否则就让 $y$ 减小一个值。

当查询概率时，再用逆 logit 变换，将 $y$ 转换至概率即可。

用数学形式来说，设某节点为 $n$，观测数据为 $z$。

那么从开始到 $t$ 时刻某节点的概率对数值为 $L\left(n \mid z_{1: t}\right)$，$t+1$ 时刻为：

{{< raw >}}
$$
L\left(n \mid z_{1: t+1}\right)=L\left(n \mid z_{1: t-1}\right)+L\left(n \mid z_{t}\right)
$$
{{< /raw >}}

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

{{< raw >}}
$$
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}
$$
{{< /raw >}}

有了对数概率，就可以根据 RGB-D 数据更新整个八叉树地图了。

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

利用这个信息，可以很好地对八叉树地图进行更新，并且能处理运动的结构。

### * TSDF 地图和 Fusion 系列

实时三维重建

在前面的地图模型中，**以定位为主体**。地图的拼接是作为后续加工步骤放在SLAM框架中的。

这种框架成为主流的原因是定位算法可以满足实时性的需求，而地图的加工可以在关键帧处进行处理，无须实时响应。

定位通常是轻量级的，特别是当使用稀疏特征或稀疏直接法时；相应地，地图的表达与存储则是重量级的。它们的规模和计算需求较大，不利于实时处理。特别是稠密地图，往往只能在关键帧层面进行计算。

但是，现有做法中并没有对稠密地图进行优化。

例如，当两幅图像都观察到同一把椅子时，只根据两幅图像的位姿把两处的点云进行叠加，生成了地图。

由于位姿估计通常是带有误差的，这种直接拼接往往不够准确，比如同一把椅子的点云无法完美地叠加在一起。

这时，地图中会出现这把椅子的两个重影——这种现象被形象地称为“鬼影”。

这种现象显然不是我们想要的，我们希望重建结果是光滑的、完整的，是符合我们对地图的认识的。

在这种思想下，出现了一种以“建图”为主体，定位居于次要地位的做法，也就是实时三维重建。

由于三维重建把重建准确地图作为主要目标，所以需要利用GPU进行加速，甚至需要非常高级的GPU或多个GPU进行并行加速，通常需要较重的计算设备。

与之相反，SLAM是朝轻量级、小型化方向发展的，有些方案甚至放弃了建图和回环检测部分，只保留了视觉里程计。而实时重建则正在朝大规模、大型动态场景的重建方向发展。

自从 RGB-D 传感器出现以来，利用 RGB-D 图像进行实时重建成为了一个重要的发展方向，陆续产生了 Kinect Fusion、Dynamic Fusion、Elastic Fusion、Fusion4D、Volumn Deform 等成果。

以经典的 TSDF 地图为代表进行介绍。TSDF是 Truncated Signed Distance Function 的缩写，不妨译作**截断符号距离函数**。

虽然把“函数”称为“地图”似乎不太妥当，但是在没有更好的翻译之前，暂时称它为 TSDF 地图、TSDF 重建等，只要不产生理解上的偏差即可。

与八叉树相似，TSDF 地图也是一种网格式（或者说方块式）的地图，如下图所示。

![image-20221130163608659](/images/2022-11-29-视觉SLAM十四讲从理论到实践第2版学习笔记(实践应用P3)/10)

先选定要建模的三维空间，比如 $3\mathrm m \times 3\mathrm m \times 3\mathrm m$ 那么大，按照一定分辨率将这个空间分成许多小块，存储每个小块内部的信息。

不同的是，TSDF 地图整个存储在显存而不是内存中。利用GPU的并行特性，可以并行地对每个体素进行计算和更新，而不像CPU遍历内存区域那样不得不串行。

每个 TSDF 体素内存储了该小块与距其最近的物体表面的距离。如果小块在该物体表面的前方，则它有一个正值；反之，如果该小块位于表面之后，那么就为负值。

由于物体表面通常是很薄的一层，所以就把值太大的和太小的都取成 $1$ 和 $-1$，这样就得到了截断之后的距离，也就是所谓的 TSDF。

那么按照定义，TSDF 为 $0$ 的地方就是表面本身——或者，由于数值误差的存在，TSDF由负号变成正号的地方就是表面本身。

在图的下部，我们看到一个类似人脸的表面出现在 TSDF 改变符号的地方。

TSDF 也有“定位”与“建图”两个问题，与SLAM非常相似，不过具体的形式与本书前面几讲介绍的稍有不同。

在这里，定位问题主要指如何把当前的 RGB-D 图像与GPU中的 TSDF 地图进行比较，估计相机位姿。

而建图问题就是如何根据估计的相机位姿，对 TSDF 地图进行更新。传统做法中，还会对 RGB-D 图像进行一次双边贝叶斯滤波，以去除深度图中的噪声。

TSDF 的定位类似于前面介绍的 ICP，不过由于GPU的并行化，可以对整张深度图和 TSDF 地图进行 ICP 计算，而不必像传统视觉里程计那样必须先计算特征点。

同时，由于 TSDF 没有颜色信息，意味着可以只使用深度图，不使用彩色图就完成位姿估计，这在一定程度上摆脱了视觉里程计算法对光照和纹理的依赖性，使得 RGB-D 重建更加稳健。

另外，建图部分也是一种并行地对 TSDF 中的数值进行更新的过程，使得所估计的表面更加平滑可靠。

### 小结

本讲介绍了一些常见的地图类型，尤其是稠密地图形式。

根据单目相机或双目相机可以构建稠密地图，相比之下，RGB-D 地图往往更容易、稳定一些。