经典文献阅读之--VoxelMap(体素激光里程计)

0. 简介

作为激光里程计，常用的方法一般是特征点法或者体素法，最近Mars实验室发表了一篇文章《Efficient and Probabilistic Adaptive Voxel Mapping for Accurate Online LiDAR Odometry》，同时还开源了代码在Github上。文中为雷达里程计提出了一种高效的概率自适应体素建图方法。地图是体素的集合，每个体素包含一个平面（或边缘）特征能够实现对环境的概率表示和新的激光雷达帧的精确配准。我们进一步分析了对粗到细体素建图的需要，然后使用了新颖的使用哈希表组织体素地图和八叉树来有效地构建和更新地图。我们将所提出的体素建图应用于一个迭代扩展卡尔曼滤波器，并构造了一个姿态估计的最大后验概率问题。

1. 文章贡献

这篇文章作为一篇比较有影响力的文章，目前已经收到了国内比较多的关注，这里也是跟着Catalina大佬的博客，并结合自己阅读文章的理解进行介绍。

1.1 动机

1. 直接用点云地图表达环境的方法很难考虑由雷达点云测量噪声带来的map的不确定性。地图的不确定性需要对点云中的特征进行显式参数化，在不同的 LiDAR 扫描帧中跟踪这些特征并估计这些特征参数及其不确定性比较困难。因为点云测量中的由于雷达分辨率、扫描类型以及空间点分布不均匀导致的，点密度随着扫描时间变化导致问题进一步变得复杂。此外，衡量map不确定性的时候还要考虑pose误差在系统中的传播。
2. surfel方法通常建模很小的面特征，但场景中经常会有比较大的面特征。surfel的数据关联一般是将map中的surfel和测量帧surfel投影到一个图像上，这种渲染过程非常耗费时间。
3. surfel建模为类似高斯混合模型的时候，一般通过EM算法来做数据关联，无效的surfel会和相邻的合并起来，进而自适应调节surfel尺寸，而自适应voxel的方法会更加鲁棒。
4. ICP方法不够鲁棒，NDT方法不够精确，结合两者，先划分网格，在网格中估计平面参数，最终鲁棒性和精度都会比较好。

1.2 贡献

1. 针对激光雷达点云的稀疏性和不规则性，提出了一种大小自适应、由粗到精的体素构建方法。自适应体素图被组织在八叉树-哈希表的数据结构中，以提高体素构建、更新和查询的效率。

2、本文提出了一种精确的概率体素地图表示法，准确地考虑了点测量和激光雷达位置估计引起的点不确定性，以对地图中平面的不确定性进行建模。

3、实现了所提出的系统在真实世界中的激光测距和测绘的应用，并与最新的方法进行了比较。

2. 详细内容

2.1 概率平面表达

这部分的主要内容是基于激光测量误差和位姿估计误差计算世界坐标系下点的误差，基于平面法向量估计方式估计法向量的误差。

2.1.1 原始点测量噪声

点云噪声来源于2部分，一是range测距不确定性（depth），二是bearing direction方位不确定性（w），公式可从我们的之前工作《Pixel-level Extrinsic Self Calibration of High Resolution LiDAR and Camera in Targetless Environments》推导得到。高斯建模如下

$${\mathbf{\delta }}_{{\mathbf{\omega }}_i}\sim \mathcal{N}\left(0_{2\times 1},{\mathbf{\Sigma }}_{{\mathbf{\omega }}_i}\right),{\delta }_{d_i}\sim \mathcal{N}\left(0,{\Sigma }_{d_i}\right)$$

设${\omega }_i\in {\mathbb{S}}^2$为测量的轴承方向，${\delta }_{{\omega }_i}\sim \mathcal{N}(0_{2\times 1}，{\Sigma }_{{\omega }_i})$为${\omega }_i$切平面上的轴承方向噪声，$d_i$为深度测量值，${\delta }_{d_i}N(0，{\Sigma }_{d_i})$为测距噪声。得到测量点${}^L{p}_i$及其协方差${\Sigma }_{L_{p_i}}$的噪声${\delta }_{L_{p_i}}$

$$\begin{gathered} {\mathbf{\delta }}_{L_{{\mathbf{p}}_i}}=\underset{{\mathbf{A}}_i}{\underbrace{\left[\begin{array}{ll}{\mathbf{\omega }}_i& -d_i\lfloor {\mathbf{\omega }}_i\rfloor \wedge \mathbf{N}\left({\mathbf{\omega }}_i\right)\end{array}\right]}}\left[\begin{array}{c}{\delta }_{d_i}\\ {\mathbf{\delta }}_{{\mathbf{\omega }}_i}\end{array}\right]\sim \mathcal{N}\left(0,{\mathbf{\Sigma }}_{L_{{\mathbf{p}}_i}}\right) \\ {\mathbf{\Sigma }}_{L_{{\mathbf{p}}_i}}={\mathbf{A}}_i\left[\begin{array}{cc}{\Sigma }_{d_i}& 0_{1\times 2}\\ 0_{2\times 1}& {\mathbf{\Sigma }}_{{\mathbf{\omega }}_i}\end{array}\right]{\mathbf{A}}_i^T \end{gathered}$$

其中$N({\omega }_i)=[N_1{N}_2]\in {\mathbb{R}}^{3\times 2}$是ωi处切平面的标准正交基，$\lfloor {\rfloor }_{\wedge }$为映射叉积的斜对称矩阵。式(1)的详细推导可以在[27]中找到。
world系下点的坐标及其不确定性为下面的公式。${\Sigma }_R$是${}_L^{W}R$在切平面空间的不确定性，${\Sigma }_t$是下式中$t$的不确定性。

$$\begin{gathered} {}^W{\mathbf{p}}_i={}_L^W{\mathbf{R}}^L{\mathbf{p}}_i+{}_L^W\mathbf{t} \\ {{\mathbf{\Sigma }}_{W_{{\mathbf{p}}_i}}={}_L^W\mathbf{R}{\mathbf{\Sigma }}_{L_{{\mathbf{p}}_{i}L}}{}^W{\mathbf{R}}^T+{}_L^W\mathbf{R}{\lfloor {}^L{\mathbf{p}}_i\rfloor }_{\wedge }{\mathbf{\Sigma }}_{\mathbf{R}}{}^L{\mathbf{p}}_i\rfloor }_{\wedge L}^{TW}{\mathbf{R}}^T+{\mathbf{\Sigma }}_{\mathbf{t}} \end{gathered}$$

2.1.2 平面的不确定性

设一个平面特征由一组LiDAR点${}^W{p}_i(i=1，\dots ，N)$，由于测量噪声和位姿估计误差，每个点都有一个不确定性${}^W{\Sigma }_{p_i}$，如(3)所示。表示点的协方差矩阵为$A$:

$$\overline{\mathbf{p}}=\frac{1}{N}\sum _{i=1}^N{}^W{\mathbf{p}}_i;\mathbf{A}=\frac{1}{N}\sum _{i=1}^N\left({}^W{\mathbf{p}}_i-\overline{\mathbf{p}}\right){\left({}^W{\mathbf{p}}_i-\overline{\mathbf{p}}\right)}^T$$

然后，平面可以表示为它的法向量$n$，它是与$A$的最小特征值相关联的特征向量，点$q=\bar{p}$，它位于这个平面上。由于$A$和$p$都依赖于${}^W{p}_i$，我们可以将平面参数$(n,q)$表示为${}^W{p}_i$的函数，如下所示:
$$[\mathbf{n},\mathbf{q}{]}^T=\mathbf{f}\left({}^W{\mathbf{p}}_1,{}^W{\mathbf{p}}_2,\dots ,{}^W{\mathbf{p}}_N\right)$$

那么真实世界中的平面normal和位置点q可以估计为下面公式
$$\begin{array}{rl}{\left[{\mathbf{n}}^{gt},{\mathbf{q}}^{gt}\right]}^T& =\mathbf{f}\left({}^W{\mathbf{p}}_1+{\mathbf{\delta }}_{W_{{\mathbf{p}}_1}},{}^W{\mathbf{p}}_2+{\mathbf{\delta }}_{W_{{\mathbf{p}}_1}},\dots ,{}^W{\mathbf{p}}_N+{\mathbf{\delta }}_{W_{{\mathbf{p}}_N}}\right)\\ & \approx [\mathbf{n},\mathbf{q}]T+\sum _{i=1}N\frac{\partial \mathbf{f}}{\partial {}^W{\mathbf{p}}_i}{\mathbf{\delta }}_{W_{{\mathbf{p}}_i}}\end{array}$$

$$\frac{\partial \mathbf{f}}{{\partial }^W{\mathbf{p}}_i}={\left[\frac{\partial \mathbf{n}}{{\partial }^W{\mathbf{p}}_i},\frac{\partial \mathbf{q}}{\partial {}^W{\mathbf{p}}_i}\right]}^T$$

假设$A$有特征向量矩阵$U$、最小特征值${\lambda }_3$和对应的特征向量$u_3$，参照[28]，我们可以对每个点${}^W{p}_i$取$n$和$q$的导数，如下所示:

$$\begin{array}{rl}& \frac{\partial \mathbf{n}}{\partial W_{{\mathbf{p}}_i}}=\mathbf{U}\left[\begin{array}{l}{\mathbf{F}}_{1,3}^{{\mathbf{p}}_i}\\ {\mathbf{F}}_{2,3}^{{\mathbf{p}}_i}\\ {\mathbf{F}}_{3,3}^{{\mathbf{p}}_i}\end{array}\right],{\mathbf{F}}_{m,3}^{{\mathbf{p}}_i}=\{\begin{array}{cc}\frac{(W_{{\mathbf{p}}_i}-\mathbf{q})T}{N\left({\lambda }_3-{\lambda }_m\right)}\left({\mathbf{u}}_m{\mathbf{n}}^T+{\mathbf{nu}}_m^T\right)& ,m\ne 3\\ 0_{1\times 3}& ,m=3\end{array}\\ & \frac{\partial \mathbf{q}}{\partial W_{{\mathbf{p}}_i}}=\mathrm{diag}\left(\frac{1}{N},\frac{1}{N},\frac{1}{N}\right)\end{array}$$

$${\mathbf{\Sigma }}_{\mathbf{n},\mathbf{q}}=\sum _{i=1}^N\frac{\partial \mathbf{f}}{\partial {}^W{\mathbf{p}}_i}{\mathbf{\Sigma }}_{W_{{\mathbf{p}}_i}}\frac{\partial \mathbf{f}}{\partial {}^W{\mathbf{p}}_i}$$

2.2 由粗到细的体素地图构建方法

地图由哈希表和八叉树构成，哈希表管理最上层的地图结构，八叉树每个节点中存放一个平面，如果一个节点中的点不能被表示为一个特征，拆分这个节点。
后续地图的点被送到对应的节点中，在面特征稳定后删除旧的观测，只保留最新观测，如果新的观测和旧的观测冲突，删去旧估计重写估计位姿。

1. Motivation

激光雷达点通常是按顺序采样的，因此点云的扫描总是从稀疏到密集，特别是在点分布的室外环境中更大的空间。当点云相对稀疏时，常用的基于面元的方法通常只能获得非常少量的平面，限制了高精确的和相对较低的扫描率的激光雷达的使用(例如，10Hz)，这样就可以积累足够数量的点。然而，这将需要在累积的范围内补偿运动。为了解决这个问题，我们提出了一种从粗到细的体素建图方法，该方法可以在点云稀疏时构建一个粗糙的体素地图，并在接收到更多的点时对地图进行细化。

2. Voxel Map Construction
   
3. Map Update

对于在线激光雷达里程计，新的激光雷达点云帧不断的进入并且配准，并估计了它们的姿态。然后使用估计的姿态将新点注册到全局地图中：当新点位于一个未填充的体素中时，它将构造该体素。否则，当将新点添加到现有体素时，应该更新体素中平面的参数和不确定性。

上图展示了不确定度的快速收敛。其中每个点的位置携带一个均值为零和方差0.1的高斯噪声。在不确定性收敛后，我们丢弃所有的历史点，并保留估计的平面参数(n，q)和协方差Σ，q。一旦新的累计点出现，我们只保留最新的10点，并计算出由这10个点组成的新平面法向量。如果新的法向量和先前收敛的法向量继续出现相对较大的差值，我们认为地图的这个区域已经发生了变化，需要重新使用算法1计算。

2.3 点与平面匹配

基于对点和平面的精确不确定性建模，我们可以很容易地实现点对平面的匹配。给定一个在具有姿态先验的世界框架中预测的激光雷达点${}^W{P}_i$，我们首先通过它的哈希键找到它所在的根体素（具有粗糙的地图分辨率）。然后，对所有包含的子体素进行轮询，以此与点匹配。具体来说，让一个子体素包含一个具有法线$n_i$和中心$q_i$的平面，我们计算了点-平面距离：
$$d_i={\mathbf{n}}_i^T\left({}^W{\mathbf{p}}_i-{\mathbf{q}}_i\right)$$

考虑到所有这些不确定性：

$$\begin{array}{rl}& d_i={\left({\mathbf{n}}_i^{gt}⊞{\mathbf{\delta }}_{{\mathbf{n}}_i}\right)}^T\left[\left({}^W{\mathbf{p}}_i^{gt}+{\mathbf{\delta }}_{W_{{\mathbf{p}}_i}}\right)-{\mathbf{q}}_i^{gt}-{\mathbf{\delta }}_{{\mathbf{q}}_i}\right]\\ & \approx \underset{0}{\underbrace{{\mathbf{n}}_i^{gtT}\left({}^W{\mathbf{p}}_i^{gt}-{\mathbf{q}}_i^{gt}\right)}}+\underset{{\mathbf{w}}_i}{\underbrace{{\mathbf{J}}_{{\mathbf{n}}_i}{\mathbf{\delta }}_{{\mathbf{n}}_i}+{\mathbf{J}}_{{\mathbf{q}}_i}{\mathbf{\delta }}_{{\mathbf{q}}_i}+{\mathbf{J}}_{W_{{\mathbf{p}}_i}}{\mathbf{\delta }}_{W_{{\mathbf{p}}_i}}}}\end{array}$$

也就是说，如果该点位于候选平面上，其距离$d_i$应服从(10)中的分布。因此，假设分布的标准差为$\sigma =\sqrt{{}^{\Sigma }{w}_i}$，就可以检验点面距离是否在$3\sigma$内。如果是，则选择为有效匹配。此外，如果一个点基于$3\sigma$准则匹配多个平面，则匹配概率最大的平面。如果没有平面通过$3\sigma$检验，则丢弃该点，以消除由体素量化引起的可能的错误匹配。

2.4 状态估计(基于卡尔曼滤波的定位方法)

我们建立了一个基于类似于FAST-LIO2的迭代扩展卡尔曼滤波器的激光雷达里程计，与C节中匹配的点到点平面距离进行融合，形成最大后验(MAP)估计。具体地说，第i个有效的点到平面匹配得到观测方程：
$$z_i=h_i(x_k)+v_i$$
线性化可以得到

…详情请参照古月居