---

前言

原文:论文
代码github

摘要：这篇论文提出了一种高效的概率自适应体素映射方法，用于LiDAR里程计。该地图是一个体素集合，每个体素包含一个平面（或边缘）特征，从而实现环境的概率表示和新LiDAR扫描的精确配准。我们进一步分析了粗到细的体素映射需求，并使用一种新颖的通过哈希表和八叉树组织的体素地图来高效地构建和更新地图。我们将所提议的体素地图应用于迭代扩展卡尔曼滤波器，并构建一个最大后验概率问题以进行位姿估计。对开放的KITTI数据集的实验表明，与其他最先进的方法相比，我们的方法具有较高的准确性和效率。在非结构化环境中进行的户外实验，以及使用非重复扫描的LiDAR进一步验证了我们映射方法对不同环境和LiDAR扫描模式的适应性。

一、介绍

近年来，随着3D LiDAR技术的发展，尤其是低成本高密度固态LiDAR的出现，LiDAR（惯性）测程在自动驾驶汽车、无人机（UAV）和3D移动测绘设备等各种应用中的使用越来越广泛。LiDAR测程的一个基本要求是对环境的合理表示（即地图），该地图能够高效维护并有效注册新点。在现有的LiDAR（惯性）测程中，主流的映射方法，如LOAM、Lego-LOAM、LINS、LIO-SAM和Lili-OM，是基于点云地图，这些地图由原始或选定的边缘或平面点组成。在注册新的扫描时，扫描中的每个点通过迭代最近点（ICP）或其变体（如广义ICP）注册到由地图中几个最近点拟合的小平面上。这一研究方向 culminates 在FAST-LIO2中，该方法开发了一种增量kd-tree结构，以高效组织点云地图。作为LiDAR测量的直接形式，点云地图的实现相对简单。然而，点云地图的一个主要缺点是难以考虑由LiDAR测量噪声引起的地图不确定性。在ICP或其变体中，由最近的地图点拟合的平面通常被视为新的点所在的真实确定性平面，但由于地图点的测量噪声，它实际上应是概率性的。虽然在拟合平面（和随后的点注册）中考虑这些噪声是可能的，但由于最近点通常较少以避免虚假平面，这种方法的可靠性较低且耗时（因为如果在注册过程中任何最近点发生变化，必须重新计算不确定性）。考虑地图不确定性需要对环境中显著特征（如平面）进行显式参数化，跨不同的LiDAR扫描跟踪这些特征，并估计这些特征参数及其不确定性。由于可能存在不同大小的特征，执行这些任务是具有挑战性的。由于LiDAR测量中的粗到细现象，问题变得更加复杂。例如，扫描或地图中的点由于LiDAR分辨率、扫描类型和空间中点的不均匀分布而通常在空间中具有截然不同的密度。点的密度也会随着LiDAR传感器的顺序扫描特性而随时间变化。
为了解决上述挑战，本文提出了一种新颖的在线自适应体素映射方法，该方法构建不同大小的体素以适应环境结构和点密度的变化。一个体素跟踪一个显著特征（本文使用平面）。基于接收到的点，特征参数及其不确定性在体素内反复估计。参数化特征使我们能够准确考虑当前点和地图的两者不确定性。具体而言，本文的贡献包括：

• 一种自适应大小的粗到细体素构建方法，能够适应结构变化的环境，并对LiDAR点云的稀疏性和不规则性具有鲁棒性。自适应体素地图以八叉树哈希数据结构组织，以提高体素构建、更新和查询的效率。

• 真正的概率地图表示，其中每个包含在体素地图中的特征（即平面）准确考虑了来自点测量噪声和姿态估计误差的不确定性。

• 在LiDAR（惯性）测程系统中对所提映射方法的开源实现，并在各种环境（结构化和非结构化）和LiDAR（多旋转LiDAR和非传统固态LiDAR）的实际数据集上进行了全面验证。特别地，我们的方法在KITTI数据集上优于其他先进方法的优越性得到了验证。

二. 相关工作

一方面，我们的体素地图中使用的平面特征与Suma和Suma++中使用的surfels非常相似，它们通过优化点到平面的残差来注册新点，并在地图中显式维护和估计surfels。在Suma和Suma++中，surfels是非常小的平面特征，在杂乱的环境中我们的平面特征也是如此。然而，由于我们体素地图中的自适应大小，在结构化环境中，我们的平面特征可以非常大。这种自适应策略使我们的方法适用于具有不同结构的环境，同时确保了高计算效率。我们平面特征与Suma和Suma++中的surfels之间的另一个区别在于surfels和平面的关联。在这些研究中，地图中的surfels和新的LiDAR点都被投影（即渲染）到虚拟图像平面上，以进行点到平面的关联。这样的surfels渲染过程非常耗时，通常需要GPU加速以适应在线应用。相对而言，我们的平面关联是通过高效的哈希表查询实现的。
surfels渲染中的计算问题可以通过将surfels地图建模为高斯混合模型，并通过期望最大化（Expectation Maximization）进行surfels关联来缓解，如MRSLaserMap中所示。MRSLaserMap的工作也采用了多分辨率的surfels地图，其中有效的surfels从最细分辨率合并到较粗分辨率。这种从细到粗的地图方法源于MRSMap，受LiDAR扫描仪的径向特性启发，通常对LiDAR点云的稀疏性和不规则性非常敏感。相比之下，我们的方法以粗到细的方式构建体素地图，更加鲁棒于LiDAR点云的不规则性，并适应不同的环境。与确定性surfels不同，[20]和[21]中的研究将LiDAR测量噪声纳入surfels，导致类似于我们概率性平面表示的概率点到surfels注册。然而，这些研究只考虑了LiDAR测量噪声，而忽略了由于LiDAR姿态估计错误引起的不确定性。相比之下，我们的平面不确定性考虑了这两种不确定性。注意，姿态估计用于将测量的LiDAR点从局部坐标系转换到估计surfels/平面的世界坐标系，因此，姿态估计误差会影响surfels/平面的不确定性。另一方面，我们的体素地图类似于法线分布变换（NDT），其中地图由许多体素（不同大小）组成。在每个体素中，基于位于该体素中的地图点拟合一个3D高斯分布，并用于通过最大化点来自该分布的可能性来注册新点。LiTAMIN2进一步通过将ICP与NDT结合来提高NDT的准确性。与这些NDT方法相比，我们的方法显式地对体素中的平面进行参数化和估计。这种显式的平面参数化允许直接进行点到平面的注册，这比NDT更为可靠和准确，因为NDT中的3D高斯分布通过惩罚所有三个方向（而不仅仅是平面法线）对新点进行约束。显式的平面参数化还允许将LiDAR点测量和姿态估计中的噪声纳入考虑，从而提供进一步的准确性提升。在数据结构方面，我们提出的八叉哈希数据结构和粗到细的映射方法类似于OGN和HSP。OGN引入了八叉哈希表示法到深度卷积解码器中，实现了计算和内存效率高的体积3D输出生成。HSP在分层表面上使用粗到细的预测，以实现更准确的3D物体重建。虽然OGN和HSP的3D物体输出和重建效果良好，但我们将高效的八叉哈希数据结构和粗到细的平面表示整合到一种新型地图中，并将该地图应用于LiDAR测程问题，从而增强了LiDAR测程的适应性、效率和准确性。一方面，我们的体素地图中使用的平面特征与Suma和Suma++中使用的surfels非常相似，它们通过优化点到平面的残差来注册新点，并在地图中显式维护和估计surfels。在Suma和Suma++中，surfels是非常小的平面特征，在杂乱的环境中我们的平面特征也是如此。然而，由于我们体素地图中的自适应大小，在结构化环境中，我们的平面特征可以非常大。这种自适应策略使我们的方法适用于具有不同结构的环境，同时确保了高计算效率。我们平面特征与Suma和Suma++中的surfels之间的另一个区别在于surfels和平面的关联。在这些研究中，地图中的surfels和新的LiDAR点都被投影（即渲染）到虚拟图像平面上，以进行点到平面的关联。这样的surfels渲染过程非常耗时，通常需要GPU加速以适应在线应用。相对而言，我们的平面关联是通过高效的哈希表查询实现的。surfels渲染中的计算问题可以通过将surfels地图建模为高斯混合模型，并通过期望最大化（Expectation Maximization）进行surfels关联来缓解，如MRSLaserMap中所示。MRSLaserMap的工作也采用了多分辨率的surfels地图，其中有效的surfels从最细分辨率合并到较粗分辨率。这种从细到粗的地图方法源于MRSMap，受LiDAR扫描仪的径向特性启发，通常对LiDAR点云的稀疏性和不规则性非常敏感。相比之下，我们的方法以粗到细的方式构建体素地图，更加鲁棒于LiDAR点云的不规则性，并适应不同的环境。
与确定性surfels不同，[20]和[21]中的研究将LiDAR测量噪声纳入surfels，导致类似于我们概率性平面表示的概率点到surfels注册。然而，这些研究只考虑了LiDAR测量噪声，而忽略了由于LiDAR姿态估计错误引起的不确定性。相比之下，我们的平面不确定性考虑了这两种不确定性。注意，姿态估计用于将测量的LiDAR点从局部坐标系转换到估计surfels/平面的世界坐标系，因此，姿态估计误差会影响surfels/平面的不确定性。
另一方面，我们的体素地图类似于法线分布变换（NDT），其中地图由许多体素（不同大小）组成。在每个体素中，基于位于该体素中的地图点拟合一个3D高斯分布，并用于通过最大化点来自该分布的可能性来注册新点。LiTAMIN2进一步通过将ICP与NDT结合来提高NDT的准确性。与这些NDT方法相比，我们的方法显式地对体素中的平面进行参数化和估计。这种显式的平面参数化允许直接进行点到平面的注册，这比NDT更为可靠和准确，因为NDT中的3D高斯分布通过惩罚所有三个方向（而不仅仅是平面法线）对新点进行约束。显式的平面参数化还允许将LiDAR点测量和姿态估计中的噪声纳入考虑，从而提供进一步的准确性提升。
在数据结构方面，我们提出的八叉哈希数据结构和粗到细的映射方法类似于OGN和HSP。OGN引入了八叉哈希表示法到深度卷积解码器中，实现了计算和内存效率高的体积3D输出生成。HSP在分层表面上使用粗到细的预测，以实现更准确的3D物体重建。虽然OGN和HSP的3D物体输出和重建效果良好，但我们将高效的八叉哈希数据结构和粗到细的平面表示整合到一种新型地图中，并将该地图应用于LiDAR测程问题，从而增强了LiDAR测程的适应性、效率和准确性。

三. 方法论

A. 概率平面表示

我们的体素地图在每个体素中包含一个概率特征。为了不失一般性，我们选择平面特征，因为它们在环境中广泛存在，并在本节中介绍平面特征的不确定性模型。
由于平面特征是从其关联点估计得出的，因此点中的任何噪声都会影响平面估计的不确定性。我们首先在第III-A1节中调查点噪声，然后在第III-A2节中推导噪声如何影响平面不确定性。注意，由于体素地图（因此包含的平面特征）是以世界坐标系表示的，点噪声也应在世界坐标系中进行调查，这导致了两个可能的噪声源：一个是原始点测量噪声，相关于局部LiDAR坐标系，另一个是将局部LiDAR点投影到世界坐标系中的LiDAR姿态估计误差。

1) 点 ${}^W{p}_i$ 的不确定性：

根据[27]中对激光雷达传感器测量噪声的分析，局部激光雷达框架中激光雷达点的不确定性由两部分组成：测距不确定性和方位方向不确定性（见图1(a)）。设 ${\omega }_i\in S^2$ 为测量的方位方向，${\delta }_{{\omega }_i}\sim \mathcal{N}(0_{2\times 1},{\Sigma }_{{\omega }_i})$ 为 ${\omega }_i$ 的切平面上的方位方向噪声，$d_i$ 为深度测量值，${\delta }_{d_i}\sim \mathcal{N}(0,{\Sigma }_{d_i})$ 为测距噪声。那么测量点 ${}^L{p}_i$ 的噪声 ${\delta }_{L_{p_i}}$ 及其协方差 ${\Sigma }_{L_{p_i}}$ 为

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

其中 $N({\omega }_i)=\left[\begin{array}{ll}{N}_1& N_2\end{array}\right]\in {\mathbb{R}}^{3\times 2}$ 是 ${\omega }_i$ 的切平面上的正交基，$\lfloor {\rfloor }_{\wedge }$ 表示斜对称矩阵映射叉积。方程(1)的详细推导可在[27]中找到。

图1：注册点的不确定性模型：(a) LiDAR测量不确定性模型，包括方向和深度测量不确定性；(b) 从（3）计算的点协方差 。

图2：平面法线的不确定性模型。

考虑到激光雷达点 ${}^L{p}_i$ 将通过估计的姿态 ${}_L^W\overline{T}=\left({}_L^{W}R,{}_L^{W}t\right)\in SE(3)$ 进一步投影到世界坐标系中，如图1(a)所示，估计不确定性为 L $\left({\Sigma }_R,{\Sigma }_t\right)$，通过以下刚性变换

$${}^W{p}_i={}_L^W{R}^L{p}_i+{}_L^{W}t(2)$$

因此，激光雷达点 ${}^W{p}_i$ 的不确定性为

$${\Sigma }_{W_{p_i}}={}_L^{W}R{\Sigma }_{L_{p_i}L}{}^W{R}^T+{}_L^{W}R{\lfloor {}^L{p}_i\rfloor }_{\wedge }{\Sigma }_R{\lfloor {}^L{p}_i\rfloor }_{\wedge L}^{TW}{R}^T+{\Sigma }_t(3)$$

其中 ${\Sigma }_R$ 是 ${}_L^{W}R$ 在切空间中的不确定性，${\Sigma }_t$ 是 ${}_L^{W}t$ 的不确定性。考虑到由于激光雷达测量和姿态估计引起的不确定性，不同位置的点云协方差有很大的差异：近距离的点的噪声主要由测距噪声主导，而远距离的点的噪声主要由方位噪声主导。单个激光雷达点的不确定性分析也是平面特征不确定性模型的基础。

2) 平面不确定性建模：

假设一个平面特征由一组激光雷达点 $W_{p_i}(i=1,\dots ,N)$ 组成，每个点由于测量噪声和姿态估计误差而具有不确定性 ${}^W{\Sigma }_{p_i}$（如(3)所示）。将点的协方差矩阵表示为 A：

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

然后，平面可以通过其法向量 n 表示，它是与 A 的最小特征值相关联的特征向量，点 $q=\bar{p}$ 位于此平面上。由于 A 和 $\overline{p}$ 都依赖于 ${}^W{p}_i$，我们可以将平面参数（n,q）表示为 ${}^W{p}_i$ 的函数，如下所示：

$$[n,q{]}^T=f({}^W{p}_1,{}^W{p}_2,...,{}^W{p}_N).(5)$$

基于第 III-A1 节的不确定性分析，每个激光雷达点 ${}^W{p}_i$ 有一个噪声 $\delta w_{p_i}\sim \mathcal{N}\left(0_{3\times 1},\Sigma w_{p_i}\right)$。因此，真实法向量 $n^{gt}$ 和真实位置 $q^{gt}$（即在现实世界中需要估计的真实值）为

$$\begin{array}{rl}{\left[n^{gt},q^{gt}\right]}^T& =f({}^W{p}_1+{\delta }_{w_{p_1}},{}^W{p}_2+{\delta }_{w_{p_1}},...,{}^W{p}_N+{\delta }_{w_{p_N}})\\ & \approx {\left[n,q\right]}^T+\sum _{i=1}^N\frac{\partial f}{{\partial }^W{p}_i}{\delta }_{w_{p_i}}\end{array}(6)$$
$\frac{\partial f}{{\partial }^W{p}_i}={\left[\frac{\partial n}{{\partial }^W{p}_i},\frac{\partial q}{{\partial }^W{p}_i}\right]}^T$。假设 A 有特征向量 ${\partial }^W{p}_i$ 矩阵 $U^{\prime }$，最小特征值 ${\lambda }_3$ 和对应的特征向量 $u_3$，参考[28]，我们可以对每个点 ${}^W{p}_i$ 的 n 和 q 求导如下：

$$\frac{\partial n}{{\partial }^W{p}_i}=U\left[\begin{array}{c}{F}_{1,3}^{p_i}\\ F_{2,3}^{p_i}\\ F_{3,3}^{p_i}\end{array}\right],F_{m,3}^{p_i}=\{\begin{array}{llc}\frac{{(}^W{p}_i-q{)}^T}{N({\lambda }_3-{\lambda }_m)}(u_m{n}^T+n{u}_m^T)& ,m\ne 3,& \\ & 0_{1\times 3}& ,m=3.\end{array}$$

$$\frac{\partial q}{{\partial }^W{p}_i}=diag\left(\frac{1}{N},\frac{1}{N},\frac{1}{N}\right)(7)$$

然后 n 和 q 的协方差矩阵 ${\Sigma }_{n,q}$ 为：

$${\Sigma }_{n,q}=\sum _{i=1}^N\frac{\partial f}{{\partial }^W{p}_i}{\Sigma }_{W_{p_i}}{\frac{\partial f}{{\partial }^W{p}_i}}^T(8)$$

可以看出 n 和 q 不是独立的，因为它们是从同一组噪声点计算得出的。

B. 粗到细高效体素地图构建

在本节中，我们首先解释粗-细体素地图的动机，然后介绍构建和更新体素地图的方法。

1) 动机：

如图 3 所示，激光雷达点通常是顺序采样的，因此点云扫描总是从稀疏到密集累积，特别是在户外环境中，点分布在更大的空间中。当点云相对稀疏时，常见的基于 surfel 的细到粗映射方法通常只能获得非常少量的平面，限制了它们在高分辨率激光雷达和相对较低的扫描率（例如，10Hz）中的应用，以便累积足够数量的点。然而，这将需要补偿累积期间的运动。为了解决这个问题，我们提出了一种粗到细的体素映射方法，可以在点云稀疏时构建一个粗略的体素地图，并在接收到更多点时细化地图。

图4：法向量协方差矩阵的迹随着点数增加而收敛。

2) 体素地图构建：

为了实现从粗到细的体素地图构建，我们为每个哈希表条目构建了一个自适应体素地图，并为每个哈希条目构建了一个八叉树。更具体地说，我们首先将空间（在全局世界框架中）切割成体素，每个体素的大小为粗略地图分辨率。然后，对于定义世界框架的第一个激光雷达扫描，包含的点被分配到体素中。有点的体素被索引到一个哈希表中。然后，对于每个有点的体素，如果所有包含的点都位于一个平面上（点协方差矩阵的最小特征值小于指定的阈值），我们存储平面点并计算平面参数（n,q）如(5)所示，以及它们的不确定性 En,q 如(8)所示；否则，当前体素将分裂成八个八分体，并在每个八分体中重复平面检查和体素切割，直到达到最大层数。请注意，体素有不同的大小，每个体素包含一个从包含的激光雷达原始点拟合的平面特征。

3) 体素地图更新：

对于在线激光雷达里程计，新的激光雷达点云扫描持续到来，它们的姿态如第 III-D 节所述进行估计。然后使用估计的姿态将新点注册到全局地图中。当新点位于一个未被占用的体素中时，它将构建该体素。否则，当新点被添加到一个现有的体素时，体素中的平面参数和不确定性应该被更新。这将导致随着新点的持续接收而不断增长的处理时间。为了解决这个问题，我们发现平面参数的不确定性会迅速收敛，如图 4 所示，每个点的位置携带一个均值为零、方差为 0.1m² 的高斯噪声。可以看到，当点的数量达到 50 时，法向量的不确定性就收敛了。不确定性收敛后，我们丢弃所有历史点，保留估计的平面参数（n,q）和协方差 En,q。一旦有更多的新点到来，我们只保留最新的 10 个点，并计算由这 10 个点组成的新平面法向量。如果新的法向量和之前收敛的法向量继续出现较大的差异，我们假设地图的这个区域已经改变，需要按照第 III-B2 节进行重建。

C. 点到平面配准

本节详细说明如何将新LiDAR扫描中的点与体素地图匹配，以构建姿态估计和后续点云注册的约束。基于点和平面准确的不确定性建模，我们可以轻松实现点到平面的扫描匹配。给定在世界坐标系中具有姿态先验的LiDAR点 ，我们首先通过其哈希键找到它所在的根体素（具有粗略地图分辨率）。然后，所有包含的子体素会被轮询，以寻找与该点的可能匹配。假设一个子体素包含一个具有法向量 $n_i$ 和中心 $q_i$ 的平面，我们计算点到平面的距离：

$$d_i=n_i^T\left({}^W{p}_i-q_i\right)(9)$$

如上所述，法向量 $n_i$、激光雷达点 ${}^W{p}_i$ 和中心 $q_i$ 都具有不确定性。考虑到所有这些不确定性，我们得到：

$$\begin{array}{rl}{d}_i& =(n_i^{gt}⊞{\delta }_{n_i}{)}^T\left[\left({}^W{p}_i^{gt}+{\delta }_{W_{p_i}}\right)-q_i^{gt}-{\delta }_{q_i}\right]\\ & \approx \underset{o}{\underbrace{n_i^{gtT}\left({}^W{p}_i^{gt}-q_i^{gt}\right)}}+\underset{w_i}{\underbrace{J_{n_i}{\delta }_{n_i}+J_{q_i}{\delta }_{q_i}+J_{W_{p_i}}{\delta }_{W_{p_i}}}}\end{array}$$

这意味着

$$d_i\sim \mathcal{N}\left(0,{\Sigma }_{w_i}\right),{\Sigma }_{w_i}=J_{w_i}{\Sigma }_{n_i,q_i,{}^W{p}_i}{J}_{w_i}^T,(10)$$

其中，

$$\begin{array}{rl}& J_{w_i}=\left[J_{n_i},J_{q_i},J_{W_{p_i}}\right]=\left[{\left({}^W{p}_i-q_i\right)}^T,-n_i^T,n_i^T\right]\\ & {\Sigma }_{n_i,q_i,W_{p_i}}=\left[\begin{array}{cc}{\Sigma }_{n_i,q_i}& 0\\ 0& {\Sigma }_{W_{p_i}}\end{array}\right].(11)\end{array}$$

也就是说，如果点位于候选平面上，其距离 $d_i$ 应该服从 (10) 中的分布。因此，让分布的标准差为 $\sigma =\sqrt{{\Sigma }_{w_i}}$，我们可以检查测量的点到平面距离是否落在 $3\sigma$ 之内。如果是，它被选为有效匹配。此外，如果一个点根据 $3\sigma$ 标准匹配了多个平面，将匹配概率最高的平面。如果没有平面通过 $3\sigma$ 测试，该点将被丢弃，以去除由体素量化引起的可能的误匹配。

D. 状态估计

我们基于迭代扩展卡尔曼滤波器构建了一个激光雷达（惯性）里程计系统，类似于 FAST-LIO2[13]。假设我们有一个具有协方差 ${\hat{P}}_k$ 的状态估计先验 ${\hat{x}}_k$。这个先验可以从激光雷达里程计的恒定速度假设（实验 A 和 B）或激光雷达惯性里程计的 IMU 传播（实验 C）中获得。这个先验将与第 III-C 节中的点到平面距离匹配融合，形成一个最大后验概率（MAP）估计。具体来说，第 i 个有效的点到平面匹配导致观测方程

$$z_i=h_i(x_k)+v_i(12)$$

其中 $z_i$ 是方程 (9) 中的点到平面距离残差 $d_i$，$h_i(x_k)$ 是观测函数，$v_i\sim (0,R_i)$ 是观测噪声。将状态 $x_k$（即，传感器姿态 $T_k$）代入 (9) 并在当前状态更新 ${\overline{x}}_k$ 处线性化，我们得到：

$$\begin{array}{rl}& z_i=h_i(x_k)+v_i=n_i^T\left({}^W{p}_i-q_i\right)\\ & ={\left(n_i^{gt}⊞{\delta }_{n_i}\right)}^T\left[\left(T_k^{gt}⊞{\delta }_{T_k}\right)\left({}^L{p}_i^{gt}+{\delta }_{L_{p_i}}\right)-q_i^{gt}-{\delta }_{q_i}\right]\\ & \approx \underset{0}{\underbrace{n_i^{g{t}^T}\left(T_k^{gt}{L}_{p_i^{gt}}-q_i^{gt}\right)}}+\underset{H_i\delta x_k}{\underbrace{J_{T_i}{\delta }_{T_k}}}+\underset{v_i}{\underbrace{J_{n_i}{\delta }_{n_i}+J_{q_i}{\delta }_{q_i}+J_{L_{p_i}}{\delta }_{L_{p_i}}}}\end{array}$$
这意味着(13)
$$R_i=J_{v_i}{\Sigma }_{n_i,q_i,{}^L{p}_i}{J}_{v_i}^T(13)$$
其中
$$J_v=\left[J_{n_i},J_{q_i},J_{L_{p_i}}\right]={\left(T_k^{\prime }{p}_i-q_i\right)}^T,-n^T,n^T{R}_k]$$
$${\Sigma }_{n_i,q_i,L_{p_i}}=\left[\begin{array}{cc}{\Sigma }_{n_i,q_i}& 0\\ 0& {\Sigma }_{L_{p_i}}\end{array}\right]$$

最后，将状态先验与所有有效的测量结合，我们可以得到最大后验概率（MAP）估计：
$$\underset{x_k}{\min }\left({\Vert x_k\boxminus {\hat{x}}_k\Vert }_{{\hat{P}}_k}^2+\sum _{i=1}^m{\Vert d_i-H_i\cdot \left(x_k\boxminus {\overline{x}}_k\right)\Vert }_{R_i}^2\right)(14)$$
其中第一部分是状态先验，第二部分是测量观测。

四. 实验

在本节中，为了展示我们的映射方法在不同环境和激光雷达类型下的高精度、高效率和适应性，我们在三种不同的环境中对三种典型的激光雷达进行了实验（城市环境、室内环境和非结构化室外环境）。使用的激光雷达的详细信息如表 I 所示。在每次实验中，我们将我们的方法与最先进的对比方法进行比较。所有实验都在配备有 Intel i7-10700 @ 2.9 GHz、16 GB RAM 和 Nvidia GeForce GTX 730 与 2 GB RAM（如果使用 GPU）的台式计算机上进行。

传感器	类型	频率	扫描模式	FoV
HDL-64E S2	机械式	10Hz	重复式	360°×32°
Realsense L515	固态	30Hz	重复式	70°×55°
Livox Avia	固态	10Hz	非重复式	70°× 77°

表 I：不同类型的激光雷达

五. 结果

图5：序列00到10的轨迹。蓝色轨迹是我们方法获得的相机路径，红色轨迹是地面真实值。黑点表示起始点。


图6：在KITTI训练序列00中，我们自适应体素地图中的平面特征示意图。平面特征通过其不确定性（平面协方差矩阵的迹，见公式(8)）着色。平面显示为一个圆盘，半径由公式(4)中定义的点协方差矩阵 𝐴 的最大特征值决定。