Octomap论文解读与ORBSLAM2应用

OctoMap:An Efficient Probabilistic 3D Mapping Framework Based on Octrees

摘要

三维模型提供了空间的立体表示，这对于各种机器人应用包括飞行机器人和配备机械臂的机器人非常重要。在本文中，我们提出了一个开源框架来生成三维立体环境模型。我们的建图方法是基于八叉树和使用概率占据估计。它不仅明确地表示了被占据的空间，而且还表示了自由和未知的领域。此外，我们还提出了一种八叉树地图的压缩方法来保持三维模型的紧凑性。我们的框架是开放源码的C++库，已经成功地应用在多个机器人项目中。我们提出了一系列的实验结果处理了真实机器人和公开可用的真实世界的数据集。结果表明，我们的方法能够在保持最小内存需求的同时，有效地更新地图表示，并对数据进行一致性建模。

主要贡献

• 提出了一种基于八叉树的三维环境综合表示框架OctoMap
• 满足了大多数机器人应用对于地图的要求:概率表示，未建图区域的建模，高效性
• 可以在保持最小内存消耗的情况下高效地更新地图中的占据和自由区域

OctoMap建图框架

八叉树

八叉树是表示三维空间的层次数据结构。八叉树中的每个节点表示立方体积中包含的空间，通常称为体素。如图所示，在达到给定的最小体素大小之前，该体积被连续细分为八个子体积。

最小体素大小决定了八叉树的分辨率。由于八叉树是一种层次数据结构，如果内部节点的状态保持一致，则可以在任何层级对树进行剪枝以获得粗略的划分。下图展示了八叉树地图在不同分辨率等级下进行占据体素查询的结果。

在最基本的形式中，八叉树可以用来模拟一个对象的属性。在机器人建图的场景中，这通常是体积的占据与否。如果某个体素被测量为已占据，则八叉树中相应的节点将被初始化。任何未初始化的节点都可能是自由的或未知的。为了解决这个矛盾，我们在树中显式地表示自由体素。这些体素是在传感器和被测点之间的区域创建的，例如，沿着光激光发射方向，在传感器中心和激光击中点连线上的所有体素都被视为自由体素。未初始化的体素隐式的建模了未知区域。下图显示了一个八叉树，其中包含来自真实激光传感器数据的自由和占据节点。

使用布尔占据状态或离散标签允许八叉树进行更加紧凑的表示：如果一个节点的所有子节点具有相同的状态（占据或空闲）它们可以被修剪掉，这将大大减少树中需要维护的节点数。

在机器人系统中，人们通常必须应对传感器噪声和暂时或永久性的环境变化。在这种情况下，单独的占据标签将是不够的。相反，必须对占据状态进行概率建模(也就是占据概率)，例如通过应用占据栅格地图(occupancy grid map)。然而，这种概率模型缺乏通过剪枝进行无损压缩的可能性(剪枝后无法无损地恢复子节点的状态)。

在数据访问复杂性方面，由于树结构的原因，与固定大小的三维网格相比，八叉树需要额外的开销。对包含n个节点，树深度为d的八叉树中的单个随机节点进行查询的复杂度为$O(d)=O(logn)$。以深度优先的方式遍历完整树需要$O(n)$的复杂性。请注意，在实践中，八叉树会被限制一个最大的深度$d_{max}$。这意味着在$d_{max}$为常数的情况下产生$O(d_{max})$的随机节点查找复杂性。因此，对于固定的深度为$d_{max}$的八叉树，与相应的3D网格相比的额外开销是恒定的。

概率传感器信息融合

在坐着的框架中，传感器读数使用占据栅格地图的方式进行融合，给定传感器的测量值$z_{1:t}$,一个叶节点$n$被占据的概率为
$$P(n|z_{1:t})=[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)}{]}^{-1}$$
此更新公式取决于当前测量值$z_t$、先验概率$P(n)$和之前的估计$P(n|z_{1:t-1})$. $P(n|z_t)$表示给定测量值时体素被占据的概率。这个值依赖于特定的传感器。

先验概率$P(n)$一般假设为0.5，通过使用对数概率表示的方法，公式可以被重写为
$$L(n|z_{1:t})=L(n|z_{1:t-1})+L(n|z_t)$$

这个公式将乘法转换成了加法，对于预先已知的传感器模型，最后一项不需要每次计算，可以事先计算出来，大大加速了更新。同时，对数概率可以很方便的转换为概率，所以在八叉树中储存的是对数概率而不是概率。

当使用3D地图进行导航时，通常会给定一个占据概率的阈值。当达到阈值时，体素被认为是被占据的，否则被认为是自由的，从而定义了两个离散状态。从更新公式可以看出，要改变体素的状态，我们需要整合尽可能多的观测值，因为当前状态的定义已经整合了所有之前的观测值。换言之，如果一个体素被观察到状态为自由$k$次，那么它必须被观察到至少占据了$k$次，然后根据阈值才有可能被认为是占据的（假设自由和占据的测量在传感器模型中的更新权重相等）。虽然这种特性在静态环境中是理想的，但是移动机器人经常面临着环境的临时或永久变化，地图必须快速适应这些变化。为了确保这种适应性，在框架中使用了一种截断更新策略，定义了占据概率的上下限。而不是直接使用上述公式更新，截断更新公式如下
$$L(n|z_{1:t})=max(min(L(n|z_{1:t-1})+L(n|z_t),l_{max}),l_{min})$$
其中，$l_{min}$和$l_{max}$表示对数概率值的上下限。直观地说，这个修改后的更新公式限制了改变体素状态所需的更新次数。应用截断更新策略有两个优点：保证了地图中的可信度保持有界，因此模型能够快速适应环境的变化。此外，可以通过修剪来整合相邻的体素。这导致必须保存的体素数量显著减少。用截断实现的压缩在完整的概率方面不再是完全无损的，因为接近0和1的信息被丢失了，但是在阶段阈值之间，完整的概率被保留。

多分辨率查询

当测量值被融合到八叉树结构中时，概率更新只对八叉树中的叶节点执行。但由于八叉树是一种层次化的数据结构，可以利用树中的内部节点来实现多分辨率查询。注意到当树只被遍历到一个给定的深度而不是叶节点的深度时，产生了一个三维空间的粗糙划分。每个内部节点包含其八个子节点占据的体积，从而为了确定一个内部节点的占据概率，必须对其子节点的占据概率进行融合。可以采用几种策略来确定一个节点在给定其其八个子节点的占据概率时的占据概率。

• 平均策略
• 取最大值策略

利用取最大值策略进行融合是一种非常适合于机器人导航的偏向于保守的策略。因此，在框架中使用最大值策略进行融合。

八叉树地图的压缩

在框架中，我们通过截断更新策略来实现地图压缩。当一个体素的对数概率值达到下限最小值或上限最大值时，则认为这个体素节点是稳定的。直观地说，稳定的节点已经在非常高的置信度下被测量为自由的或者占据的。在静态环境中，经过足够多的测量后，所有的体素都会收敛到一个稳定的状态。如果一个内部节点的所有子节点都是具有相同稳定状态的叶节点，则可以对子节点进行剪枝。如果未来的测量与相应的内部节点的状态相矛盾，则其子节点将相应地重新生成和更新。

八叉树地图用于储存其他信息

可以扩展八叉树节点来存储额外的数据，以丰富地图的表示。例如，体素可以存储地形信息、环境数据（如温度）或颜色信息。每一个附加的体素属性都有一种用来融合多种测量量的方法（例如上述提到的概率信息融合方法）。例如，可以扩展了建图框架来存储每个体素的平均颜色信息。这为用户进行可视化，并实现基于颜色的环境分类或基于外观的机器人定位提供了方便。它也可以作为创建彩色高分辨率表面网格的起点。

八叉树的地图按子图分层构建

这个方法在树结构中维护子图的集合，其中每个节点表示环境的中的一个子空间。子空间的定义基于用户定义的输入分割和表示各个分割体之间的相对空间关系。

下图给出了基于对象位于支撑平面顶部的假设的层次结构的图示。

在这个应用中，首先估计了输入中的支撑平面。然后在输入数据中分割这些支撑平面上的物体，并在单独的子图中对这些物体进行建模。因此，桌子是一个位于地板上的子图，而一些家具则依次表示为位于桌子顶部的子图。

与单一的、整体的环境地图相比，这样的基于子图构建的分层地图具有许多优点：首先，每个子地图都是独立维护的，并且可以为每个子图调整诸如分辨率之类的地图参数，例如图中的桌子和背景具有较低的分辨率，而桌子上的对象则被以较高的分辨率进行建模，这样的模型可以在保留必要的细节的同时更加紧凑和高效。其次，子图可以独立地进行操作。例如，可以移动表示单个对象的其中一个子图，而保持其他子图不变。第三，子图的层次依赖可以按照层级编码。例如，一个桌子上的所有对象都可以关联到这个桌子，如果这个桌子被移动，那么这些桌子上的对象也会随之移动。

基于ORBSLAM2的八叉树地图构建

对应的代码为ORB-SLAM2_DENSE

编译

依赖与ORBSLAM2的依赖项相同，参见另外一篇文章ORBSLAM2安装与运行。

注意，此代码只支持Eigen 3.1.0版本，安装后路径为/usr/local/include/eigen3

将克隆后的工程放在ROS工作空间中

cd ~/catkin_ws/src/ORB-SLAM2_DENSE/

修改CMakeLists中的PCL路径为你自己的安装路径，并修改Eigen3路径到usr/local/include/eigen3

添加词典txt文件到./Vocabulary, 编译ORB-SLAM2库:

chmod +x build.sh
./build.sh

安装octomap功能包

sudo apt-get install ros-<你的ros版本>-octomap

修改ROS功能包中的CMakeLists中的PCL路径为你自己的安装路径并修改Eigen3路径到usr/local/include/eigen3，之后执行以下代码

cd ~/catkin_ws/
catkin_make

运行

首先下载数据集，链接为数据集链接

之后将数据集放入你的自定义路径下

修改ROS功能包中的两个launch文件中涉及到的路径，主要为Rosbag文件路径,配置文件路径和词典路径, 运行launch文件:

roslaunch orb_slam2_dense tum_pioneer.launch

结果