0. 简介

束调整（Bundle Adjustment，BA）是指同时确定传感器姿态和场景几何的问题，这是机器人视觉中的一个基本问题。本文提出了一种高效且一致的激光雷达束调整方法。该方法利用边缘和平面特征来表示场景几何，并直接最小化每个原始点到相应几何特征的自然欧氏距离。这种表述的一个好处是几何特征可以通过解析方法求解，从而大大降低了数值优化的维度。为了更高效地表示和求解所得到的优化问题，《Efficient and Consistent Bundle Adjustment on Lidar Point Clouds》提出了一种新颖的概念——点簇，通过一组紧凑的参数（点簇坐标）来编码与同一特征相关联的所有原始点。我们基于点簇坐标推导了BA优化的闭式导数，包括一阶和二阶导数，并展示了它们的理论性质，如零空间和稀疏性。基于这些理论结果，本文开发了一种高效的二阶BA求解器。除了估计激光雷达姿态，该求解器还利用二阶信息估计由测量噪声引起的姿态不确定性，从而得到一致的激光雷达姿态估计。此外，由于使用了点簇，所开发的求解器在优化的所有步骤中都能避免枚举每个原始点（由于数量庞大，这是非常耗时的）：成本评估、导数评估和不确定性评估。所提出的方法在模拟和实际环境中进行了广泛评估，包括一致性、准确性和计算效率。在涵盖各种数据集（Hilti、NTU-VIRAL和UrbanLoco）、环境（校园、城市街道、办公室、实验室和建筑工地）、激光雷达类型（Ouster OS0-64、Ouster OS1-16、Velodyne HDL 32E）和运动类型（手持、基于无人机和基于地面车辆）的19个真实开放序列上进行了基准评估，结果显示我们的方法在定位准确性、建图质量和计算效率方面始终明显优于其他最先进的方法。具体的代码已经在Github上开源了

1. 主要贡献

在这项工作中，我们提出了一个专门针对激光雷达点云的高效一致的BA（Bundle Adjustment）框架。该框架遵循我们之前的工作BALM [32]，该工作基于激光雷达扫描中丰富的边缘和平面特征，对激光雷达BA问题进行了建模。BA的公式自然地最小化了每个点到相应边缘或平面的欧氏距离，决策变量包括激光雷达的位姿和特征（边缘和平面）参数。此外，我们证明了几何参数（即边缘和平面）可以通过解析方法求解，从而使优化问题仅依赖于激光雷达的位姿。由于几何特征的数量通常很大，从优化中消除这些几何特征将大大减少优化的维度（从而减少时间）。

我们提出的BA框架中的一个新设计是点簇，它通过一组紧凑的参数（点簇坐标）总结了与一个特征相关联的激光雷达扫描中的所有点。基于点簇，我们推导出了BA优化相对于其决策变量（即激光雷达位姿）的闭式导数（高达二阶）。我们证明了通过点簇可以完全表示所建模的BA优化和闭式导数，而无需枚举激光雷达扫描中的大量个别点。消除对个别原始点的依赖大大加快了代价函数和导数的计算速度，进一步使我们能够开发出高效一致的二阶求解器BALM2.0，并将其发布在Github上，以造福社区。我们的实验视频可在https://youtu.be/MDrIAyhQ-9E上观看。

我们对所提出的BA方法进行了广泛的评估。模拟研究表明，BA方法能够产生一致的激光雷达位姿估计。对19个真实世界开放序列进行的详尽基准比较表明，BA方法在位姿估计准确性、建图准确性和计算效率方面均表现出一致较高的性能，优于其他对比方法。最后，我们将BA方法集成到三个重要的激光雷达应用中：激光雷达惯性测距、多激光雷达标定和全局建图，并展示了通过所提出的BA方法如何提高它们的准确性和/或计算效率。

2. 束调整的公式和优化

在本章中，我们推导出我们的束调整公式和优化方法。首先，根据[32]的方法，我们将束调整公式化为最小化点到平面（或点到边缘）距离（第3节），并且证明了特征参数可以从公式化的优化中消除（第4节）。然后，我们在第5节介绍了点簇，基于此，我们在第6节中推导出了一阶和二阶导数。基于这些理论结果，我们在第7节中提出了我们的二阶求解器。然后，在第8节中分析了所提出的束调整的时间复杂度。最后，在第9节中，我们展示了如何估计束调整解的不确定性。在本文中，我们使用了表I中总结的符号，或者在上下文中另有说明。

图1. 束调整公式的因子图表示

3 束调整公式

如图1所示，假设有M_f个特征，每个特征由参数π_i（i = 1, …, M_f）表示，由M_p个观测到激光雷达位姿，每个位姿用T_j = (R_j, t_j)表示（j = 1, …, M_p），束调整是指同时确定所有激光雷达位姿（用T = (T_1, · · · , T_{M_p})表示）和特征参数（用π = (π_1, · · · ,π_{M_f})表示），以使重建的地图与激光雷达测量结果最好地一致。记c(π_i, T)为第i个特征的地图一致性，一个直观的束调整公式为

在我们的BA公式中，我们利用了在激光雷达点云中通常丰富的平面和边缘特征，并最小化每个测量的原始激光雷达点与其对应的平面或边缘特征之间的欧氏距离。具体而言，假设在第j个激光雷达姿态上测量到了N_{ij}个激光雷达点，每个点用p_{f_{ijk}}表示（k = 1, …, N_{ij}）。它在全局坐标系中的预测位置为

对于一个平面特征，它由参数π_i = (n_i, q_i) 表示，其中n_i是平面的法向量，q_i是平面上的任意一点，都是在全局坐标系下表示的（见图2(a)）。

图2. 激光雷达BA中使用的平面和边缘特征。(a) 平面表示法。q_i是平面上的一个点，ni是平面的法向量。(b) 边缘表示法。q_i是边缘上的一个点，n_i是边缘的方向。

然后，测量点p_{f_{ijk}}到该平面的欧氏距离为||n^T_i(p_{ijk} − q_i)||^2。将所有姿态中观测到的所有点的距离聚合起来，得到与该平面特征对应的总地图一致性。

在平面特征上，N_i = \sum^{M_p}_{j=1} N_{ij} 表示所有姿态下观测到的激光雷达点的总数。对于边缘特征，它由π_i = (n_i, q_i) 参数化，其中n_i是边缘方向向量，q_i是边缘上的任意一点，两者都在全局坐标系中（见图2（b））。然后，测量点p_{f_{ijk}}到边缘的欧氏距离为||(I − n_in^T_i)(p_{ijk} − q_i)||_2。将所有姿态下观测到的所有点的距离聚合起来，得到与该边缘特征对应的总地图一致性。

在这里，N_i = \sum^{M_p}_{j=1} N_{ij} 是所有姿态观察到的边缘特征上的激光雷达点的总数

对于边缘特征而言，它由参数π_i = (n_i, q_i)来描述，其中n_i是边缘方向向量，q_i是边缘上的任意一点，两者均在全局坐标系中表示（见图2（b））。然后，测量点p^f_{ijk}到边缘的欧氏距离为：||(I − n_in_i^T)(p_{ijk} − q_i)||^2。将所有姿态中观测到的所有点的距离聚合起来，得到与该边缘特征对应的总地图一致性。

其中 N_i =\sum ^{M_p}_{j=1} N_{ij} 是所有姿态在边缘特征上观察到的激光雷达点的总数。

4. 特征参数的消除

在这一节中，我们展示在BA优化中，特征参数π实际上可以通过封闭形式的解来求解。关键的观察是，一个成本项c(π_i, T)仅依赖于一个特征参数，因此这个特征参数可以独立优化。具体来说，这意味着在优化所有特征参数时，我们可以独立地优化每一个特征参数。

对于平面特征，有一个特定的表达式来描述这个优化问题。该表达式涉及矩阵的特征值和特征向量。值得注意的是，优化解在(6)中不是唯一的，任何从q^__i沿垂直于n^__i的方向的偏差都将等同于最优解。然而，这些等效的最优解既不会改变平面，也不会改变最优成本。

对于边缘特征，也有一个特定的表达式来描述这个优化问题，与平面特征相似。再次，最优解在(8)中不是唯一的，但这些等效的最优解也不会改变边缘或最优成本。

从(6)和(8)可以看出，无论是平面还是边缘的每一个特征的参数πi都可以解析地求解，从而从BA优化过程中移除。因此，原始的BA优化简化为一个新的表达式(9)。

需要注意的是，(9)中的矩阵A_i取决于激光雷达的姿态T，因为每个涉及的点p_{ijk}都依赖于姿态。因此，(9)中的优化只涉及激光雷达的姿态T，这大大减少了优化的维度，从而减少了计算时间。

5. 点集群

在BA优化中，另一个困难是评估矩阵A_i（及其用于开发数值求解器所必需的雅可比或海森矩阵）需要枚举每个激光雷达姿态下观测到的每个点。由于激光雷达扫描中的点数目巨大，这种枚举计算成本非常高。本节介绍了一个新概念“点集群”，可以避免这种点的枚举。

点集群是一个有限的点集。它可以被视为一个广义的点，我们可以对其应用刚性变换。并定义了点集群上的刚性变换和集群合并操作。这些操作可以完全由其点集群坐标表示。重要的是，点集群可以在一个帧（如局部激光雷达帧）中构造，并在不枚举每个单独点的情况下转移到另一个帧（如全局帧）。

图3可能显示了两种点集群上的操作：（a）刚性变换（Rigid transform）和（b）集群合并（Cluster merging）

注意，点集和其坐标并不是一一对应的关系。从点集到坐标是唯一确定的，但反过来则不成立。因此，如果需要重新聚类，则必须保存原始点。

文章接着将点集群应用到BA问题。首先，将同一特征上的所有点分组为一个点集群。这个点集群的坐标完全足够代表BA优化中所需的矩阵Ai。这意味着，BA优化只需要知道点集群坐标，而无需枚举每个单独的点。

图4. 对于第i个特征（平面或边缘），在第j个姿态观测到的所有点被聚集为一个点集群，并在其本地框架中被表示为C_{f_{ij}}。

此外，还提出了一个定理，说明对所有姿态T_j进行相同的刚性变换并不改变优化。这意味着BA优化对全局参考帧的变化是不变的，这在束调整问题中是众所周知的计量自由度。