文章目录
- 前言
- 一、介绍
- 二、相关工作
- 三、方法
- A. 概述
- B. 自底向上的分层BA（Bundle Adjustment）
- C. 自顶向下位姿图优化
- 四. 实验
- A. 精度分析
前言
代码：github
原文：原文
摘要——重建准确且一致的大规模LiDAR点云地图对机器人应用至关重要。现有的解决方案，姿态图优化，虽然时间效率较高，但并未直接优化地图的一致性。近期提出的LiDAR束调整（BA）旨在解决这一问题；然而，在大规模地图上，计算耗时过长。为了解决这一问题，本文提出了一种适用于大规模地图的全局一致性和高效建图方法。我们提出的工作包括自下而上的层次BA和自上而下的姿态图优化，结合了两种方法的优点。通过层次设计，我们解决了多个BA问题，其Hessian矩阵的大小远小于原始BA；通过姿态图优化，我们平滑且高效地更新LiDAR姿态。我们的方法在多个空间和时间上的大规模公共旋转LiDAR数据集（如KITTI、MulRan和Newer College）以及在结构化和非结构化场景下自收集的固态LiDAR数据集上得到了有效性和鲁棒性的验证。在适当的设置下，我们展示了我们的工作可以在约12%的序列时间内生成一个全局一致的地图。
一、介绍
重建三维（3D）高分辨率地图在机器人、环境和土木工程领域具有重要意义。这种3D地图可以作为自主服务机器人使用的先验信息，以及建筑和地理测量的信息模型。与传统的3D激光扫描仪相比，光学探测和测距（LiDAR）传感器由于其快速扫描率而特别适合这个目的。此外，它更轻便、具有成本效益，并且能够灵活地安装在多种平台上，例如地面或空中车辆和手持设备。本文重点开发一种适用于大规模地图的准确且一致的LiDAR建图方法。关于基于LiDAR的建图算法的丰富研究成果已经被提出，这些算法生成点云地图和LiDAR里程计。由于扫描到地图配准误差的累积，里程计漂移通常会出现，进一步导致点云地图的发散。改善映射质量（缩小误差）的最著名方法是姿态图优化，它最小化两个LiDAR帧之间的相对姿态误差。在姿态图优化中，相对姿态估计被假设为服从高斯分布。然而，这种近似在现实中可能被高估。此外，姿态图优化并未直接优化点云的一致性，点云地图中的发散可能仅被缩小而未完全消除（或甚至未被察觉）。当检测到错误的回环或发生不正确的相对变换估计时，这种现象更为明显。
LiDAR束调整（BA）方法直接通过最小化整体点到平面的距离来优化映射一致性，这通常会导致必要的高映射质量。在文献中，平面参数首先被解析求解，从而最终优化问题仅与LiDAR姿态相关。在每次迭代中，通过施用Schur余量技巧（如视觉BA中）来消除平面参数。无论哪种方式，结果优化至少是LiDAR姿态数量N的维度，求解时需要O(N³)的时间。计算时间的立方增长阻碍了大规模地图中大数量姿态的束调整。
为了解决上述问题，我们提出了一种层次LiDAR BA方法，以全局优化映射一致性，同时保持时间效率。该方法构建了一种帧姿态的金字塔结构，并进行自下而上的层次束调整和自上而下的姿态图优化。自下而上的过程在从**底层（局部BA）到顶层帧（全局BA）**的局部窗口中进行层次束调整。这种设计有利于计算时间，因为每一层的局部BA过程适合并行处理，并且由于涉及的姿态数量较少，每个局部BA的时间复杂度相对较低。在自下而上的过程中，一个问题是忽略了在不同局部窗口中共同可见的特征，这可能会降低准确性。为了解决这个问题，自上而下的过程从顶层到底层构建姿态图，并通过姿态图优化分配误差。这两个过程迭代直到收敛。
通过层次束调整设计，我们可以直接优化点云内平面表面的一致性，避免求解大维度的代价函数。通过姿态图优化，我们快速且可靠地更新整个LiDAR姿态以实现收敛。为了保持每两个相邻关键帧之间的平滑性，我们通过设置步幅大小小于窗口大小来保留它们之间的重叠区域。为了进一步提升优化速度，我们应用了过滤器来移除离群点，并在构建金字塔时实现了基于CPU的并行处理。总之，我们的贡献如下：
- 我们提出了一种层次束调整方法，以全局优化LiDAR映射一致性和里程计精度。我们提出的方法在给定良好初始姿态轨迹（例如，来自姿态图优化）时改善了映射质量，甚至在初始姿态轨迹存在较大漂移时也能缩小误差。
- 我们的方法在多个公共机械旋转LiDAR数据集以及在结构化和非结构化场景下我们自收集的固态LiDAR数据集上得到了有效性验证。
二、相关工作
文献中讨论了多种提高映射质量的方法，主要可以分为两类：姿态图优化和基于平面（束）调整的方法。在姿态图优化中，两个帧之间的相对变换（姿态约束）是通过ICP或其变种来估计的。然后，这个相对姿态误差通过信息矩阵加权，信息矩阵通常是对应Hessian的逆矩阵，或者仅仅是一个常数矩阵。通过最小化总的相对姿态误差来优化姿态图。尽管计算效率高，姿态图优化的一个重要问题是，它并没有直接优化点云的一致性。由于相对姿态约束的错误估计或不精确建模，姿态图可能会收敛到一个局部最小值，而点云中仍可能存在显著的发散。
平面调整方法通过最小化总的点到平面的距离直接优化点云的一致性。在平面调整中，每个平面特征由两个参数表示，即其中心到视点的距离和估计的平面法向量。在文献中，作者同时优化LiDAR姿态和几何平面特征。这种方法需要在优化过程中维护和更新所有特征的参数，而随着地图规模的扩大，特征的总数会迅速增长，导致成本函数的维度非常庞大。尽管通过Schur余量技术可以将优化变量减少到仅LiDAR姿态，这种方法在实际优化过程中容易产生姿态估计的波动。
束调整方法通过在优化之前使用闭式解消除特征参数来改进平面调整技术。在文献中，作者将点云分割为多个体素，每个体素包含一个平面特征。原始的点到平面的最小化问题被转化为每个体素中点协方差的特征值最小化。该方法需要迭代每个特征中的每个点以导出Hessian矩阵，时间复杂度是点数的平方，造成巨大的计算需求。后续工作解决了这一问题，通过聚合同一姿态观察到的特征的所有点，从根本上消除了时间复杂度对点数的依赖。尽管如此，在上述所有平面和束调整方法中，计算复杂度仍然是姿态数的立方，这在大规模地图中并不实际。此外，当地图中的发散大于或接近最大体素大小时，这些方法可能会有较慢的收敛速率。
我们提出的层次束调整方法结合了束调整和姿态图优化的优点。我们使用束调整直接最小化点到平面的距离，并使用姿态图优化平滑且高效地更新LiDAR姿态，以避免姿态估计中的波动。通过层次设计，我们可以并行解决多个束调整问题，相比于原始问题，Hessian矩阵的大小要小得多。此外，我们可以根据初始姿态轨迹的质量，从底层到顶层灵活设置束调整参数。
三、方法
图1：提出的分层捆绑调整的金字塔结构，层数
l
=
3
l=3
l=3，步长
s
=
3
s=3
s=3，窗口大小
w
=
6
w=6
w=6。第一层、第二层和第三层的因子连接同一层中每两个相邻节点。红色虚线连接应该相同的节点，例如，
x
9
i
1
x_{9i}^{1}
x9i1，从下层的局部窗口的第一个节点到上层由这个局部窗口构建的节点，例如，
x
3
i
2
x_{3i}^{2}
x3i2 和
x
i
3
x_{i}^{3}
xi3。
图2：系统概述。浅黄色区域表示自下而上的过程，浅蓝色区域表示自上而下的过程。
A. 概述
我们提出的方法的系统工作流程如图2所示。输入为来自每个LiDAR扫描的原始或去畸变点以及它们在全局框架中的初步姿态估计，这些可以通过一般的LiDAR里程计或同时定位与地图构建（SLAM）算法获得。该方法由两个过程组成：自下而上（见第III-B节）和自上而下（见第III-C节），两个过程迭代直到收敛。在自下而上的过程中，对较小的局部窗口内的LiDAR帧进行局部束调整，以从第一层到第二层构建关键帧（见图1）。该过程以层次方式执行，直到达到最佳层数，然后对顶层关键帧进行全局束调整。接下来，使用来自每个优化层和相邻层之间的因素构建姿态图（见图1）。
如图1所示，第一层（在上述上下文中也称为底层）描述了初始LiDAR帧和姿态的集合。类似地，第二层表示通过局部束调整从第一层创建的LiDAR关键帧和姿态的集合。顶层意味着最后剩余的LiDAR关键帧的集合（在图1中，顶层指的是第三层）。从底层到顶层层次创建LiDAR关键帧的过程称为自下而上的过程。通过姿态图优化更新底层LiDAR姿态的过程称为自上而下的过程。
B. 自底向上的分层BA（Bundle Adjustment）
我们用 F j i F_{j}^{i} Fji 表示第i层的第j个激光雷达帧，用 x j i ≜ T j i = ( R , t ) ∈ S E ( 3 ) x_{j}^{i}\triangleq T_{j}^{i}=(R, t)\in SE(3) xji≜Tji=(R,t)∈SE(3) 表示其对应的位姿。我们用 T j , k i T_{j,k}^{i} Tj,ki 表示 T j i T_{j}^{i} Tji 和 T k i T_{k}^{i} Tki 之间的相对位姿，即 T j , k i = ( T j i ) − 1 ⋅ T k i T_{j,k}^{i} = \left(T_{j}^{i}\right)^{-1}\cdot T_{k}^{i} Tj,ki=(Tji)−1⋅Tki。需要注意的是， F j i F_{j}^{i} Fji 中的点是在激光雷达局部坐标系中表示的，而T是在全局坐标系中。我们用w表示在自底向上构建激光雷达关键帧过程中的局部窗口大小，用s表示步长，如图1示。假设我们从第i层获得了总共 N i N_{i} Ni个激光雷达帧。在自底向上的过程中，使用提供的初始位姿轨迹，在每个局部窗口中执行局部BA（Bundle Adjustment），并优化这个窗口中每帧与第一帧之间的相对位姿。从每个局部窗口的BA中得到的Hessian矩阵H也被记录下来，并用作后续自顶向下位姿图构建中的信息矩阵。给定一个包含w个激光雷达帧的局部窗口 { F s j + k i ∣ j = 0 , ⋯ , ⌊ s ⌊ N i − w s ⌋ ; k = 0 , ⋯ , w − 1 } \{F_{s j+k}^i \mid j=0,\cdots,\lfloor s \left\lfloor\frac{N_{i}-w}{s}\right\rfloor; k=0,\cdots, w-1\} {Fsj+ki∣j=0,⋯,⌊s⌊sNi−w⌋;k=0,⋯,w−1}在第i层，以及窗口中优化后的相对位姿 T j , k i ∗ T_{j, k}^{i*} Tj,ki∗，我们将这些帧聚合为第 ( i + 1 ) (i+1) (i+1)层的关键帧。这个关键帧中的点都被转换到局部窗口的第一帧中，关键帧的位姿，记作 T j i + 1 T_{j}^{i+1} Tji+1，被设置为前一个局部窗口中优化的第一帧的位姿，即，
F j i + 1 ≜ ⋃ k = 0 w − 1 ( T s ⋅ j , s ⋅ j + k i ∗ ⋅ F s ⋅ j + k i ) T j i + 1 = T s ⋅ j i ∗ = ∏ k = 1 j T s ⋅ k − s , s ⋅ k i ∗ ( 1 ) \begin{align*} F_{j}^{i+1} &\triangleq \bigcup_{k=0}^{w-1}\left(T_{s\cdot j,s\cdot j+k}^{i*}\cdot F_{s\cdot j+k}^{i}\right) \\ T_{j}^{i+1} &= T_{s\cdot j}^{i*} = \prod_{k=1}^{j}T_{s\cdot k-s,s\cdot k}^{i*} \end{align*} \qquad(1) Fji+1Tji+1≜k=0⋃w−1(Ts⋅j,s⋅j+ki∗⋅Fs⋅j+ki)=Ts⋅ji∗=k=1∏jTs⋅k−s,s⋅ki∗(1)
这个过程可以从较低层重复执行到较高层，直到达到最优的层数l。值得注意的是，新关键帧的构建（局部BA）不依赖于局部窗口外的帧，这使得使用多个局部窗口变得适合。我们可以在同一层中进行并行处理。假设我们总共有N个激光雷达帧，即
N
1
=
N
N_{1}=N
N1=N，并且每次我们选择将下层的w帧聚合成一帧到上层，步长为s。设n为可用于并行处理的线程数。由于BA的计算时间为
O
(
M
3
)
O(M^3)
O(M3)，其中M是涉及的位姿数量，我们可以推导出第l层金字塔的整体时间消耗
O
(
T
l
)
O(T_{l})
O(Tl)。
第l层金字塔的总时间消耗包括每层局部BA消耗的时间以及顶层全局BA的时间。对于第l层金字塔，第i层的局部窗口数
(
i
<
l
)
(i<l)
(i<l) 是
N
s
i
\frac{N}{s^i}
siN，每个局部窗口消耗
O
(
w
3
)
O(w^3)
O(w3) 的时间。有n个并行线程的情况下，局部BA的总时间消耗等于每层局部BA的总和，即
w
3
⋅
(
∑
i
=
1
l
−
1
N
s
i
⋅
1
n
)
w^3 \cdot \left(\sum_{i=1}^{l-1} \frac{N}{s^i} \cdot \frac{1}{n}\right)
w3⋅(∑i=1l−1siN⋅n1)，而第l层的全局BA需要
O
(
(
N
s
l
−
1
)
3
)
O\left(\left(\frac{N}{s^{l-1}}\right)^3\right)
O((sl−1N)3) 的时间。总之，
O
(
T
l
)
O(T_l)
O(Tl) 表示为
T l = { N 3 ( l = 1 ) w 3 n ⋅ ∑ i = 1 l − 1 N s i + ( N s l − 1 ) 3 ( l > 1 ) ( 2 ) T_l=\begin{cases} N^3 & (l=1) \\ \frac{w^3}{n} \cdot \sum_{i=1}^{l-1} \frac{N}{s^i} + \left(\frac{N}{s^{l-1}}\right)^3 & (l>1) \end{cases} \quad (2) Tl={N3nw3⋅∑i=1l−1siN+(sl−1N)3(l=1)(l>1)(2)
我们将 T l T_{l} Tl 视为l的函数，并通过将 T l T_{l} Tl 的导数设为零来计算最优的 l ∗ l^{*} l∗，得到
l ∗ = ⌊ 1 2 log s ( 3 N 2 ( s 3 − s ) n w 3 ) ⌋ ( 3 ) l^*=\left\lfloor\frac{1}{2}\log_s\left(\frac{3 N^2\left(s^3-s\right) n}{w^3}\right)\right\rfloor \quad (3) l∗=⌊21logs(w33N2(s3−s)n)⌋(3)
图3显示了在不同帧数N下，计算时间
T
l
T_{l}
Tl 与层数l的关系。可以看出，当层数从
l
=
1
l=1
l=1（原始BA）增加到
l
∗
l^{*}
l∗ 时，计算时间大幅减少，表明所提出的分层BA的有效性。当
l
>
l
∗
l>l^{*}
l>l∗ 时，计算时间没有显著增加，几乎保持恒定，表明任何大于
l
∗
l^{*}
l∗ 的层数都将同样有效。
图3：使用
w
=
10
w=10
w=10，
s
=
5
s=5
s=5 和
n
=
8
n=8
n=8 的情况下，时间消耗
T
l
T_{l}
Tl 相对于层数 l 和位姿数量 N 的示例图表。可以看出，从原始BA
(
l
=
1
)
(l=1)
(l=1) 到我们提出的分层BA
(
l
=
3
(l=3
(l=3 或
l
=
4
)
l=4)
l=4)，整体时间消耗显著减少。
在自底向上的分层BA的每层中进行特征提取和关联时，我们使用了文献[5]中提出的自适应体素化方法，该方法能够提取适合不同结构环境的、不同大小的平面特征。为了提取这些不同大小的平面特征，整个点云在利用初始位姿轨迹转换到同一全局坐标系后，被分割成多个大小为V的体素，每个体素都通过检查所包含点的最小和最大特征值比是否小于一个阈值来进行平面测试，即 λ 1 λ 3 < θ \frac{\lambda_{1}}{\lambda_{3}}<\theta λ3λ1<θ。如果平面测试通过，体素内的点将被视为位于同一平面上，并用于BA。否则，体素将被递归分割，直到所包含的点形成一个平面。
当点的数量极大时，上述自适应体素化过程会非常耗时。为了缓解这个问题，我们注意到在较低层中未被视为平面特征的点在上层也不会形成平面。因此，我们仅使用每个体素中的平面特征点在局部BA中构建上层的关键帧，以自底向上的方式进行。这一过程进一步节省了下一层自适应体素图构建的时间，并提高了局部BA的计算精度。
C. 自顶向下位姿图优化
自顶向下位姿图优化过程旨在减少自底向上分层BA过程中的位姿估计误差，该过程仅考虑在同一局部窗口中共视的特征，但忽略了在不同局部窗口中观察到的特征。如图中所示，在金字塔结构中以自顶向下的方式构建位姿图。
在金字塔的每一层中，因子是相邻帧之间的相对位姿。具体来说，第i层因子中节点x和xj+1之间的成本项定义为
e
j
,
j
+
1
=
Log
(
(
T
j
i
+
1
)
−
1
(
T
g
)
−
T
j
+
1
i
+
1
)
(
4
)
e_{j,j+1} = \text{Log}((T_{j}^{i+1})^{-1} (T_{g}) - T_{j+1}^{i+1}) \quad (4)
ej,j+1=Log((Tji+1)−1(Tg)−Tj+1i+1)(4)
(4)
其中x+1, i属于C，j属于F’，并通过反转从自底向上分层BA过程中获得的Hessian矩阵H来计算。L = {1, …, l}表示l层的集合，Fi = {0, …, Ni-1}表示Ni数量的集合。
图4：我们提出的最终因子图，层数
l
=
3
l=3
l=3，步长
s
=
3
s=3
s=3，窗口大小
w
=
6
w=6
w=6。
因此，(4)中的成本项简化为
e
j
,
j
+
1
i
=
Log
(
(
T
j
,
j
+
1
i
∗
)
−
1
(
T
s
i
−
1
⋅
j
1
)
−
1
T
s
i
−
1
⋅
(
j
+
1
)
1
)
∨
c
(
x
s
i
−
1
⋅
j
1
,
x
s
i
−
1
⋅
(
j
+
1
)
1
)
=
(
e
j
,
j
+
1
i
)
T
(
Ω
j
,
j
+
1
i
)
−
1
e
j
,
j
+
1
i
(
5
)
\begin{align*} & e_{j, j+1}^i = \operatorname{Log}\left(\left(T_{j, j+1}^{i*}\right)^{-1}\left(T_{s^{i-1}\cdot j}^1\right)^{-1} T_{s^{i-1}\cdot(j+1)}^1\right)^\vee \\ & c\left(x_{s^{i-1}\cdot j}^1, x_{s^{i-1}\cdot(j+1)}^1\right) = \left(e_{j, j+1}^i\right)^T\left(\Omega_{j, j+1}^i\right)^{-1} e_{j, j+1}^i \end{align*} \quad (5)
ej,j+1i=Log((Tj,j+1i∗)−1(Tsi−1⋅j1)−1Tsi−1⋅(j+1)1)∨c(xsi−1⋅j1,xsi−1⋅(j+1)1)=(ej,j+1i)T(Ωj,j+1i)−1ej,j+1i(5)
其中
i
∈
L
i \in \mathcal{L}
i∈L 和
j
∈
F
i
j \in \mathcal{F}^{i}
j∈Fi，原始位姿图在图1中简化为图4。需要注意的是，当
s
<
w
s < w
s<w 时，连续局部窗口重叠区域内的帧可能会贡献多个成本项（每个局部窗口贡献一个）。要最小化的目标函数为
f
(
F
,
X
)
=
∑
i
∈
L
∑
j
∈
F
i
c
(
x
s
i
−
1
⋅
j
1
,
x
s
i
−
1
⋅
(
j
+
1
)
1
)
(
6
)
f(F, \mathcal{X}) = \sum_{i \in \mathcal{L}} \sum_{j \in \mathcal{F}^i} c\left(x_{s^{i-1} \cdot j}^1, x_{s^{i-1} \cdot (j+1)}^1\right) \quad (6)
f(F,X)=i∈L∑j∈Fi∑c(xsi−1⋅j1,xsi−1⋅(j+1)1)(6)
其中 KaTeX parse error: Expected '}', got '\right' at position 47: …\mathcal{F}^{1}\̲r̲i̲g̲h̲t̲} 是所有第一层帧的集合。然后，这个因子图通过使用GTSAM
1
{}^{1}
1 的Levenberg-Marquardt方法来求解。
四. 实验
A. 精度分析
- 初始里程计与回环闭合：在本节中，我们以最先进的（SOTA）激光雷达SLAM算法1-3的里程计结果作为输入，并进一步用我们提出的方法进行优化。我们证明了即使在初始位姿已经通过位姿图优化之后，我们的工作仍然可以提高制图质量（一致性）和位姿估计的准确性。在所有后续实验中使用的BA和金字塔参数，如果没有进一步说明，都列在表中。根据每个序列中的实际位姿数量N，使用③计算得到最优的l*。对于数据长度N< 5（KITTI 17, MulRan[18] 和 Newer College[19]），我们有l*= 3，对于更大的序列，即N≥ 5 x 10^3（New College[20]），我们有 l ∗ = 4 l^{*}=4 l∗=4。
图5：来自（A）MULLS和（B）我们提出的方法在KITTI序列07上的映射结果。白色虚线矩形强调了发散现象。
我们选择绝对轨迹误差（ATE）作为评估标准，因为每个激光雷达帧都有一个唯一的真实值。可以看出，即使在位姿图优化后，我们提出的方法也能进一步提高位姿估计的准确性，特别是在平移方面。尽管我们的方法并不是在每个序列中都取得了最好的结果，但与其它最先进的方法相比，我们的工作平均产生了最优的ATE结果（0.7°/1.4m）。此外，由于位姿图优化并不直接优化点云的一致性，[3]中的地图发散并没有完全消除（见图5）。这种发散可以通过我们提出的分层BA和位姿图优化来迭代解决，从而反过来减少了位姿估计的ATE。
然后我们在另一个空间大尺度的旋转激光雷达数据集MulRan[18]上测试了我们提出的方法。所包含序列DCC、KAIST和RIVER-SIDE的平均长度分别为4.9公里、6.1公里和6.8公里。与大部分数据收集于城市区域的KITTI数据集不同，MulRan数据集包含了更多来自高架桥、河流和森林的具有挑战性的场景。我们选择使用LIO-SAM[2]的位姿图优化位姿轨迹结果作为我们的输入。绝对平移误差的均方根误差（RMSE）已在表III中总结。可以看出，即使在这些具有挑战性的非结构化森林场景中，我们提出的方法仍然可以提高位姿估计的准确性。然而，这些场景共有7339帧。金字塔参数设置与表中相同，但由于室内场景尺寸较小，初始体素尺寸减小（Vlocal=Vglobal=1m）。第二个测试场景是一个无结构的户外公园，周围有灌木丛、草地和树林（见图9）。这个场景的尺寸大约为95x195米，序列长度为3407帧。因此，我们调整了初始体素尺寸（Vlocal=Vglobal=2m），而没有改变表中的其他金字塔参数。对于这两个测试序列，我们使用没有回环闭合的FAST-LIO2的位姿估计作为我们的输入。由于没有可用的真实值测量，我们改用平均地图熵（MME[27]）来评估制图质量（见表VI）。由于MME计算协方差矩阵的行列式的自然对数，MME越小，点云的一致性越高。可以看出，我们提出的方法能够进一步改善制图一致性，并在结构化和非结构化场景中缩小差距，无论激光雷达类型如何。
