读书笔记：建图
本讲我们开始介绍建图部分的算法。在前端和后端中，我们重点关注同时估计相机运动轨迹与特征点空间位置的问题。然而，在实际使用SLAM 时，除了对相机本体进行定位之外，还存在许多其他的需求。例如，考虑放在机器人上的SLAM，那么我们会希望地图能够用于定位、导航、避障和交互，特征点地图显然不能满足所有的这些需求。所以，本章我们将更详细地讨论各种形式的地图，并指出目前视觉SLAM 地图中存在着的缺陷。
概述：
应用层面对于“定位”的需求是相似的，他们希望SLAM 提供相机或搭载相机的主体的空间位姿信息。而对于地图，则存在着许多不同的需求。在视觉SLAM 看来，“建图”是服务于“定位”的；但是在应用层面看来，“建图”明显还带有许多其他的需求。关于地图的用处，我们大致归纳如下:
- 定位。定位是地图的一个基本功能。在前面的视觉里程计章节，我们讨论了如何利用局部地图来实现定位。或者，在回环检测章节，我们也看到，只要有全局的描述子信息，我们也能通过回环检测确定机器人的位置。更进一步，我们还希望能够把地图保存下来，让机器人在下次开机后依然能在地图中定位，这样只需对地图进行一次建模，而不是每次启动机器人都重新做一次完整的SLAM。
- 导航。导航是指机器人能够在地图中进行路径规划，从任意两个地图点间寻找路径，然后控制自己运动到目标点的过程。该过程中，我们至少需要知道地图中哪些地方不可通过，而哪些地方是可以通过的。这就超出了稀疏特征点地图的能力范围，我们必须有另外的地图形式。稍后我们会说，这至少得是一种稠密的地。
- 避障。避障也是机器人经常碰到的一个问题。它与导航类似，但更注重局部的、动态的障碍物的处理。同样的，仅有特征点，我们无法判断某个特征点是否为障碍物，所以我们将需要稠密地图。
- 重建。有时候，我们希望利用SLAM 获得周围环境的重建效果，并把它展示给其他人看。这种地图主要用于向人展示，所以我们希望它看上去比较舒服、美观。或者，我们也可以把该地图用于通讯，使其他人能够远程地观看我们重建得到的三维物体或场景——例如三维的视频通话或者网上购物等等。这种地图亦是稠密的，并且我们还对它的外观有一些要求。我们可能不满足于稠密点云重建，更希望能够构建带纹理的平面，就像电子游戏中的三维场景那样。
- 交互。交互主要指人与地图之间的互动。例如，在增强现实中，我们会在房间里放置虚拟的物体，并与这些虚拟物体之间有一些互动——比方说我会点击墙面上放着的虚拟网页浏览器来观看视频，或者向墙面投掷物体，希望它们有（虚拟的）物理碰撞。另一方面，机器人应用中也会有与人、与地图之间的交互。例如机器人可能会收到命令“取桌子上的报纸”，那么，除了有环境地图之外，机器人还需要知道哪一块地图是“桌子”，什么叫做“之上”，什么又叫做“报纸”。这需要机器人对地图有更高级层面的认知——亦称为语义地图。
地图的表达仍在研究中：
- 每一个地图展开谈都是比较大的主题
- 动态/带人物/长时间的地图
- 参数化的紧凑表达
- 语义信息
实践部分
首先,从 这里 下载示例程序所用的数据。解压后，将在 test_-data/Images 中发现从 0 至 200 的所有图像，并在 test_data 目录下看到一个文本文件，它记录了每张图像对应的位姿:
单目稠密重建：dense_mapping.cpp
程序输出的信息比较简洁,仅显示了迭代次数、当前图像和深度图。关于深度图,我们显示的是深度值乘以 0.4 后的结果——也就是纯白点(数值为 1.0)的深度约 2.5 米,颜色越深表示深度值越小,也就是物体离我们越近。如果实际运行了程序,应该会发现深度估计是一个动态的过程——从一个不怎么确定的初始值逐渐收敛到稳定值的过程。我们的初始值使用了均值和方差均为 3.0 的分布。当然你也可以修改初始分布,看看对结果会产生怎样的影响。
编译此程序后,以数据集目录作为参数,运行之：
/home/wh/shenlan/slambook2/ch12/dense_mono/cmake-build-debug/dense_mapping /home/wh/shenlan/slambook2/ch12/test_data
read total 202 files.
*** loop 1 ***
Average squared error = 1.85683, average error: -1.13456
*** loop 2 ***
Average squared error = 1.44679, average error: -0.879464
*** loop 3 ***
Average squared error = 1.08111, average error: -0.63987
*** loop 4 ***
Average squared error = 0.906275, average error: -0.533651
*** loop 5 ***
Average squared error = 0.47777, average error: -0.193735
*** loop 6 ***
...
下图是演示程序运行时截图。两图分别是迭代 8 次和 34 次的结果。
从截图可以发现,当迭代次数超过一定次之后,深度图趋于稳定,不再对新的数据产生改变。观察稳定之后的深度图,我们发现大致可以看出地板和桌子的区别,而桌上的物体深度则接近于桌子。整个估计大部分是正确的,但也存在着大量错误估计。它们表现为深度图中,与周围数据不一致的地方,为过大或过小的估计。此外,位于边缘处的地方,由于运动过程中看到的次数较少,所以亦没有得到正确的估计。综上所述,我们认为这个深度图的大部分是正确的,但没有达到预想的效果。
点云地图：dense_RGBD
pointcloud_mapping：
首先需要安装pcl库，关于安装，参考本人之前的博客。
编译运行结果：
然后在dense_RGBD目录下输入命令：pcl_viewer map.pcd ，即可看到内容：
surfel_mapping ：
在配置项中选择我们刚才生成的pcd图，即map.pcd，然后运行：
运行结果为：
/home/wh/shenlan/slambook2/ch12/dense_RGBD/cmake-build-debug/surfel_mapping /home/wh/shenlan/slambook2/ch12/dense_RGBD/map.pcd
point cloud loaded, points: 31876
computing normals ...
computing mesh ...
display mesh ...
Process finished with exit code 0
从点云重建得到的表面和网格模型为：
octomap_mapping:
这个是通过程序演示一下 octomap 的建图过程。首先,请安装 octomap 库:github地址。Octomap 库主要包含 octomap 地图与 octovis(一个可视化程序),二者都是 cmake 工程。请读者自行对它们进行编译和安装。主要依赖项是 doxygen:
sudo apt-get install doxygen
我们使用了 octomap::OcTree 来构建整张地图。实际上 octomap 提供了许多种八叉树:有带地图的,有带占据信息的,你也可以自己定义每个节点需要携带哪些变量。简单起见,我们使用了不带颜色信息的,最基本的八叉树地图。
在之前编译 octovis 时,我们实际上安装了一个可视化程序,即 octovis：
sudo apt-get install libqglviewer-dev-qt4
sudo apt-get install liboctomap-dev octovis
安装完成后运行：octovis：
程序运行结果如下：
现在,调用它打开地图文件，就能看到地图的实际样子了：
下图显示了我们构建的地图结果。由于我们没有在地图中加入颜色信息,所以一开始打开地图时将是灰色的,按 1 键可以根据高度信息进行染色：
在右侧有八叉树地深度限制条,这里可以调节地图的分辨率。由于我们构造时使用的默认深度是 16 层,所以这里显示 16 层的话即最高分辨率,也就是每个小块的边长为 0.05米。当我们将深度减少一层时,八叉树的叶子节点往上提了一层,每个小块的边长就增加两倍,变成 0.1 米。可以看到,我们能够很容易地调节地图分辨率以适应不同的场合。Octomap 还有一些可以探索的地方,例如,我们可以方便地查询任意点的占据概率,以此设计在地图中进行导航的方法。
课后习题
1. 推导式（12.6）。
2. 把本讲的稠密深度估计改成半稠密，你可以先把梯度明显的地方筛选出来。
3. *把本讲演示的单目稠密重建代码，从正深度改成逆深度，并添加仿射变换。你的实验效果是否有改进？
参考第二问中的博客
4. 你能论证如何在八叉树中进行导航或路径规划吗？
通过平面检测，识别出地面、天花板等，直接用一个大的立方体替换掉Octomap的小方块；或者通过物体识别算法识别出环境中的物体，用物体的3D模型替换Octomap，这样也可以大大减少碰撞检测的计算量
5. 研究[120]，探讨TSDF 地图是如何进行位姿估计和更新的。它和我们之前讲过的定位建图算法有何异同？
6. *研究均匀——高斯混合滤波器的原理与实现。
