IMLS-SLAM:scan-to-model matching based on 3D data论文阅读

基于3D数据的扫描到模型间匹配

ICRA 2018
翻译参考 http://www.sohu.com/a/255747843_715754

摘要

Abstract— The Simultaneous Localization And Mapping (SLAM) problem has been well studied in the robotics community, especially using mono, stereo cameras or depth sensors. 3D depth sensors, such as Velodyne LiDAR, have proved in the last 10 years to be very useful to perceive the environment in autonomous driving, but few methods exist that directly use these 3D data for odometry. We present a new low-drift SLAM algorithm based only on 3D LiDAR data. Our method relies on a scan-to-model matching framework. We first have a specific sampling strategy based on the LiDAR scans. We then define our model as the previous localized LiDAR sweeps and use the
Implicit Moving Least Squares (IMLS) surface representation. We show experiments with the Velodyne HDL32 with only 0.40% drift over a 4 km acquisition without any loop closure (i.e., 16 m drift after 4 km). We tested our solution on the KITTI benchmark with a Velodyne HDL64 and ranked among the best methods (against mono, stereo and LiDAR methods) with a global drift of only 0.69%.
在机器人领域，SLAM方面的问题已经进行了深入的研究，包括使用单目，双目以及深度传感器的系统。三维的深度传感器，比如像Velodyne的激光雷达，在过去的十多年已经被证明在自动驾驶环境中感知周围的环境是很有用的，但是很少有方法利用这些方法来做里程计进行计算。文章提出了一种新的基于三维激光雷达数据的SLAM系统。我们的方法依赖于一个单帧激光点云到整体模型的匹配。首先我们需要一个采样机制挑选足够可靠的点来进行匹配。其次使用之前已经定位好的激光点云作为模型，使用隐式移动最小二乘Implicit Moving Least Squares (IMLS)表示表面。利用Velodyne的32线激光雷达采集的数据进行测试且不使用任何闭环检测方法，结果显示4km左右的数据集只有0.4%左右的误差（4km的数据漂移误差为16米）。利用KITTI数据集进行的测试结果显示出，相比于其他单目，双目以及LiDAR的方法，我们的结果是最好的，全局误差仅为0.69%左右。

1.Introduction

车辆定位是自动驾驶领域的一项重要工作。目前研究趋势是用精确地图来寻找解决方案。然而，当这样大地图不可靠（区域不可制图或者发生巨大改变自从上次更新），我们需要SLAM方案。有许多基于各种传感器的方案，像相机（单目或者双目），里程计和深度相机或者这些传感器的融合。
对比相机，激光雷达的优势是每次距离测量的噪声与距离和光照条件无关。但是要处理的数据量和采集的距离图像的稀疏密度仍然是个挑战。在这篇论文，我们展示一个新的scan-to-model框架，使用受以前RGB-D方法启发的地图的隐式曲面表示法来更好的处理获取数据的数据量大和稀疏性问题。结果是低漂移的激光雷达里程计和地图质量的改善。

2.Related work

文献中有数百篇关于SLAM的著作，这里，我们只展示在6自由度激光雷达SLAM3D建图方面最新的进展。大多数激光雷达方法是传统的最近邻迭代扫描匹配的变种。ICP是一种著名的scan-to-scan配准方法。[1]和最近[2]调研了ICP的有效变化，如point-to-plane匹配。
[3]总结了各种基于2D或者3D深度传感器的6自由度激光雷达方法，但是他们的方法只用了一种stop-scan-go的策略。[4]研究了一种连续旋转的2D激光雷达，他们从雷达点云中建立了体素栅格，并且在每个体素中计算形状以仅保留圆柱和平面区域来匹配。使用3D雷达（Velodyne HDL64)，[5]展示了一个SLAM,考虑到velodyne的快速旋转影响，修正沿着轨迹的距离图像的歪斜。使用3D雷达，[6]使用一个基于稀疏体素表示的地图和一个泛化的ICP的6自由度SLAM来寻找轨迹。
最近，激光雷达里程计和建图（LOAM）[7]被认为是最先进的6自由度LIDAR SLAM。他们在雷达扫描中聚焦于边缘和平面特征，并且将它们保存在一个地图中，以便于边缘线和平面表面匹配。
区别于2D或者3D旋转雷达，RGB-D传感器如kinect，能够在高频率下生成稠密距离图像。Kinect Fusion[8]介绍了利用这些传感器的3D建图和定位算法。他们依靠体素地图跟踪Kinect的6自由度位置，该地图存储截断的表面距离。这些方法快而且准确但测量范围有限。
我们的方法仅依赖三维激光雷达传感器，如Velodyne生产的传感器，以及这些传感器的连续旋转效果。我们不使用任何其他传感器的数据如IMU，GPS或者相机。我们的算法可以分成三个部分。首先，我们从3D LiDAR的一次旋转计算局部去偏斜点云。其次，我们从该点云中选择特定样本，以最小化第三部分中与模型云的距离。我们的工作主要有两个方面的贡献，即每次激光扫描的点选择和将模型地图定义为点集曲面。

3.scan egomotion and dynamic object removal(扫描运动和动态物体移除)

我们将扫描S定义为来自LiDAR传感器一次旋转的数据。 在旋转期间，车辆已经移动并且我们需要创建点云，其考虑到位移（其运动，定义为在扫描的采集时间期间车辆的运动）。 为此，我们假设两次连续扫描之间的运动相对相似; 因此，我们使用先前的相对位移计算实际运动。
我们定义在任何时间t，车辆姿势相对于初始位置的变换为τ (t)。我们只寻找车辆位置的离散解：τ (t _k )为当前扫描结束时车辆位置（t _k时扫描k）。对时刻t时的任意雷达测量，车辆姿势计算为在上一帧扫描结束τ (t _k−1 )和当前帧扫描结束τ (t _k )的线性插值。
t_k时刻，我们已知所有τ (t_i )对i<k-1，然后寻找离散位置τ (t_k )。为了从当前扫描测量中建立一个局部去斜点云，我们必须对t_k时刻的车辆位置有一个预估τ̃ (t_k )。我们仅使用之前里程，定义τ̃ (t_k )=τ (t_k−1 ) ∗ τ (t_k−2 )⁻¹ ∗ τ (t_k−1 )。
我们利用τ (t_k−1 )和̃ (t_k )之间位置的线性插值创建点云扫描S_k。如果我们假设LiDAR的角速度和线速度是平滑且连续的，那么这种运动是一个很好的近似。然后，我们对点云扫描和地图进行严格的配准来找到τ (t_k )。
扫描匹配模型地图之前，我们需要从扫描中删除所有动态对象。这是一项非常复杂的任务，需要有关场景的高水平语义信息。我们通过删除小型物体来替代动态物体删除，并从场景中丢弃那些尺寸被认为可能是动态物体的对象。首先，我们使用与[9]中相似的体素检测扫描点云中的地面点。我们移除这些地面点并将其余的点聚在一起(在我们的例子中，聚类距离最近点小于0.5米的点)。我们将小的点的类去除，他们可能是行人，小汽车，公共车或者货车。我们移除那些边界框X_v方向上小于14m，Y_v方向上小于14m，Z_v方向上小于4m的点类。即使删除了所有这些数据，我们也保留了足够的大型基础设施信息，如墙壁、栅栏、立面和树木（高度超过4米的）。最后，我们将地面点添加回扫描点云。

4.scan sampling strategy(扫描采样策略）

邻域选择[10,11,12]
[10]Geometrically stable sampling for the ICP algorithm
[11]Fast and accurate computation of surface normals from range images
[12]Dimensionality Based Scale Selection in 3d LIDAR Point Clouds

一旦我们从扫描中创建了未扭曲点云，我们需要选择采样点来做匹配。传统的ICP策略是随机选择采样点[1]。[10]给出了一个更有趣的策略，利用扫描点云的协方差矩阵找到几何稳定采样。他们表明，如果选择合适的样本，ICP在随机样本不可能收敛的情况下是可能收敛的。但是，对于匹配部分，他们的策略可能比较慢，因为他们可能会选择大量的样本。
我们提出了一个受[10]启发的不同抽样策略。 我们保留车架的轴，而不是使用协方差矩阵中的点云的主轴。我们定义了车辆框架中的激光雷达扫描点云坐标（(X_V,Y_V,Z_V)）。这样，扫描点云的大多数平面区域（如果存在）都与（X_V,Y_V,Z_V)轴对齐。例如地面是Z轴平移的参照,立面是XY轴上变换的参考。
首先，我们需要计算每一点的法线。为了快速做到这一点，我们可以使用传感器的球面范围图像计算近似法线，类似于[11]。对于每个点，我们保持它领域的平面表量a_2D，在[12]中定义为a_2D = (σ₂ − σ₃ )/σ₁，σ_i = λ_i，λ_i是用于正常计算的PCA特征值（更多详细信息，请参阅[12]）。其次，我们计算扫描云S_k中每个点x_i的9个值。

在我们的方法中，没有强制要求在环境中有平面区域，但是与非平面区域相比，这种区域允许我们提高匹配质量；这就是为什么我们在选择样本时有公式里有a_2D。前6个值给出了扫描点x_i对车辆三个未知角度（滚转，俯仰，偏航）的可观察性的贡献（我们发现我们对远离传感器中心的点提供了更重要的贡献）。最后3个值给出了X_i点对位置上的变换的可观测性的贡献（对于远离传感器中心或接近传感器中心的点同样重要）。我们将所有点分别按降序对九个列表进行排序，以便每个列表的第一个点都是相对于未知参数具有更大可观测性的点。在匹配环节，我们从每个列表的表头开始选择一个采样x。我们在模型点云中寻找x的最近点P_c。只有当||x − p_c|| ≤ r，我们才保留采样点x。参数r在扫描和模型点云间剔除离群点。我们一直采样直到从每个列表中采集了s个采样点为止。我们可能在不同的表中有相同的高评分点，因此我们会在整个匹配过程中多次使用它作为采样点。在任何情况下，我们有总共9s个采样点（我们选择参数s保证点的个数小于扫描S_K的尺寸）。
图片2展示一个从每个列表采样s=100个点的示例，每个scan由13683个点云组成。对我们的实验，我们只使用了900个点作为采样点（scan的7%）去匹配。有最少的采样点是很重要的（匹配过程的速度主要取决于采样点的数量），但同时，我们需要足够的采样点，以便对变换τ (t_k )的所有参数都具有良好的可观测性。我们将通过我们采样策略选择的点S_k的子集定义为S ̃_k。

图片2：在一张扫描点云图中我们的采样策略。红色的是选择的用来匹配的采样点。我们可以看到离传感器中心较远的点可以更好地锁定旋转和最平面区域上的点，以便更好地匹配

5.scan-to-model matching with implicit moving least squares(IMLS)surface representation(用隐式移动最小二乘(IMLS)表面表示法做扫描到模型的匹配)

[8,13,14,15,16]

Kinect Fusion[8] 是一种基于Kinect深度传感器的著名SLAM算法。他们利用一个从[13]中得到的隐性表面代表模型去做scan-to-model的匹配。隐式曲面由截断有符号距离函数（TSDF）定义，并在体素地图中进行编码。[8]显示了scan-to-model对比传统scan-to-scan的显著成果。TSDF的问题是，表面由一个体素网格（空，SDF，未知）定义，然后只能在小体积空间中使用。这就是为什么TSDF表示法不能用于大型户外环境中的自动驾驶。在我们的SLAM中，我们使用相同的scan-to-model策略，但是我们选择了不同的表面表示方法。我们采用隐式移动最小二乘（IMLS）表示法，直接在最近n次局部扫描的地图点云上计算。

点集曲面是直接在点云上的曲面的隐式定义。在[14],Levin是第一个定义移动最小二乘（MLS）曲面的人，这是一个投影操作符的稳定点集。它从原始的噪声点云生成一个C^∞光滑的表面。稍后，[15]定义了IMLS曲面：函数i(x)的一组零。函数i(x)也表现为接近曲面的距离函数。

使用[15]的IMLS框架，我们定义函数I^(P~k~) 使用方程式1作为任意在R³空间的点x和定义在点云P_k的隐性表面的近似距离：p_i为点云P_k的点，n_i为p_i的法线。

权重W_i(x)定义为W_i(x)=e^{-||x-pi||2/h2},for x in R³,
因为当点p_i远离x时，函数W_i(x)下降的很快，所以我们仅保存P_k在一个球型B(x,r)（r=3h，点p_i大于r到x，W_i(x)<=0.0002）范围内的点。参数r是邻域搜索的最大距离，被拒绝的异常值被视为扫描和地图之间没有对应关系(如前一节所述)。H是定义IMLS表面的一个参数，在以前的论文[15]中已经做了很好的研究，它取决于点云P_K的采样密度和噪声。

我们希望在点云P_K中定位当前扫描S_k，为实现它，我们想要寻找变换R和t使IMLS距离的平方和最小：

由于指数权重的存在，我们不能用线性最小二乘法来近似非线性最小二乘优化问题，如在点对平面的ICP中。
我们将S_k的每个点投影到由P_k定义的IMLS曲面上，而不是最小化该和。n_j是最近点p_c到x_j的法向，是投影点y_j处曲面法向的一个很好的近似值。

现在我们有点云Y_k，映射点集y_i，我们寻找变换R，t使和最小。类似于点到平面的ICP，现在我们可以对R做小角度假设，得到一个可以有效解决的线性最小二乘优化问题（更多细节见技术报告[16]）。我们计算R和t并且使用该变换移动S_k。然后这个过程再开始：将扫描点集x_j映射到IMLS表面形成Y_k，找到新变换R和t在扫描S_k和点云Y_K中，用变换R和t移动扫描。我们迭代它直到达到最大迭代数。最终的变换τ(t_k)是匹配过程中扫描的第一次和最后一次迭代和估计位置τ(t_k)之间b变换组成。现在，我们可以通过τ(t_k-1)和τ(t_k)之间的车辆位置的线性插值从当前扫描的原始数据计算新的点云。 我们将点云添加到地图点云并删除最旧的点云扫描，以始终在地图中保留n次扫描。

使用IMLS公式，我们需要从点云P_k为扫描S_k的每个查询点X_j计算正态ñ_j。
这在邻域搜索期间的每次迭代中都会完成，但仅对使用相同法线的选定样本进行邻域点搜索（因此在每次迭代中计算9s法线）。
图3是IMLS扫描到模型匹配和经典ICP扫描到点云匹配之间差异的示意图示例。该公式的优点是使扫描收敛于隐式曲面，提高匹配质量。

图3：与IMLS扫描模型框架相比，经典ICP扫描匹配的示意图示例。蓝点是之前n次局部扫描中的噪声点云P_k。红色点是新扫描的点。第一行显示了ICP点对面匹配的第一次和最后一次迭代的示例。在每次迭代中，我们将到最近点的距离最小化。第二行显示了IMLS点对模型距离的第一次和最后一次迭代的示例。黑色虚线是IMLS表面。在每次迭代中，我们将到IMLS曲面的距离之和最小化（为了简化起见，我们从模式中删除了正常公式）。

6.实验

我们的IMLS SLAM已经在C++中实现，只使用FLANN库（用于K-D树的最近邻研究）和特征。我们已经对一个真实的室外数据集进行了测试，该数据集由激光雷达Velodyne HDL32和Velodyne HDL64以10赫兹的频率旋转而成（每次扫描都是在100毫秒内完成的）。该方法在一个4 GHz的CPU内核上运行，使用的RAM少于1次。Velodyne HDL32和HDL64是旋转的3D激光雷达传感器，有32和64束激光。
对于所有实验，我们使用s=100表示每个列表中的采样点数量，h=0.06 m（对于IMLS表面定义），r=0.20 m（邻域搜索的最大距离），20表示匹配迭代次数（为了保持恒定的计时过程而不是使用收敛标准），以及n=100是模型点云中的扫描次数。

Tests on our Velodyne HDL32 dataset

为了测试SLAM，我们在巴黎市中心进行了4公里的采集，在车顶的垂直位置使用了一个Velodyne HDL32传感器（共12951次扫描）。这是一个2公里的环行，我们做了两次，完全回到了同一个地方（相差不到一米）。然后我们测量第一次和最后一次局部扫描之间的距离作为误差度量。图1显示了IMLS SLAM的轨迹和经典ICP扫描到扫描匹配的轨迹（相当于n=1）。我们可以看到两个环与我们的SLAM有很好的叠加。图4显示了由SLAM产生的点云的一小部分。我们可以看到很好的细节，比如篱笆。这意味着每次扫描时我们都能很好地观察到车辆。使用IMLS SLAM的第一次和最后一次扫描之间的距离误差为16米，漂移仅为0.40%。

图1：我们的IMLS SLAM的红色轨道是一辆位于巴黎市的汽车（两圈2公里长）顶部安装垂直的Velodyne HDL32。我们可以看到两个环的良好叠加。在黑色是经典扫描轨迹的扫描匹配。两种方法的起点是同一个绿圈。IMLS SLAM的终点和扫描到扫描匹配的终点位于蓝色圆圈内。

图4：IMLS SLAM产生的点云的一小部分（图1中的红色矩形）。栅栏的可见细节表明，我们对每一次激光扫描都有一个很好的假设。

我们用不同方向的Velodyne HDL32测试了我们的SLAM。Velodyne仍在车顶上，但倾斜角度为60度。采集是在里尔市的一个广场上进行的，经过多次旋转，以测试匹配的稳健性（总共1500次扫描）。图5以红色显示了由我们的冲击和生成的点云计算的车辆轨迹。我们可以看到，尽管在广场上做了许多转弯，但没有重复的物体。点云提供了地图的定性评估。

图5：我们IMLS SLAM的红色和点云轨道在里尔市。我们可以看到地图的质量，尽管经过多次旋转，但没有重复的对象。

Tests on the public dataset KITTI with Velodyne HDL64

我们在公共数据集KITTI上测试了SLAM方法。里程计评估数据集有22个序列，包含立体声和激光雷达数据（86种方法的结果可在线获得）。激光雷达是一个垂直的Velodyne HDL64在车顶上。11个序列具有地面真值（GPS+IMU导航），11个序列没有地面真值用于里程计评估。数据集由多种环境组成（城市、农村道路、公路、植被丰富的道路、低或高交通量等）。更多详情请访问1或[17]关于用于评估的指标。激光雷达扫描是通过一个外部的里程表去偏斜，所以我们没有将我们的egomation算法应用到这个数据集。

在训练数据集中，我们得到0.55%的平移漂移和0.0015度/米的旋转误差。我们可以将结果与[6]进行比较，后者的平移误差约为1.5%，旋转误差约为0.01度/米。表1将我们的培训数据集结果与LOAM[7]进行了比较。我们看到我们超过了先前公布的结果。

在测试数据集中，我们有0.69%的翻译漂移和0.0018度/米的旋转误差（见Kitti网站）。这比发表在[7]的最先进的LOAM结果要好，在那里它们有0.88%的漂移。在Kitti网站上，LOAM改进了他们的结果，这比我们的有0.64%的漂移更好。

我们在KITTI基准上得到的漂移不如我们用Velodyne HDL32得到的结果好。这是由三个事实造成的。首先，我们发现扫描点云的变形是由于内部校准不好（我们使用训练数据对所有0.22度激光束的内部垂直角进行了校准）。其次，我们发现GPS数据（用作地面真值）中存在较大误差，例如，序列8开头的误差超过5米。第三，环境的多样性（植被、公路等）比城市环境的velodybe HDL32测试更多。

我们测量算法的不同部分对Kitti训练数据集的贡献。表二显示了动态物体移除的重要性。表三显示了我们的抽样策略相对于随机抽样和几何稳定抽样的贡献（来自[10]）。表四显示了参数n的重要性。我们通过对地图进行n=100而不是n=10次扫描来不断改进结果。我们还尝试保留超过n=100个扫描，但这不会改变结果，因为最旧的扫描距离当前扫描太远，无法产生影响。我们还测试了改变表V中样本数量的参数。当样本数量太少（s=10）时，我们没有足够的点来具有良好的可观测性，无法将扫描与地图点云相匹配。但是当样本数量太大（s=1000）时，结果会更糟，因为我们在扫描中保留了太多的点（如[10]中所述），保留太多的点会改变约束以找到最终姿势。

Discussion of the processing time

我们的IMLS SLAM实现不是实时的。首先，我们使用扫描的三维点云计算每次扫描的法向，而不是使用球面范围图像中[11]的快速法向计算（快17倍）。这是因为Kitti数据集只提供三维点云，而不提供原始激光雷达数据。每次扫描需要0.2秒来进行法向计算。第二，在每次扫描时，我们从整点云p k计算一个新的k-d树，以在imls公式中找到最近的邻居。这需要时间，取决于上次存储的扫描次数。当n=100时，每次扫描需要1秒。一种解决方案是建立一个特定的K-D树（通过只从最旧的扫描中删除点并从以前的扫描中添加点来保持扫描之间的K-D树）。匹配迭代非常快（由于我们的采样策略查询数量有限），每次扫描需要0.05秒。因此，由于Kitti数据集和我们的实现，我们的slam每次扫描运行1.25秒。我们认为，通过更好的常规计算和特定的K-D树来实时运行，可以对其进行改进。为了进行比较，如[7]中所述，在Kitti数据集上，LOAM每次扫描以1s的速度运行。

7.CONCLUSION

我们提出了一个新的三维激光雷达SLAM基于一个特定的采样策略和新的扫描模型匹配。实验表明，在Velodyne HDL32数据集上的漂移较低，在Kitti基准测试中的结果最好。我们认为我们的方法可以改进为在将来的工作中实时运行。