【论文阅读】SLAMesh

摘要

目前大多数激光雷达 SLAM 系统在点云中构建地图，这些点云在放大后稀疏，但在人眼看来却很密集。密集地图对于机器人应用程序是必不可少的，比如基于地图的导航。由于网格的低内存成本，近年来网格成为一种很有吸引力的稠密建图模型。 然而，现有的方法通常使用离线生成网格图。这两步流程不允许这些方法在线使用构建的网格地图，也不允许本地化和网格化相互受益。为了解决这一问题，我们提出了第一个 CPU 唯一的实时 LiDAR SLAM 系统，该系统可以同时建立网格图并对网格图进行定位。基于 高斯过程 重构的网格划分策略实现了网格建图的快速建立、配准和更新。实验结果表明，我们的 SLAM 系统可以在 40Hz 左右的频率下运行。该方法的定位精度和网格划分精度也优于目前最先进的方法，包括 TSDF 建图和泊松重建。

1. 引言

通常 LiDAR SLAM 会生成点云图，使用这种地图类型，可以直接对新的距离数据进行匹配和融合。但是点云地图需要额外的数据结构，如KD-tree，以加快无序点的查询。内存成本也随着规模或密度的增加而显著增加。此外，点云图在人眼看来虽然稠密，但放大后仍然稀疏。由于稀疏性，在用于导航之前通常需要进行后期处理操作，如体素化，以生成密集的体素图。
在这项工作中，我们认为稠密地图是一种表征，在表面或体积方面模拟三维物体。在这个定义下，存在几种类型的密集地图。例如，占用栅格地图 和 TSDF地图 将三维空间划分为体素，适用于机器人导航。然而，这些方法离散化引入了可扩展性问题，并将定位精度限制在网格大小。NDT 和 surfel 也可以看作是稠密建图。它们使用参数化的表示来描述局部结构。参数化确保了内存效率，但限制了描述细节的能力，除非使用高分辨率。此外，在 NDT 和 surfel 地图元素周围也存在空白。
Mesh 是由面、边和顶点组成的三维实体曲面。与 grid map(栅格地图) 不同，mesh map(网格地图) 缓解了离散化问题，并可以构建光滑的表面。与 surfel 和 NDT 地图相比，mesh map 还可以描述多种结构。此外，mesh map 具有内存高效、可伸缩、保留拓扑信息的特点。
然而，实时三维激光雷达网格建图目前仍难以实现，特别是在大规模环境下。主要的困难是网格构建和更新通常是耗时的。主流的解决方案是首先集中于点云建图，然后使用点云地图分别执行网格划分。这种两步法的解决方案不能使定位和网格化互相受益。

图 1 左：在 Mai City 数据集上由 SLAMesh、Puma 和 Voxblox 生成的网格地图。每个地图的放大视图突出了网格边缘的连接。Puma 的网格图是防水的，但有相对复杂的多层。由 Voxblox 和我们的 SLAMesh 生成的网格地图是高度有序的，顶点是均匀分布的。右：由我们的 SLAMesh 在 KITTI 序列 00、07 和 09 上构建的网格地图展示了 SLAMesh 在大规模环境中的准确性和可伸缩性。

为了解决这一问题，本工作设计了一个名为 SLAMesh 的同时定位和网格划分系统。系统只需要一个 CPU 就可以实时运行。在 SLAMesh 中，我们使顶点均匀分布，使网格构建、匹配和更新更加有效。图 1 显示了这种由高斯过程 (GP) 重建构建的网格图。之前的工作 GP-SLAM 已经展示了在SLAM中使用 GP 的好处，特别是在距离数据稀疏的情况下。然而，它们生成的点云地图没有导出表面模型。此外，当使用具有密集点云的传感器 (如64束激光雷达) 时，它们无法实现实时性能。本工作利用 GP 构建网格地图，是在 GP- SLAM+ 基础上的升级版本。本工作的主要贡献如下:

• 我们提出了一种 基于 GP 重建和顶点连接的网格划分策略，该策略允许快速构建、查询和更新网格地图。
• 我们设计了一种 点到网格的配准方法。结合约束组合和多线程实现，保证了网格地图定位和建图的效率和精度。
• 我们开发了一种 基于网格地图的密集、实时、可扩展的开源 LiDAR SLAM 系统。

2. 相关工作

室内密集建图比室外情况研究得更多。目前广泛采用的网格建图思想是将深度数据融合到 TSDF 地图中，然后通过像 Voxblox 和 Kinectfusion 这样的 Marching cubes 算法按需提取隐式网格。相反，有些方法显式地生成网格地图。Ryde等人将点云体素化，并在每个体素内部回归一个平面以形成多边形。Piazza等人利用稀疏特征点上的 Delaunay 三角剖分来构建和更新网格。Schreiberhuber等人将相机图像中的深度点连接起来，然后将局部网格patch拼接在一起。我们的工作明确地基于 GP 生成网格地图。我们的 SLAMesh 处理无序的点云，并在世界框架中构建网格地图，而不是在不同的图像帧中合并网格补丁。
最近，可靠的 3D激光雷达 使户外建图成为可能。室外环境比室内环境更具挑战性，因为场景要大几个数量级，车辆移动速度快，偏远地区点云稀疏。Suma 和 Suma++ 提供了精确的里程测量，但surfel 地图相对比较杂乱。LiTAMIN2 显示了利用椭球图进行无损检测配准的效率优势。根据 TSDF 地图，Kuhner 等人修改了激光雷达的射线跟踪模型，并包含了离线的细化过程。Roldao 等人结合显式和隐式网格方法获得更好的精度和网格密度。这些方法主要集中在建图功能上，不能提供实时里程计。Puma 和我们的 SLAMesh 都使用网格建图。但是Puma使用光线投射进行配准，需要通过泊松重建[26]来重复重建网格图，而不是更新。Lin 等人也研究了 LiDAR SLAM 中的在线网格划分。他们使用一个独立的 SLAM 系统和直接连接网格策略在点云地图上。我们的方法重构无序点，将网格建图集成到整个系统中。
上述基于 TSDF 的方法中的射线跟踪模型忽略了局部表面是连续的这一事实。相反地，GP 建立了一个伴随着不确定性的连续模型。这也吸引人们使用 GP 来恢复自然连续的占用场和TSDF场。不同的是，我们使用 GP 来预测曲面的欧几里德坐标。此外，我们的系统也可以应用于大型环境。

3. 所提出的方法

A. 方法概述

这项工作的动机是在一个激光雷达 SLAM 系统中建立和维护一个网格地图。图 2 说明了主要流程。该系统主要由 网格划分、配准 和 网格管理 三个部分组成。首先，每一次新的 LiDAR 扫描通过匀速模型的初始值转换到世界坐标系下 {W} 记为$s^{raw}$。接下来的操作也在 {W} 中执行。然后，将点分配到体素 cell 中。GP 对每个 cell 内部的局部曲面进行重构，得到顶点$v_i$，并将顶点$v_i$连接起来形成一个网格。在配准中，设计了点到网格的配准，将重构的当前帧与构建的网格地图$M$对齐，并对网格地图进行迭代更新。

图 2 SLAMesh的总图。该系统由 网格划分、配准 和 网格管理 三部分组成。(a) 将原始点云转换到世界坐标系{W}下，下采样，并赋值到体素 cell 中；(b) 使用 GP 对每个 cell 的局部表面 (浅紫色) 进行建模，得到均匀分布的顶点。顶点的颜色表明了它们的不确定性(越热意味着越高)，这种不确定性随着它与原始点距离的增加而增加；© 我们通过直接连接相邻的顶点来建立网格(浅蓝色)；(d) 点到网格配准将重构的当前帧(红点)对准网格地图。基于顶点位置可以实现快速匹配。法线是从周围的网格平滑得到的。GPed扫描 是指GP重建后的扫描。最后，我们只需要调整顶点的一维预测和它们的不确定性来维持网格图。

B. 网格化策略

如上所述，构建和更新网格是耗时的。针对这一问题，我们采用了重构和连接的策略使整个系统能够实时运行。如图 2，使用 GP 从体素内部的噪声和稀疏的点云恢复局部表面。然后，顶点是表面的插值结果。3-D顶点的两个坐标均匀定位（称为位置），并且另一个坐标（称为预测）具有连续值域。这些位置用作索引，以实现在恒定时间内的快速查询。预测的值域是连续的，以避免离散化引起的精度损失。
这里描述了 GP 过程。粗体小写字母表示向量，大写字母表示矩阵。输入到 GP 中的是原始点云包含$n_i$个点的子集 ${\mathcal{S}}_k^{raw}=\left\{{\mathbf{p}}_i=(x_i,y_i,z_i,{\sigma }_{in}^2),i=1,....,n_i\right\}$，其中$k$表示为当前帧的第$k$个子集，${\sigma }_{in}^2$做为输入噪声表示为各向同性的方差（对于多维随机变量，方差通常是一个协方差矩阵，其中包含了各个维度之间的协方差信息。各向同性方差意味着协方差矩阵是一个对角矩阵，其中对角线上的元素都相等，表示各个维度上的随机变量具有相同的方差）。输出的是包含$n_j$个顶点的层 L k z = { v j = ( x j , y j , z j , σ j 2 ) , j = 1 , . . . . , n j } \mathcal{L}_{k}^{z} = \{\boldsymbol{v}_{j}=(x_{j},y_{j},z_{j},\sigma_{j}^{2}),j=1,....,n_{j}\} Lkz​={vj​=(xj​,yj​,zj​,σj2​),j=1,....,nj​}。$z$表示 GP 将$z$坐标预测为 f ~ = { z j , j = 1 , . . . . , n j } , \widetilde{\boldsymbol{f}}=\{z_{j},j=1,....,n_{j}\}, f ​={zj​,j=1,....,nj​}, 并且当坐标可以是$x$、$y$或$z$中的任何一个时,可以省略它。我们将输入和输出点集的位置表示为$i$和$j$。给定输入观测$f$，预测$\tilde{f}$也遵循高斯分布。$\tilde{f}$的期望值为：$$\begin{array}{c}\begin{array}{r}\tilde{\mathbf{f}}={\mathbf{k}}_{\mathbf{i}\mathbf{j}}^T{\left({\sigma }_{in}^2\mathbf{I}+{\mathbf{K}}_{\mathbf{i}\mathbf{i}}\right)}^{-1}\mathbf{f}.\end{array}\end{array}$$
预测的不确定性是其方差：$$\begin{array}{c}\begin{array}{r}{{\sigma }_j}^2=k_{jj}-k_{ij}^T{\left({\sigma }_{in}^2\mathbf{I}+K_{i\mathbf{i}}\right)}^{-1}{k}_{i\mathbf{j}},\end{array}\end{array}$$
其中$k_{ij}$、$k_{ij}$和$K_{ii}$表示位置的核函数的不同组合。比如说，
$$\begin{array}{c}\begin{array}{r}k\left(i,j\right)=\exp \left(-\kappa \left|i-j\right|\right),\end{array}\end{array}$$
其中$k(i,j)$是尺度值，并且κ是常数（在我们的算法中κ = 1）。该指数核函数可以表示2-D流形中的局部光滑表面。因此，当对复杂结构建模时，一个单元可以包含具有不同 GP 函数的更多网格层（如果所有坐标都被插值，则最多3个）。
在某个阈值${\sigma }_{match}^2$下具有不确定性${\sigma }_j^2$的顶点是足够准确或有效的。三角形网格面是通过连接二维位置空间中的相邻顶点或对角顶点来构建的。如果所有顶点都有效，则网格面有效。类似于 Delaunay三角剖分（Delaunay 三角剖分是计算机图形学和计算几何中的一个重要概念，用于将给定的点集合进行三角化，生成一组不重叠的三角形，以便进行各种计算和渲染），它建立了一个2-D空间中的边，我们的方法可以防止出现薄片网格面（在计算机图形学和三维建模中，“薄片网格面” 指的是那些非常狭窄且呈现细长形状的三角形或四边形网格面，它们通常被认为是不良网格）。

C. 点到网格配准

图 3 数据关联可以跨预测坐标旁边的单元（绿色单元）建立。查询长度 b 可以随着配准趋于收敛而减小。从（a）到（c），b 从2减小到0。

如上所述，我们同时执行定位和网格化，以便网格化可以有利于定位。为此，一个直观的想法是 将顶点视为点 或 从面部提取点，然后采用传统的点云配准进行姿态估计。然而，这种想法忽略了网格面的法线信息。Puma 指出，点到网格误差可以提高精度。与Puma 中的光线投射数据关联不同，我们的 SLAMesh 基于位置建立对应关系。对于重构的当前扫描$S^{gp}$中的顶点$v_p$，我们首先查询位于相同或相邻单元中的子网格层（参见图 3），然后找到共享相同位置的顶点$v_q$（参见图 2(d)）。在$v_p$和包含$v_q$的有效面之间建立数据关联。在网格中沿着边可以快速找到$n_q$个相邻顶点。在表面凹凸不平的情况下，对包含$v_q$的那些面的法线进行平均以获得平滑的法线$n$：
$$\begin{array}{c}\begin{array}{r}\overline{{\mathbf{n}}_q}=\frac{{\sum }_{q=1}^{n_q-2}({\mathbf{v}}_q-{\mathbf{v}}_{q+1})\times ({\mathbf{v}}_q-{\mathbf{v}}_{q+2})}{{\sum }_{q=1}^{n_q-2}\Vert ({\mathbf{v}}_q-{\mathbf{v}}_{q+1})\times ({\mathbf{v}}_q-{\mathbf{v}}_{q+2})\Vert },\end{array}\end{array}$$
其中$||\cdot ||$是2-范数。我们将一个单元中的对应数目表示为$n_q$，并且将重叠单元的数目表示为K。在将包含$R$和$t$的变换矩阵$T\in SE3$ 应用在点$v_p$上之后得到点$v_p^{\prime }$和点到网格的残差：
$$\begin{array}{c}\begin{array}{r}[\begin{array}{c}{v}_p^{\prime }\\ 1\end{array}]=\mathbf{T}\left[\begin{array}{c}{v}_p\\ 1\end{array}\right]=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathbf{v}}_p\\ 1\end{array}\right],\end{array}\end{array}$$
$$\begin{array}{c}\begin{array}{r}{e}_p=\overline{{\mathbf{n}}_q^T}({\mathbf{v}}_p^{\prime }-{\mathbf{v}}_q),\end{array}\end{array}$$
在优化问题中计算最优相对变换$T$，其中所有$K$个重叠层$L_k$中的点到网格残差和的最小化问题可以表示为：
$$\begin{array}{c}\begin{array}{r}\mathbf{T}=argmin\sum _{k=1}^K\sum _{{\mathbf{v}}_p\in {\mathcal{L}}_k,p=1}^{n_p}{e}_p.\end{array}\end{array}$$
我们使用 Ceres solver 中的 Levenberg-Marquardt 算法来解决这个非线性最小二乘问题。雅可比矩阵求解过程为：
$$\begin{array}{c}\begin{array}{r}\mathbf{J}=\frac{\partial e_p}{\partial \mathbf{T}}=\frac{\partial e_p}{\partial {\mathbf{v}}_p^{\prime }}\frac{\partial {\mathbf{v}}_p^{\prime }}{\partial \mathbf{T}},\end{array}\end{array}$$
$$\begin{array}{c}\begin{array}{r}\frac{\partial e_p}{\partial {\mathbf{v}}_p^{\prime }}=\overline{{\mathbf{n}}_p^T},\frac{\partial {\mathbf{v}}_p^{\prime }}{\partial \mathbf{t}}=\mathbf{I},\frac{\partial {\mathbf{v}}_p^{\prime }}{\partial \mathbf{R}}=-{\left(\mathbf{R}{\mathbf{v}}_p+\mathbf{t}\right)}_{\times },\end{array}\end{array}$$
其中符号$(\cdot {)}_{\times }$表示向量对应的反对称矩阵。网格可能包含许多面，因此优化问题可能很大。在优化之前，我们将每个层内的残差合并为一，以加快优化过程。（？？？？？既然一个网格内有很多面，那么如何准确找到对应面呢？？？？？）
将数据存储在体素单元中会导致边界上数据关联的不连续性。如果只对重叠单元进行配准，当车辆快速移动时，配准的收敛域会太窄。因此，我们允许跨单元重叠，通过查询每个层的预测轴旁边的单元，如图 3 所示。当配准趋于收敛时，查询的步长减小。

D. 网格管理和多线程

网格（mesh）地图比点云地图和基于栅格（grid）的地图更难维护。点和栅格可以独立于其邻域。相反，网格是保持顶点彼此连接的拓扑结构。地图更新过程应保持现有连接并避免狭长三角形。典型的解决方案包括使用 Delaunay三角剖分 重新网格化，保持类似于Voxblox 的隐式场，或重复Puma 的网格化过程。SLAM 要求网格的快速更新。在我们的 SLAMesh 中，顶点的位置是固定的，因此只需要更新1-D预测。给定低于阈值${\sigma }_{update}^2$的先前$t$个数据，可以通过迭代最小二乘法来求解新的预测：
$$\begin{array}{c}\begin{array}{r}{\tilde{f}}_{t_k}=\sum _{t=1}^{t_k}({\tilde{f}}_t{\sigma }_t^2)/\sum _{t=1}^{t_k}({\sigma }_t^2),\text{if}{\sigma }_t^2<{\sigma }_{update}^2.\end{array}\end{array}$$
cell 存储在哈希地图中，这确保了插入、删除和查询的线性复杂度。此外，这种结构是灵活的，因为它可以增量增长。我们与 VGICP 共享的基于单元格的映射的优点是并行处理的兼容性。由于每个单元在这些步骤中是独立的，因此我们可以使用多线程加速 GP 重建和网格发布过程。

4. 实验结果