题目：π-LSAM: LiDAR Smoothing and Mapping With Planes
作者：Lipu Zhou, Shengze Wang and Michael Kaess
机构：The Robot Perception Lab, Carnegie Mellon University
来源：ICRA 2021

1. 摘要

本文提出了一种用于室内环境的实时密集平面激光雷达扫描系统π-LSAM。目前被广泛使用的LOAM框架[1]不包括约束调整(Bundle Adjustment，又称BA或光束法平差)模块，这导致它只能生成低保真度的跟踪位姿。本文试图克服已有算法在室内环境中的这些缺点。具体来说，我们使用平面作为地标，并引入平面调整(PA)作为后端来共同优化平面和关键帧的位姿。我们提出了π因子来显著降低PA的计算复杂度。此外，我们还提出了一种基于RANSAC框架的高效的平面循环检测算法。在前端，我们的算法可以实时进行全局配准。为了实现这点，我们保留了局部到全局的点到平面的对应扫描，因此我们只需要一个小的局部kd树来建立激光雷达扫描和全局平面之间的数据关联，而不是像以前的工作那样使用一个大的全局kd树。通过这种局部到全局的数据关联，我们的算法可以直接识别激光雷达扫描中的平面，并产生一个准确的、全局一致的位姿。实验结果表明，该算法显著优于最先进的LOAM变体LeGO-LOAM[2]，并且实现了实时处理。

二、主要工作及贡献

（1）引入了平面调整(PA)，用来联合优化关键帧位姿和平面参数以最小化后端中点到平面的距离。
（2）提出了π因子来显著降低PA的计算复杂度。
（3）提出了一种有效的方法，使用平面对齐两帧激光雷达扫描以进行回环检测。
（4）引入了一个ICP过程来实现实时的全局点云注册。与传统的ICP只考虑两帧点云之间的数据关联不同，我们的ICP过程可以逐帧维持点云间的数据关联。这使得我们可以直接识别点云中的平面，而不必在每次扫描中额外进行耗时的平面检测。此外，本文的ICP过程允许我们从一个小的局部kd树中获得局部-全局数据关联，而不是像之前的工作[1] (LOAM)、[2] (LEGO-LOAM)，[8] (LOAM-LIVOX)那样使用了一个大的全局kd树。
（5）该算法中的地图用平面参数表示，易于存储和更新。相比之下，基于ICP的方法中的地图，如LOAM[1]，通常会使用kd树。因此，更新地图需要重新构建kd树。对于大型场景来说，这样的代价是很大的。

3.算法过程

1、系统流程

图1 π-LASM系统概述

系统共有两个部分：前端和后端。前端通过ICP过程建立局部到全局的点到平面对应，并实时提供全局一致的位姿。后端联合优化平面和关键帧的位姿，并提供信息来纠正前端的位姿漂移。

2、前端

2.1 平面提取

我们在整个激光雷达扫描中只以第一个位姿探测这个平面。我们采用论文[30]中引入的区域生长方法来提取平面，并进行了以下改动。对于一个旋转的激光雷达，其点云中包含一个结构。我们参考论文[31]中的做法，将点云转换成一个范围图像。对于同一条扫描线内的点，我们根据旋转角度对它们进行排序。扫描线i中某一点p的K个最近邻只能来自同一条扫描线i或相邻扫描线i−1和i+1。在区域增长步骤中，我们在第i−1、i和i+1条扫描线中为查询点p分别选择5个最近的点。由于可能存在遮挡，一个平面可能被分解成几块。我们试图合并具有相似参数的平面，且只保留得分超过50分的平面。我们也会为新的关键帧提取新的平面，但这只针对一小部分与当前全局平面不匹配的激光雷达扫描。

2.2 扫描中的运动畸变

激光雷达扫描的测量值（即点云）不是在同一时间采集的，因此，测量值会受到运动失真的影响。这个问题可以通过对扫描中的运动进行建模来解决（如论文[1][8]所示）。

2.3 基于ICP的全局点云注册

我们引入了一种新的ICP过程来建立局部-全局点对点-平面对应，并实时估计相对位姿，提出的ICP方法有效避免了构建和使用大型KD树。如图2所示。

图2 提出的ICP方法与传统ICP方法比较

• 数据关联
  假设有N个全局平面被第k次扫描Lk部分观测到，我们结合这N个平面建立集合Qk。然后我们在Qk上建立一个KD树。基于此我们提出Lk坐标系下的点集Lk+1与N个全局平面之间的数据关联如下：
  
  其中P表示来自Lk的K个最近邻点且属于平面πn的点集。
  
• 状态估计
  基于P与πn的数据关联，我们定义点到平面的残差如下：
  
  我们使用LM算法来最小化如下残差向量：
  
  

2.4 关键帧选择

我们使用以下三个标准来确定新的关键帧是否是我们需要的：
a) Lk+1中超过30%的点未能匹配；
b) 当前帧与最新关键帧之间的距离大于τ(在我们的实验中τ=0.15m)；
c) 当前帧与最新关键帧之间的旋转夹角大于θ（在我们的实验中θ=10°）；
在初始阶段(即轨迹长度小于2m时)，我们会放宽引入新关键帧的标准，即设置τ=0.1m和θ=5°。这是因为激光雷达点云是稀疏的，一帧扫描的平面参数通常质量较低。并不是所有的关键帧都会被保留，其中一些将在后续步骤中被删除（如[32]中所示）。

2.5 新平面的选取

对于一个新的关键帧，我们首先使用第2.1节中引入的算法，从与全局平面不匹配的点中检测平面。我们只保留了得分超过50分的平面。如果一个新的全局平面不能在超过三个关键帧中被跟踪到，它将被删除。

3、后端

3.1 平面优化

我们的目标是共同优化平面参数和关键帧位姿，这类似于视觉SLAM中的BA（即视觉SLAM中对3D点和相机位姿进行共同优化）。由于BA对视觉SLAM中的点特征有特殊的意义（如论文[33]所述），这里我们将其命名为联合最小化问题平面调整(PA)。图3展示了一个PA问题的因子图：

图3 PA问题的因子图

假设我们有N个关键帧和M个平面，Pij是在位姿Ti处观测到的在平面πj上的Nij点的集合，而Pijk是Pij的第k个点。假设pijk为pijk的齐次坐标，则pijk与πj之间的有符号距离的形式为：

PA问题的损失函数为：

由于Nij通常非常大，而即使是小场景下的PA问题也是一个运算量很大的最小二乘问题。我们引入π因子来解决这个问题，如下所述。

3.2 π因子

基于3.1中定义的dijk，我们有：

那么，Nij个点的最小二乘损失函数有如下形式：

关于Ti和πj的π因子定义如下：

如图3所示，π因子可以很容易地纳入因子图中。对于每个Pij，我们只需要计算一次Gij。在优化过程中，Gij是一个常数组成的4×4矩阵。利用π因子，我们将Nij所有约束整合为一个，这大大降低了LM算法的计算成本。利用π因子的定义，我们可以将PA的损失函数E重写为：

3.3 局部平面优化（LPA）

我们采用窗口优化策略[32]来更新最新的w个关键帧位姿和相应的平面以提高效率，该步骤被称为局部平面调整(LPA)。当关键帧位姿的数量大于窗口大小时，我们将从滑动窗口中删除最旧的关键帧。我们将删除满足以下条件的关键帧：
a) 此关键帧不包括一个新平面或其所有新平面都已被删除。
b) 此关键帧与此关键帧之前的关键帧之间的距离小于0.3m。
c) 此关键帧与此关键帧之前之间的旋转角度小于15°。如不满足以上条件，我们保将留这个关键帧用于全局优化，并边缘化它和LPA滑动窗口中其余关键帧没有看到的平面。边缘化的步骤是通过使用Schur complement来实现的。我们采用LM算法来解决这个最小化问题。在这里，我们稀疏化关键帧，以减少全局优化的运行时间。

3.4 全局平面优化（GPA）

我们进行全局平面调整(GPA)来最小化损失函数，以全局优化关键帧位姿和平面参数。具体步骤包括触发式回环检测、平面关联、RANSAC、合并全局平面，最后以GPA优化后的参数更新整个系统。

4. 实验结果

我们首先进行了一项消融研究，以测试不同成分的影响。然后，我们将我们的算法SOTA的LOAM改进算法LeGO-LOAM进行比较。最后，我们分析了算法运行速度。实验环境为i7-3770CPU和16G内存的电脑。

1、数据集

我们使用VLP-16激光雷达收集了三个室内数据集。表1列出了统计数据。如下文中图4所示，这三个数据集都具有挑战性。它们都有多种快速旋转的运动，且都包含一个闭环。同时，它们的起点和终点是相同的。因此，我们可以利用起始点和结束点的位姿之间的差异来定量地评估一个SLAM算法的准确性。

2、消融实验

我们评估了我们的算法的不同配置的影响。滑动窗口的大小被设置为10。我们测试了算法的以下配置：
a) π-LASM-Undis：运动的独立性被删除，我们采用[35]中的方法来解决点到平面的配准问题。
b) π-LASM-GPA：已删除GPA。
c) π-LASM-GPA-LPA：删除GPA和LPA。
表2展示了不同配置下的算法得到的起点和终点之间的位姿误差。很明显，运动的独立性、LPA和GPA都有利于位姿估计。

3、与SOTA算法LEGO-LOAM的对比

表2列出了起点和终点之间的位姿误差。图4展示了由我们的算法和LeGO-LOAM生成的轨迹和关键帧位姿。在我们自己采集的数据集上，LeGO-LOAM无法在后两个数据集上顺利运行。可见，LeGO-LOAM的跟踪位姿和关键帧位姿的质量是不稳定的。这是因为LeGO-LOAM的跟踪位姿是通过记录两次连续扫描计算出来的。但我们的跟踪位姿是由全球注册生成的。另一方面，我们发现π-LASM-GPA-LPA比LeGO-LOAM效果更好。在KITTI数据集上，π-LSAM和LOAM的相对位置误差分别为0.72%和0.78%，我们的算法达到了SOTA。

图4 KITTI数据集00序列的运行结果

此外，如表III所示，我们算法的LPA变体可以达到实时运行。我们也在数据集3上测试了没有π因子的LPA和GPA的运行时间。结果表明，π因子可使LPA和GPA的运行速度分别提高28倍和36倍。