0. 简介
最近激光SLAM的新工作真的是越来越多了，而大多数当前的激光SLAM系统都是在点云中构建地图，即使在人眼看来是稠密的，但当放大时，点云是稀疏的。稠密地图对于机器人应用至关重要，例如基于地图的导航。由于内存成本低，近年来，网格已成为一种有吸引力的稠密建图模型。《Real-time LiDAR Simultaneous Localization and Meshing》提出了第一个仅使用CPU的实时LiDAR SLAM系统，该系统可以同时构建网格地图并针对网格地图执行定位。采用高斯过程重构的直接网格化策略实现了网格地图的快速构建、配准和更新。具体的开源代码也已经在Github上了。
1. 主要贡献
本文贡献如下：
-
我们提出了一种基于GP重建和顶点连接的网格划分策略，该策略允许快速构建、查询和更新网格地图
-
我们设计了一种点到网格的配准方法。结合约束组合和多线程实现，我们确保了网格地图定位和建图的效率和准确性。
-
我们在网格地图上开发了一个稠密、实时、可扩展的开源LiDAR SLAM系统，并通过实验证明了其优势。
2. 方法概述
本文的动机是在LiDAR SLAM系统中构建、利用和维护一个网格地图。图2展示了一个总体概述。该系统主要由三个组件组成：网格化、配准和网格管理。首先，每个新的LiDAR扫描都使用常速度模型的初始猜测将其转换为世界坐标系{W}下的S^{raw}。以下操作也在{W}中执行。然后，将点分配到体素单元中。GP在每个单元内重建局部表面并获得顶点v_i，这些顶点相互连接形成网格。在配准组件中，设计了一种点对网格的配准方法，将重建的当前扫描与构建的网格地图M对齐。最后，网格地图进行迭代更新。
图2. 我们SLAMesh的一般图示。系统分为三个部分：网格化、配准和网格管理。(a)原始点云被转换到世界坐标系{W}，进行下采样，并分配到体素单元格中；(b)高斯过程模型（浅紫色）对每个单元格的局部表面进行建模，以获得均匀分布的顶点。顶点的颜色表示它们的不确定性（较暖表示较高），随着其与原始点的距离的增加而增加；(c)我们通过直接连接相邻顶点来建立网格（浅蓝色）；(d)点对网格配准将重建的当前扫描（红点）对齐到网格地图。基于顶点位置的快速匹配是可行的。法线从周围的网格平滑。GPed扫描表示经过高斯过程重建后的扫描。最后，我们只需要调整顶点的1-D预测和它们的不确定性，以维护网格地图。
3. 网格化策略
如前所述，构建和更新网格是耗时的。为了解决这个问题，我们采用了一种重建和连接策略，以便后面的流程可以在实时运行。如图2所示，GP从体素内部的嘈杂和稀疏点云中恢复局部表面。然后，顶点是表面的插值结果。三维顶点的两个坐标均匀分布（称为位置），而另一个坐标（称为预测）具有连续的值域。位置作为索引，以实现快速查询，预测的值域连续，以避免离散化导致的精度损失。
在这里描述了GP过程（更详细的GP描述可以在[16][17]中找到）。粗体小写字母表示向量，大写字母表示矩阵。GP的输入是原始点云S^{raw}_k= { p_i = (x_i , y_i , z_i , σ^2_{in}), i = 1, …., n_i}的子集，包含n_i个点，其中k表示当前扫描中的第k个单元格C_k，σ^2_{in}是输入噪声的各向同性方差。输出是一个包含n_j个带有不确定性σ^2_j的顶点L^z_k，其中{v_j =（x_j，y_j，z_j，σ^2_j），j = 1，…，n_j}。上标z表示GP预测坐标z为\tilde{f} = {z_j，j = 1，…，n_j}，当坐标可以是x，y或z中的任何一个时，该上标被省略。我们将输入和输出点集的位置表示为i和j。给定输入观测f，预测\tilde{f} 也遵循高斯分布。\tilde{f} 的期望值是：
预测的不确定性指的是其方差。
k_{jj}、k_{ij}和k_{ii}分别表示位置核函数的不同组合
其中k (i, j)是一个比例值，κ是一个常数（我们算法中κ=1）。这个指数核函数可以表示2D流形中的局部光滑曲面。因此，当建模一个复杂的结构时，一个单元格可以包含更多具有不同高斯过程函数的网格层（如果所有坐标都进行了插值，最多可以有3层）[17]。
在某个阈值σ^2_{match}下，具有不确定性σ^2_j 的顶点是足够准确或有效的。三角网格面是通过连接相邻或对角线顶点在位置的2D空间中建立的。如果所有顶点都有效，则网格面有效。与Delaunay三角化类似，在2D空间中建立边缘，我们的方法可以防止薄片网格面。
4. 点对网格配准(重点)
如前所述，我们同时进行定位和网格化，以便网格化可以从定位中受益。为此，一个直观的想法是将顶点视为点或从面中提取点，然后使用传统的点云配准进行姿态估计。然而，这个想法忽略了网格面的法线信息。Puma [3] 表明点对网格的误差可以提高精度。与Puma中的射线投射数据关联不同，我们的SLAMesh是基于位置建立对应关系的。
图3。数据关联可以在预测坐标（绿色单元格）旁边的单元格之间建立。随着配准趋于收敛，查询长度b可以减小。从(a)到(c)，b从2减小到0
对于重建的当前扫描S^{gp}中的顶点v_p，我们首先查询位于相同或相邻单元格中的子网格层（参见图3），然后找到共享相同位置的顶点v_q（参见图2(d)）。在网格中沿边查找n_q个相邻顶点是快速的。包含v_q的那些面的法线被平均以获得平滑的法线\bar{n}，以防表面不光滑。建立v_p和包含v_q的有效面之间的数据关联:
其中||·||是2-范数。我们将一个单元格中的对应数量表示为n_q，重叠单元格的数量为K。点v’_p和应用变换T∈SE3（包括旋转R和平移t）后的点到网格的残差。
其中上标T表示矩阵转置。最优相对变换T是在优化问题中计算的，其中所有K个重叠层L_k中的点到网格的残差被合并。
我们使用Ceres solver1中的Levenberg-Marquardt算法来解决这个非线性最小二乘问题。分析雅可比矩阵被导出以加速解决过程。
其中符号(·)_×表示向量的对应的反对称矩阵。网格可能包含许多面，因此优化问题可能非常大。我们在优化之前通过平均将每个层内的残差合并为一个，以加快优化过程。
将数据存储在体素单元中会在边界上引起数据关联的不连续性。如果我们只在重叠单元上执行配准，当车辆移动速度很快时，配准的收敛区会太窄。因此，我们允许交叉单元重叠，通过查询每个层的预测轴旁边的单元来实现，如图3所示。当配准趋于收敛时，查询的长度会减小。
5. 网格管理和多线程
网格地图比点云地图和基于网格的地图更难维护。点和网格可以独立于它们的邻域。相反，网格保留了顶点相互连接的拓扑结构。地图更新过程应该维护现有的连接并避免出现细长的三角形。典型的解决方案包括使用Delaunay三角剖分进行重新网格化[21]，维护类似于Voxblox[4]的隐式字段，或像Puma[3]一样重复网格化过程。SLAM需要快速更新网格。在我们的SLAMesh中，顶点的位置是固定的，因此只需要更新1-D预测。给定阈值σ^2_{update}以下的前t个数据，新的预测可以通过迭代最小二乘解决：
单元格存储在哈希映射中，这可以确保插入、删除和查询的线性复杂度。此外，这种结构具有灵活性，因为它可以逐步增长。我们基于单元格的映射与VGICP [30]共享的一个优势是兼容并行处理。由于每个单元格在这些步骤中是独立的，我们可以通过多线程加速GP重建和网格发布过程。
评论（0）
您还未登录，请登录后发表或查看评论