论文Review LSLAM BALM | 经典激光SLAM方案！港大MARS出品！RAL2021

基本信息

题目：BALM: Bundle Adjustment for Lidar Mapping

来源：RAL2021

学校：HKU-MARS

摘要：激光SLAM、激光BA优化

关键帧滑动窗口上的局部集束调整( Bundle Adjustment，BA )已被广泛应用于视觉SLAM中，并被证明在降低漂移方面非常有效。但在激光雷达SLAM中，由于特征点稀疏( e.g. ,边和面)使得无法进行精确的点匹配，BA方法几乎不被使用。我们将激光雷达BA建模为最小化特征点到其匹配边缘或平面的距离。与视觉SLAM (以及激光雷达SLAM中的先验平面调整方法)中特征必须与位姿协同确定不同，我们表明特征可以解析求解并从BA中移除，得到的BA仅依赖于扫描位姿。这大大减少了优化规模，并允许使用大规模密集的平面和边缘特征。为了加速优化，我们推导出了成本函数的解析导数的闭合形式，直到二阶。此外，我们提出了一种新颖的自适应体素化方法来高效地搜索特征对应。所提出的公式被集成到LOAM后端，用于地图精化。结果表明，尽管作为后端，即使在优化20次扫描点云时，局部BA也可以非常有效地求解，甚至在10 Hz下是实时的。局部BA也显著地降低了LOAM漂移。

Introduction

BA 是联合求解三维结构(即,特征点的定位)和相机姿态的问题。在运动恢复结构( structure from motion，Sf M ) 、视觉SLAM (同时定位和定位)、视觉惯导等视觉应用中，该问题一直是一个基本问题。

对于激光雷达建图，可以定义类似的光束法平差，目标是联合确定激光雷达位姿和全局三维点云地图。这将是降低激光雷达SLAM中漂移的关键问题。受限于现有扫描配准方法的成对匹配性质，如迭代最近点( ICP ) 、GICP 、正态分布变换、基于surfel的配准、常用的激光雷达导航与建图( LOAM )框架及其变体[ lego-LOAM ]等，[ Loam_livox ]通常通过增量配准新扫描构建地图。这样的增量映射过程必然会积累配准误差，尤其是在退化发生的无特征环境或小视场激光雷达。降低这种漂移的一种方法是在激光雷达扫描的滑动窗口上执行局部BA，这使得我们可以根据新扫描中的信息重新评估过去的扫描。该方法在视觉导航中得到了广泛的应用，并被证明是非常有效的。

虽然由于直接进行深度测量，激光雷达BA看似比视觉BA简单，但其公式实际上更为复杂。在视觉BA中，测量值是高分辨率图像，其中每个像素对应空间(见图1 ( a ))中的单个特征)。因此，一个自然的提法是最小化投影的特征位置与其在图像上的实际位置之间的差异。然而，这种自然的公式并不适用于激光雷达：激光雷达点云通常非常稀疏，甚至不重复[ Loam_livox 12 ]，使得精确的点匹配不可行。

我们提出了激光雷达BA的提法，并将其作为后端纳入LOAM框架，以完善增量构建的地图。具体来说，我们的贡献如下：1 )我们通过直接最小化特征点到边缘或平面(见图1 ( b ))的距离来构建稀疏激光雷达特征点(包括边缘和平面)上的BA。与同时求解特征位置和相机位姿的视觉BA不同，我们证明了激光雷达BA中的特征(边缘与平面)参数可以在闭式解中解析求解，从而只在扫描位姿上进行BA优化。将特征参数从BA中剔除，极大地降低了优化的维度，从而允许对大规模密集特征进行优化；2 )为了实现有效的BA优化，我们解析地推导了代价函数关于扫描姿态的梯度和Hessian矩阵；3 )提出了一种自适应的体素化方法来有效地搜索特征对应；4 )我们将所提出的激光雷达BA集成到LOAM后端用于地图优化，并通过与旋转激光雷达和小FoV激光雷达的比较证明了其有效性。现有的LOAM实现如图2所示。结果表明，局部BA有效地降低了漂移。虽然它是作为后端设计的，但本地BA运行速度非常快：当优化一个20次扫描的滑动窗口时，它在10Hz下运行几乎实时。

Related Works

我们对激光雷达BA的定义与多视图配准最为相似。对于多线扫描的情况，其代价函数是任意两次扫描中两个对应点之间的所有距离之和。类似地，也有方法使用了任意两次扫描中两个对应曲面之间的距离。虽然这些方法对密集的三维扫描(例如,深度相机)效果很好，但它们都需要精确的点或曲面匹配，而这在激光雷达点云中很少存在。

传统工作采用标准的成对扫描配准方法对任意两个重叠的扫描进行配准。然后将得到的相对位姿作为测量值构建位姿图，通过图优化的方法求解位姿。这些方法需要在所有具有重叠的扫描之间进行重复的成对扫描配准。此外，它没有直接对点云地图进行优化，限制了映射一致性的可达到水平。

激光雷达BA (或多视图配准)的难点在于定义一个度量，有效地评估所有扫描中稀疏点的对齐质量，同时允许有效的优化。有文献基于相关性(或熵)的扫描配准自然地扩展到多次扫描，然而，它需要计算所有点对之间的相关性，这是一个需要仔细工程或GPU加速的计算成本高昂的过程。

为了降低计算量，最近的工作集中在仅对平面特征进行光束法平差。这些工作同时优化了平面参数和扫描姿态，导致了高维的优化。相比之下，我们的方法同时考虑平面和边缘，在BA优化之前解析地以闭合形式求解这两个特征。所得到的BA仅限于在扫描姿态上最小化特征值。类似于[ 32 ]的代价函数(即,特征值)，它使用了一个低效的梯度下降优化，我们解析地推导了二阶导数，并使用了一个高效的Gauss - Newton方法来加速优化。这两个新颖的贡献，即从优化中剔除了特征(既有边缘,又有平面)参数，显著降低了优化维度，同时二阶优化显著加快了收敛速度，即使在优化数量非常大的(例如,几百个)特征时，BA也可以近乎实时地进行。此外，不同于先前的方法通常需要从原始点云分割平面，并且通常承认真实的平面特征，我们提出了一种自适应的体素化方法来匹配平面和边缘特征，而不需要分割。该方法能够进一步适应大平面(例如,地面、墙面)和小平面面片(例如,树冠)的多种环境。

[32] G. Ferrer, “Eigen-factors: Plane estimation for multi-frame and timecontinuous point cloud alignment.” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), 2019, pp. 1278–1284.

对于带滑动窗口优化的LOAM也有一些现有的工作。YShan等[ Liosam ]通过将滑动窗口中的每个扫描注册到目前为止构建的地图，优化了激光雷达扫描的滑动窗口。这实质上忽略了滑动窗口内扫描之间的所有并发约束，从而导致次优解。考虑所有这些约束将导致重复的成对扫描配准。使用多分辨率占据栅格地图，允许多视角配准，但内存或计算开销太大。与这些工作相比，我们的方法考虑了所有扫描之间的所有约束，无论是在滑动窗口还是从地图，并且可以非常有效地求解。

Method

A. Direct BA Formulation

本节讨论的是基于激光雷达的束调整（Bundle Adjustment, BA）问题，目标是同时优化多个激光雷达扫描的位姿（pose）以及全局3D点云地图。在传统视觉SLAM中，BA优化的是相机位姿和3D地标点的位置，而在LiDAR SLAM中，优化对象是LiDAR扫描的位姿和点云地图的几何特征。在本论文中，点云地图被简化为单一几何特征（平面或边），而不是分散的3D点。这简化了问题，使得BA的目标变为