背景
自己虽然用过很多次八叉树地图，但是对于其概率的计算方式一直有些含糊，因此决定重新读一遍论文，来了解其原理。
论文
Hornung, Armin, et al. “OctoMap: An efficient probabilistic 3D mapping framework based on octrees.” Autonomous robots 34.3 (2013): 189-206.
正文
3 OctoMap mapping framework
3.1 Octree
- Octree 是一个有层次的数据结构。
- Octree 中的每个Node表示空间中的一个立方体，也称为一个voxel。这个立方体会被递归地分为八个块，直到每个块达到最小的尺寸。
- 当node的状态只有占据与free两种状态时，一个比较好的剪枝方法为，当一个node的所有children都是同一个状态，则这些children可以被裁剪掉
3.2 Probabilistic sensor fusion
| 给定$1:t$时刻的观测值$Z_{1:t}$，则叶子节点n的占据(后验)概率$P(n | z_{1:t})$为 |
这个式子涉及三个概率：
-
$P(n z_t)$为根据当前测量得到的栅格n的占据概率 -
$P(n z_{1:t-1})$为历史的占据概率。 - $P(n)$为其先验概率
正常情况下，设置先验概率为0.5，然后利用log-odds表示，则上式可以表示成 \(\mathrm{L}\left(n \mid z_{1: t}\right)=\mathrm{L}\left(n \mid z_{1: t-1}\right)+\mathrm{L}\left(n \mid z_{t}\right)\) 其中 \(\mathrm{L}(n)=\log \left[\frac{P(n)}{1-P(n)}\right]\)
由于log-odds是可以转换成概率的，因此每个node不再直接存储概率，而是直接存储log-odds value。
注意到log-odds值是可以无限叠加的，所以当一个node被观测到k次占据后，至少也得要k次非占据的观测才能改变该node的占据状态。这个性质在环境物体是静态的时候是可以的，但是当环境是高度动态的时候，会导致地图的更新过于缓慢。因此，通常给log-odds函数人为设置取值上下限（称为Clamp），如下式所示。
\[\begin{array}{l} \mathrm{L}\left(n \mid z_{1: t}\right) \\ \quad=\max \left(\min \left(\mathrm{L}\left(n \mid z_{1: t-1}\right)+\mathrm{L}\left(n \mid z_{t}\right), l_{\max }\right), l_{\min }\right) \end{array}\]3.3 Multi-resolution queries
当点云的测量值给出的时候，概率值是只在八叉树的叶子节点更新的。而父节点的占据概率更新可以有两种方式
- 平均占据概率 \(\bar{l}(n)=\frac{1}{8} \sum_{i=1}^{8} \mathrm{L}\left(n_{i}\right)\)
- 最大占据概率 \(\hat{l}(n)=\max _{i} \mathrm{L}\left(n_{i}\right)\)
为了在机器人的导航中获得一条没有碰撞的路线，通常采用策略2。
3.4 Octree map compresson
在3.1节提到的剪枝策略是针对node状态只有0或者1而言的。当node中存储的是log-odd值时，就不能直接这么操作了。
该文提出的方法是，将log-odd值达到clamp的上界或者下界的node视为稳定的node。当一个node的四个children均为稳定，且状态相同，即可进行裁剪。并且，当有新的测量使得该node的状态发生变化的时候，该node的childern又会被重新创建。
3.5 Extensions
Octree的节点可以被拓展以用来存储其他信息，例如颜色等。
本文还设计了一个软件，用来分割八叉树模型。
4 Implementation details
4.1 Memory-efficient node implementation
为了节约八叉树的存储空间，文章采取了以下措施：
- 首先，每个node的位置信息和size信息是在遍历的时候可以得到的，因此这些信息不会被存储在node中。
- 其次，每个node里只保存一个指向children指针数组 的指针。当这个node为leaf时，该指针为空；否则，指向一个由八个指针组成的数组。
- node的占据信息用一个float来存储。
- 由于虚继承会导致虚函数表指针的创建，因此，只在octree类使用虚继承，而在octree node类中不使用虚继承。
4.2 Octree types
类之间的继承关系如下所示。
在目前的实现中，树的最大深度被设置为16，从而导致当分辨率为1cm时，八叉树只能表示长宽高为655.36m的区域。但是可以修改树的最大深度为32，使得区域大大扩大。
4.3 Map file generation
这里讲的是如何紧致地存储地图哈。暂时用不着。
4.4 our OctoMap implementation
4.4.1 Integrating sensor measurements
测量量使用$insertRay()$方法来更新ocTree。这个方法使得从机器人到测量值的终点（除终点以外）的voxel为free，而终点所在的vocel为occupied.
$insertScan()$方法则是一个优化后的批量方法，比单独trace每一条ray速度块很多。
$updateNode()$方法是根据某个点测量，单独更新octree的某个Node。
4.4.2 Accessing data
- 通过坐标可以查找到某个node
- Octomap实现了迭代器，可以查找任何node,任何叶子节点以及任何bounding box内的叶子节点。
- 还提供了$castRay()$方法来供用户查询ray intersection。(即查询一束光线首次hit到的占据的volume)
Evaluation
5.1 Sensor model for laser range data
log-odds的值可以设置为：
- $l_{occ} = 0.85$
- $l_{free} = -0.4$ 以对应occupied与free的概率分别为0.7与0.4
clamping threshold为$l_{min}=-2$，$l_{max}=3.5$。
当一个激光雷达在摇摆的时候，某些本该是占据的栅格会被认为是free的，如下图所示。
为了解决这个问题，整个sweep的点云会被叠加到一起，再塞入到octomap中去。