SLAM拾萃(1)：octomap

前言

---

 　　大家好，时隔多年之后，我又开始了博客旅程。经历了很多事情之后呢，我发现自己的想法真的很简单：好好读书做课题，闲下来时写写博客，服务大家。所以我会继续写SLAM相关的博客。如果你觉得它对你有帮助，那是最好不过的啦！写作过程中得到了许多热心读者的帮助与鼓励，有些读者还成了要好的朋友，在此向大家致谢啦！关于SLAM，读者也会有很多问题。由于我个人精力和学力都有限，无法一一回答，向大家说声抱歉！有些共同的问题，我肯定会在博客里介绍的！

　　前两天刚从珠海开会回来，与中山大学的同学们聚在一起玩耍，很开心！

　　《一起做》系列已经结束，事实上它是我以前探索过程中的一些总结。虽然仍然有很多令人不满意的地方，不过相信读了那个系列，读者应该对SLAM的流程有一定的了解了。尤其是通过代码，你能知道许多论文里没讲清楚的细节。在这之后，我现在有两个规划。一是对目前流行的SLAM程序做一个介绍，沿着《视觉SLAM实战》往下写；二是介绍一些好用的开源工具/库，写成一个《SLAM拾萃》。我觉得这两部分内容，对读者了解SLAM会有较大的帮助。当然，如果你对我的博客有任何建议，可以在下方评论或给我发邮件。

　　本篇是《SLAM拾萃》第一篇，介绍一个建图工具：octomap。和往常一样，我会介绍它的原理、安装与使用方式，并提供例程供读者学习。必要时也会请小萝卜过来吐槽。（小萝卜真是太好用了，它可以代替读者提很多问题。）

---

 什么是octomap？

　　RGBD SLAM的目的有两个：估计机器人的轨迹，并建立正确的地图。地图有很多种表达方式，比如特征点地图、网格地图、拓扑地图等等。在《一起做》系列中，我们使用的地图形式主要是点云地图。在程序中，我们根据优化后的位姿，拼接点云，最后构成地图。这种做法很简单，但有一些明显的缺陷：

• 地图形式不紧凑。
  　　点云地图通常规模很大，所以一个pcd文件也会很大。一张640$\times$480的图像，会产生30万个空间点，需要大量的存储空间。即使经过一些滤波之后，pcd文件也是很大的。而且讨厌之处在于，它的“大”并不是必需的。点云地图提供了很多不必要的细节。对于地毯上的褶皱、阴暗处的影子，我们并不特别关心这些东西。把它们放在地图里是浪费空间。
• 处理重叠的方式不够好。
  　　在构建点云时，我们直接按照估计位姿拼在了一起。在位姿存在误差时，会导致地图出现明显的重叠。例如一个电脑屏变成了两个，原本方的边界变成了多边形。对重叠地区的处理方式应该更好一些。
• 难以用于导航
  　　说起地图的用处，第一就是导航啦！有了地图，就可以指挥机器人从A点到B点运动，岂不是很方便的事？但是，给你一张点云地图，是否有些傻眼了呢？我至少得知道哪些地方可通过，哪些地方不可通过，才能完成导航呀！光有点是不够的！

　　octomap就是为此而设计的！亲，你没有看错，它可以优雅地压缩、更新地图，并且分辨率可调！它以八叉树（octotree，后面会讲）的形式存储地图，相比点云，能够省下大把的空间。octomap建立的地图大概是这样子的：（从左到右是不同的分辨率）

　　

　　由于八叉树的原因，它的地图像是很多个小方块组成的（很像minecraft）。当分辨率较高时，方块很小；分辨率较低时，方块很大。每个方块表示该格被占据的概率。因此你可以查询某个方块或点“是否可以通过”，从而实现不同层次的导航。简而言之，环境较大时采用较低分辨率，而较精细的导航可采用较高分辨率。

　　小萝卜：师兄你这是介绍吗？真像广告啊……

---

 octomap原理

　　本段会讲一些数学知识。如果你想“跑跑程序看效果”，可以跳过本段。

1. 八叉树的表达

　　八叉树，也就是传说中有八个子节点的树！是不是很厉害呢？至于为什么要分成八个子节点，想象一下一个正方形的方块的三个面各切一刀，不就变成八块了嘛！如果你想象不出来，请看下图：

 

　　实际的数据结构呢，就是一个树根不断地往下扩，每次分成八个枝，直到叶子为止。叶子节点代表了分辨率最高的情况。例如分辨率设成0.01m，那么每个叶子就是一个1cm见方的小方块了呢！

　　

　　每个小方块都有一个数描述它是否被占据。在最简单的情况下，可以用0－1两个数表示（太简单了所以没什么用）。通常还是用0～1之间的浮点数表示它被占据的概率。0.5表示未确定，越大则表示被占据的可能性越高，反之亦然。由于它是八叉树，那么一个节点的八个孩子都有一定的概率被占据或不被占据啦！（下图是一棵八叉树）

　　用树结构的好处时：当某个节点的子结点都“占据”或“不占据”或“未确定”时，就可以把它给剪掉！换句话说，如果没必要进一步描述更精细的结构（孩子节点）时，我们只要一个粗方块（父节点）的信息就够了。这可以省去很多的存储空间。因为我们不用存一个“全八叉树”呀！

　　2.　八叉树的更新

　　在八叉树中，我们用概率来表达一个叶子是否被占据。为什么不直接用0－1表达呢？因为在对环境的观测过程中，由于噪声的存在，某个方块有时可能被观测到是“占据”的，过了一会儿，在另一些方块中又是“不占据”的。有时“占据”的时候多，有时“不占据”的时候多。这一方面可能是由于环境本身有动态特征（例如桌子被挪走了），另一方面（多数时候）可能是由于噪声。根据八叉树的推导，假设$t＝1，\ldots, T$时刻，观测的数据为$z_1, \ldots, z_T$，那么第$n$个叶子节点记录的信息为：$$P(n|z_{1:T}) =  \left[ 1+ \frac{1-P(n|z_T)}{P(n|z_T)} \frac{1-P(n|z_{1:T-1})}{P(n|z_{1:T-1})} \frac{P(n)}{1-P(n)} \right]^{-1}  \quad \quad (1)$$ 

　　小萝卜：哇！又一个好长的式子！这说的是啥师兄？

　　师兄：哎，写论文非得把一些简单的事情写得很复杂。为了解释这东西，先讲一下 logit 变换。该变换把一个概率$p$变换到全实数空间$R$上：　　$$ \alpha = logit(p) = log \left( \frac{p}{1-p} \right) $$

　　这是一个可逆变换，反之有：　　$$ p = logit^{-1} (\alpha) = \frac{1} { 1+ exp(-\alpha) }. $$

　　$\alpha$叫做log-odds。我们把用$L()$叶子节点的log-odds，那么(1)就可以写成：　　$$ L( n| z_{1:T} ) = L(n|z_{1:T-1}) + L(n|z_T) $$

　　小萝卜：哦！这个我就懂了！每新来一个就直接加到原来的上面，是吧？

　　师兄：对，此外还要加一个最大最小值的限制。最后转换回原来的概率即可。

　　八叉树中的父亲节点占据概率，可以根据孩子节点的数值进行计算。比较简单的是取平均值或最大值。如果把八叉树按照占据概率进行渲染，不确定的方块渲染成透明的，确定占据的渲染成不透明的，就能看到我们平时见到的那种东西啦！

　　octomap本身的数学原理还是简单的。不过它的可视化做的比较好。下面我们来讲讲如何下载、安装八叉树程序，并给出几个小的例程。

---

 安装octomap

　　octomap的网页见：https://octomap.github.io

　　它的github源码在：https://github.com/OctoMap/octomap

　　它还有ROS下的安装方式：http://wiki.ros.org/octomap

　　在开发过程中，可能需要不断地查看它的API文档。你可以自己用doxygen生成一个，或者查看在线文档：http://octomap.github.io/octomap/doc/

　　为了保持简洁，我们不要求读者安装ROS，仅介绍单独的octomap。我的编译环境是ubuntu 14.04。ubuntu系列的应该都不会有太大问题。

　　1.　　编译octomap
　　　新建一个目录，拷贝octomap代码。如果没有git请安装git：sudo apt-get install git

1. git clone https://github.com/OctoMap/octomap

   git会把代码拷贝到当前目录/octomap下。进入该目录，参照README.md进行安装。编译方式和普通的cmake程序一样，如果你学过《一起做》就应该很熟悉了：

   1 mkdir build
   2 cd build
   3 cmake ..
   4 make

    事实上，octomap的代码主要含两个模块：本身的octomap和可视化工具octovis。octovis依赖于qt4和qglviewer，所以如果你没有装这两个依赖，请安装它们：sudo apt-get install libqt4-dev qt4-qmake libqglviewer-dev

   如果编译没有给出任何警告，恭喜你编译成功！

2. 使用octovis查看示例地图
   在bin/文件夹中，存放着编译出来可执行文件。为了直观起见，我们直接看一个示例地图：
   

   bin/octovis octomap/share/data/geb079.bt

    octovis会打开这个地图并显示。它的UI是长这样的。你可以玩玩菜单里各种东西（虽然也不多，我就不一一介绍UI怎么玩了），能看出这是一层楼的扫描图。octovis是一个比较实用的工具，你生成的各种octomap地图都可以用它来看。（所以你可以把octovis放到/usr/local/bin/下，省得以后还要找。）

   

---

例程1：转换pcd到octomap

　　GUI玩够了吧？仅仅会用UI是不够滴，现在让我们开始编代码使用octomap这个库吧！

　　我为你准备了三个小例程。在前两个中，我会教你如何将一个pcd格式的点云地图转换为octomap地图。后一个中，我会讲讲如何根据g2o优化的轨迹，以类似slam的方式，把几个RGBD图像拼接出一个octomap。这对你研究SLAM会有一些帮助。所有的代码与数据都可以在我的github上找到。有关编译的信息，我写在这个代码的Readme中了，请在编译前看一眼如何编译这些代码。

　　源代码地址：https://github.com/gaoxiang12/octomap_tutor

　　源代码如下：src/pcd2octomap.cpp 这份代码将命令行参数1作为输入文件，参数2作为输出文件，把输入的pcd格式点云转换成octomap格式的点云。通过这个例子，你可以学会如何创建一个简单的OcTree对象并往里面添加新的点。　　

 1 #include <iostream>
 2 #include <assert.h>
 3 
 4 //pcl
 5 #include <