SLAM文献之Efficient and Consistent Bundle Adjustment on Lidar Point Clouds（BALM）

一、简介

束调整（Bundle Adjustment，简称 BA）是同时优化传感器姿态与场景几何的核心问题，广泛应用于机器人视觉系统。本文提出了一种高效且一致的激光雷达束调整方法，该方法充分利用激光雷达中的边缘与平面几何特征，通过直接最小化原始点到对应几何特征的欧氏距离来表述优化问题。

该方法的一个关键创新在于：几何特征（如边缘和平面）可以解析求解，从而消除其在优化中的显式表示，仅保留激光雷达位姿作为优化变量，大幅降低维度。为了进一步提升效率，本文引入了 **点簇（Point Clusters）**的概念：通过一组紧凑的参数（点簇坐标）统一编码属于同一几何特征的所有原始点。

推导了基于点簇的 BA 优化问题的闭式一阶与二阶导数，并分析其理论特性（如零空间结构与稀疏性）。基于此，本文提出了一个高效的二阶求解器，不仅能优化位姿，还能估计由观测噪声导致的位姿不确定性，实现一致性估计。

此外，该求解器在代价函数计算、导数计算及不确定性评估等各阶段均无需遍历所有原始点，显著提升了整体效率。方法已在多个模拟与真实场景下进行了充分实验验证，包括 Hilti、NTU-VIRAL 与 UrbanLoco 等 19 个多样性数据集，涵盖多种环境、雷达类型及运动平台。结果表明，该方法在定位精度、建图质量与计算效率上始终优于现有最先进方法。项目代码已开源，视频演示详见：https://youtu.be/MDrIAyhQ-9E。

---

二、算法背景

• Bundle Adjustment（BA） 是一种用于同时优化传感器位姿和三维场景几何结构的关键技术，广泛应用于视觉重建、SLAM 和 3D 重构领域 。
• 在激光雷达（LiDAR）SLAM 中，虽然前端通常执行扫帧（frame-to-map）配准来估计位姿，但真正的一致性和精细地图构建还需后端的全局优化。
• 激光雷达 BA 方法因需处理大量原始点，会面临计算开销大、优化变量维度高、不易估计不确定性等挑战 。

---

三、需解决的关键问题

1. 高效处理大量点云数据

   • LiDAR 扫描含数十万原始点，直接参与 BA 会导致计算爆炸性增长（残差函数与雅可比矩阵）。

2. 降低优化变量维度

   • 处理边缘(edge)和平面(plane)等几何特征时，传统方法把这些特征作为优化变量，会增加优化规模。
   • 所以研究者希望解析消元几何特征变量，仅优化 LiDAR 位姿，从而减小变量维度 。

3. 保持估计的一致性（Consistency）

   • 不仅要找到最优解，还需量化估计信任范围（pose uncertainty），需融入二阶信息（Hessian）。

4. 避免显式枚举所有原始点

   • BA 的核心步骤——成本计算、雅可比、协方差评估——如果枚举每个原始点都参与，将极度耗时。

5. 保持稀疏结构和明确 null-space

   • 优化问题需保留稀疏性（only pose variables），并明确雅可比矩阵的零空间等数学结构，便于数值解法 。

---

四、论文提出的核心方法

1. 基于边缘和平面特征的几何表达

A、 将原始点到特征的欧式距离作为残差BA建模
如下图1 所示，假设共有 $M_f$ 个特征，每个特征用参数 ${\pi }_i$ 表示（其中 $i=1,...,M_f$），并且这些特征被 $M_p$ 个激光雷达位姿观测到，每个位姿记为 $T_j=(R_j,t_j)$（其中 $j=1,...,M_p$）。捆绑调整的目标是同时求解所有激光雷达位姿（记为 $T=(T_1,\cdots ,T_{M_p})$）和所有特征参数（记为 $\pi =({\pi }_1,\cdots ,{\pi }_{M_f})$），使得重建出来的地图尽可能与激光雷达的测量结果一致。

记 $c({\pi }_i,T)$ 表示第 $i$ 个特征所带来的地图一致性（map consistency）项，那么一个直接的 BA 问题可表述为：

$$\underset{T,\pi }{\min }\sum _{i=1}^{M_f}c({\pi }_i,T)\text{\hspace{1em}(1)}$$

在提出的 BA 模型中，利用了激光雷达点云中常见的平面特征和边缘特征，并最小化每个观测点到其对应平面或边缘的欧氏距离。

具体地，假设在第 $j$ 个激光雷达位姿下，第 $i$ 个特征上总共被观测到了 $N_{ij}$ 个点，每个点记为 ${\mathbf{p}}_{ijk}^f$（其中 $k=1,...,N_{ij}$）。该点在全局坐标系下的预测位置为：

$${\mathbf{p}}_{ijk}=R_j{\mathbf{p}}_{ijk}^f+t_j\text{\hspace{1em}(2)}$$

---

平面特征建模：

一个平面特征参数化为 ${\pi }_i=({\mathbf{n}}_i,{\mathbf{q}}_i)$，其中 ${\mathbf{n}}_i$ 是平面的单位法向量，${\mathbf{q}}_i$ 是平面上的任意点，二者都在全局坐标系中定义（参见图 2(a)）。

每个点 ${\mathbf{p}}_{ijk}$ 到平面的欧氏距离为：

$$\Vert {\mathbf{n}}_i^T({\mathbf{p}}_{ijk}-{\mathbf{q}}_i){\Vert }^2$$

将所有观测点的距离平方进行累加，可得该平面特征对应的总一致性误差项：

$$c({\pi }_i,T)=\frac{1}{N_i}\sum _{j=1}^{M_p}\sum _{k=1}^{N_{ij}}{\Vert {\mathbf{n}}_i^T({\mathbf{p}}_{ijk}-{\mathbf{q}}_i)\Vert }^2\text{\hspace{1em}(3)}$$

其中，

$$N_i=\sum _{j=1}^{M_p}{N}_{ij}$$

表示该平面特征在所有位姿下被观测到的激光点总数。

边缘特征建模：

一个边缘特征参数化为 ${\pi }_i=({\mathbf{n}}_i,{\mathbf{q}}_i)$，其中 ${\mathbf{n}}_i$ 是单位方向向量，表示边缘方向，${\mathbf{q}}_i$ 是边缘上的任意点，二者也都在全局坐标系中定义（参见图 2(b)）。

点到直线（边缘）的欧氏距离通过正交投影获得，其误差为：

$${\Vert (I-{\mathbf{n}}_i{\mathbf{n}}_i^T)({\mathbf{p}}_{ijk}-{\mathbf{q}}_i)\Vert }^2$$

将所有点的误差平方相加，得到该边缘特征的总一致性误差为：

$$c({\pi }_i,T)=\frac{1}{N_i}\sum _{j=1}^{M_p}\sum _{k=1}^{N_{ij}}{\Vert (I-{\mathbf{n}}_i{\mathbf{n}}_i^T)({\mathbf{p}}_{ijk}-{\mathbf{q}}_i)\Vert }^2\text{\hspace{1em}(4)}$$

同样地，$N_i={\sum }_{j=1}^{M_p}{N}_{ij}$ 表示该边缘特征所观测到的激光点总数。

该节提出了完整的平面与边缘特征建模方法，将 BA 问题转化为一个典型的非线性最小二乘优化问题，并为后续的一阶、二阶导数推导及求解器设计奠定了数学基础。

---

B、使用闭式求解消除几何特征，使其不作为优化变量

本节将展示，在优化公式 (1) 所定义的 BA（束束调整）问题中，特征参数 π 实际上可以通过闭式解求出。关键观察在于，每一项代价函数项 c(πᵢ, T) 仅依赖于一个特征参数，因此每个特征参数可以被单独优化。具体而言：

$$\underset{T,\pi }{\min }\left(\sum _{i=1}^{M_f}c({\pi }_i,T)\right)=\underset{T}{\min }\left(\underset{\pi }{\min }\left(\sum _{i=1}^{M_f}c({\pi }_i,T)\right)\right)=\underset{T}{\min }\left(\sum _{i=1}^{M_f}\underset{{\pi }_i}{\min }c({\pi }_i,T)\right)\text{\hspace{1em}(5)}$$

对于一个平面特征，将公式 (3) 代入到 c(πᵢ, T) 中，有：

$$\underset{{\pi }_i}{\min }c({\pi }_i,T)=\underset{{\pi }_i}{\min }\left(\frac{1}{N_i}\sum _{j=1}^{M_p}\sum _{k=1}^{N_{ij}}{\Vert n_i^T(p_{ijk}-q_i)\Vert }^2\right)$$

该式最小值为矩阵 $A_i$ 的第三大特征值 ${\lambda }_3(A_i)$，即：

$$\underset{{\pi }_i}{\min }c({\pi }_i,T)={\lambda }_3(A_i)，\text{当}{n}_i^{\star }=u_3(A_i),q_i^{\star }={\bar{p}}_i\text{\hspace{1em}(6)}$$

其中，λₗ(Aᵢ) 表示矩阵 Aᵢ 的第 l 大特征值，uₗ(Aᵢ) 表示其对应的特征向量，矩阵 Aᵢ 和向量 ${\bar{p}}_i$ 定义如下：

$$A_i\triangleq \frac{1}{N_i}\sum _{j=1}^{M_p}\sum _{k=1}^{N_{ij}}(p_{ijk}-{\bar{p}}_i)(p_{ijk}-{\bar{p}}_i{)}^T$$

$${\bar{p}}_i\triangleq \frac{1}{N_i}\sum _{j=1}^{M_p}\sum _{k=1}^{N_{ij}}{p}_{ijk}\text{\hspace{1em}(7)}$$

该推导的详细证明将在补充材料 III-A [59] 中给出。需要注意的是，公式 (6) 中的最优解 $q_i^{\star }$ 并不是唯一的，沿着法向量 $n_i^{\star }$ 垂直方向上的任意偏移都会得到等价的最优解。然而，这些等价解不会改变平面的位置，也不会影响最小代价值，因此不会影响接下来的结果。实际上，$q_i^{\star }$ 可以是该平面上的任意一点。

对于一个边缘特征，将公式 (4) 代入 c(πᵢ, T) 中，有：

$$\underset{{\pi }_i}{\min }c({\pi }_i,T)=\underset{{\pi }_i}{\min }\left(\frac{1}{N_i}\sum _{j=1}^{M_p}\sum _{k=1}^{N_{ij}}{\Vert (I-n_i{n}_i^T)(p_{ijk}-q_i)\Vert }^2\right)$$

该式的最小值为矩阵 Aᵢ 的第二大和第三大特征值之和：

$$\underset{{\pi }_i}{\min }c({\pi }_i,T)={\lambda }_2(A_i)+{\lambda }_3(A_i)，\text{当}{n}_i^{\star }=u_1(A_i),q_i^{\star }={\bar{p}}_i\text{\hspace{1em}(8)}$$

同样地，公式 (8) 中的最优解 $q_i^{\star }$ 也不是唯一的，沿着边缘方向 $n_i^{\star }$ 的任意偏移将得到等价的最优解。但这些等价解不会改变边缘的位置或最小代价，因此不会影响后续优化。

由公式 (6) 和 (8) 可以看出，无论是平面还是边缘特征，其参数 πᵢ 都可以解析地解出，并从 BA 优化中剔除。因此，原始的 BA 优化问题 (1) 可简化为仅关于激光雷达位姿的优化问题：

$$\underset{T}{\min }\left(\sum _{i=1}^{M_f}{\lambda }_l(A_i)\right)\text{\hspace{1em}(9)}$$

其中 l ∈ {2, 3}，为简洁起见省略了在代价函数中明确指定的特征值数量。

需要指出的是，公式 (9) 中的矩阵 Aᵢ 依赖于激光雷达位姿 T，因为每个点 $p_{ijk}$ 本身依赖于 T（见公式 (2) 和 (7)）。因此，优化变量仅包含激光雷达位姿 T，极大地降低了优化问题的维度，从而显著减少计算时间。

2. 引入“点簇（point cluster）”概念

在特征参数已被消除后，BA（Bundle Adjustment）优化（公式9）中剩下的一大难点是：计算矩阵 $A_i$（以及开发数值求解器所需的其雅可比矩阵或海森矩阵）仍需枚举每一个激光帧下观测到的点。而由于每帧激光点云数量极大，这种逐点枚举的方式计算代价极高。在本节中，我们将说明如何通过“点集簇（point cluster）”避免逐点枚举的计算开销，具体定义如下。

一个点集簇是一个有限点集，表示为集合

C = { p k ∈ R 3 ∣ k = 1 , … , n } C = \{p_k \in \mathbb{R}^3 \mid k = 1, \dots, n\} C={pk​∈R3∣k=1,…,n}

其对应的点集簇坐标，记作 $\mathbb{R}(C)$，定义为：

其中：

• $P={\sum }_{k=1}^n{p}_k{p}_k^T$
• $v={\sum }_{k=1}^n{p}_k$
• ${\mathbb{S}}_{4\times 4}$ 表示对称 $4\times 4$ 矩阵集合。

点集簇可以视作一种“广义点”，它也可以被施加刚性变换。我们定义点集簇上的刚性变换如下：

---

定义 1（刚性变换）
给定一个点集簇 C = { p k ∈ R 3 ∣ k = 1 , … , n } C = \{p_k \in \mathbb{R}^3 \mid k = 1, \dots, n\} C={pk​∈R3∣k=1,…,n}，以及一个位姿 $T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\in SE(3)$，其刚性变换作用于点集簇 $C$，记作：

T ∘ C ≜ { R p k + t ∈ R 3 ∣ k = 1 , … , n } (11) T \circ C \triangleq \{R p_k + t \in \mathbb{R}^3 \mid k = 1, \dots, n\} \tag{11} T∘C≜{Rpk​+t∈R3∣k=1,…,n}(11)

---

除了刚性变换，我们还定义了点集簇的合并操作：

---

定义 2（簇合并）
给定两个处于同一参考系下的点集簇

C 1 = { p k 1 ∈ R 3 ∣ k = 1 , … , n 1 } , C 2 = { p k 2 ∈ R 3 ∣ k = 1 , … , n 2 } C_1 = \{p_k^1 \in \mathbb{R}^3 \mid k = 1, \dots, n_1\}, C_2 = \{p_k^2 \in \mathbb{R}^3 \mid k = 1, \dots, n_2\} C1​={pk1​∈R3∣k=1,…,n1​},C2​={pk2​∈R3∣k=1,…,n2​}

合并后的簇记作：

C 1 ⊕ C 2 ≜ { p k l ∈ R 3 ∣ l = 1 , 2 ; k = 1 , … , n l } (12) C_1 \oplus C_2 \triangleq \{p_k^l \in \mathbb{R}^3 \mid l = 1, 2; k = 1, \dots, n_l\} \tag{12} C1​⊕C2​≜{pkl​∈R3∣l=1,2;k=1,…,nl​}(12)

---

接下来我们展示，上述两个操作（刚性变换与簇合并）可以完全用点集簇坐标表示：

---

定理 1（刚性变换的坐标表示）
给定一个点集簇 $C$ 和一个位姿 $T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\in SE(3)$，其刚性变换对应的簇坐标满足：

$$\mathbb{R}(T\circ C)=T\mathbb{R}(C)T^T\text{\hspace{1em}(13)}$$

证明见补充材料 III-B [59]

---

定理 2（簇合并的坐标表示）
给定两个在同一参考系下的点集簇 $C_1$ 和 $C_2$，其合并后的簇坐标满足：

$$\mathbb{R}(C_1\oplus C_2)=\mathbb{R}(C_1)+\mathbb{R}(C_2)\text{\hspace{1em}(14)}$$

证明见补充材料 III-C [59]

---

由此可见，点集簇上的刚性变换与簇合并操作可以分别转化为对其坐标矩阵进行矩阵乘法与矩阵加法操作。如图3所示，两种操作及其坐标表示有形象的图示。

这两个定理具有重要意义：

• 定理1 表明：我们可以在一个参考系（例如本地激光雷达坐标系）中构造点集簇，然后通过刚性变换将其变换至另一个参考系（例如全局坐标系），无需枚举每一个点。
• 定理2 表明：两个（乃至更多，归纳而言）点集簇可以合并为一个新的点集簇。特别地，当第二个簇只包含一个点时，意味着点集簇可以随着激光点的到达而逐个增量构建。

---

备注 1： 点集簇的概念及其操作并非首次提出，已有前人工作（如 [33]–[35]）中使用了类似思想。本文在此基础上进行了形式化描述，包括：

1. 引入了点集簇坐标（由 $P$, $v$, $n$ 组成，见公式10）；
2. 正式定义了两种操作：刚性变换与簇合并；
3. 明确展示了簇操作与其坐标之间的对应关系。

备注 2： 点集与其坐标之间不是一一对应的关系。虽然从点集可唯一计算出簇坐标（如公式10所示），但反过来却不成立：从簇坐标无法唯一恢复出原始点集，因为不同的点集可能具有相同的坐标。因此，如果后续需要重新聚类，必须保存原始点数据。

---

现在我们将点集簇引入本文中考虑的 BA 优化问题中。首先，我们将所有落在同一特征上的点划分为一个点集簇。例如，第 $i$ 个特征的点集簇定义为：

C i ≜ { p i j k ∣ j = 1 , … , M p , k = 1 , … , N i j } C_i \triangleq \{p_{ijk} \mid j = 1, \dots, M_p, k = 1, \dots, N_{ij} \} Ci​≜{pijk​∣j=1,…,Mp​,k=1,…,Nij​}

其点集簇坐标为（根据公式10）：

其中：

• $P_i={\sum }_{j=1}^{M_p}{\sum }_{k=1}^{N_{ij}}{p}_{ijk}{p}_{ijk}^T$
• $v_i={\sum }_{j=1}^{M_p}{\sum }_{k=1}^{N_{ij}}{p}_{ijk}$

接下来，我们展示一个关键结论：该点集簇坐标 $\mathbb{R}(C_i)$ 就完全足够表示 BA 优化中所需的矩阵 $A_i$。由公式 (7) 可得：

$$A_i=\frac{1}{N_i}\sum _{j=1}^{M_p}\sum _{k=1}^{N_{ij}}(p_{ijk}-{\bar{p}}_i)(p_{ijk}-{\bar{p}}_i{)}^T$$

$$=\frac{1}{N_i}\sum _{j=1}^{M_p}\sum _{k=1}^{N_{ij}}{p}_{ijk}{p}_{ijk}^T-{\bar{p}}_i{\bar{p}}_i^T=\frac{1}{N_i}{P}_i-\frac{1}{N_i^2}{v}_i{v}_i^T\triangleq A(C_i)\text{\hspace{1em}(16)}$$

将 $A_i$ 记作函数 $A(C_i)$，强调它是由点集簇坐标唯一确定的。

另一方面，由于点集 $C_i$ 是定义在全局坐标系下的，其坐标 $\mathbb{R}(C_i)$ 显然依赖于雷达的位姿，而该位姿正是我们在优化中要求解的变量。

为了显式表示位姿的影响，我们注意到：

C i = { p i j k ∣ j = 1 , … , M p , k = 1 , … , N i j } (17) C_i = \{p_{ijk} \mid j = 1, \dots, M_p, k = 1, \dots, N_{ij}\} \tag{17} Ci​={pijk​∣j=1,…,Mp​,k=1,…,Nij​}(17)

= ⋃ j = 1 M p { p i j k ∣ k = 1 , … , N i j } (18) = \bigcup_{j=1}^{M_p} \{p_{ijk} \mid k = 1, \dots, N_{ij}\} \tag{18} =j=1⋃Mp​​{pijk​∣k=1,…,Nij​}(18)

根据定义2（簇合并）：

= ⨁ j = 1 M p C i j , 其中  C i j ≜ { p i j k ∣ k = 1 , … , N i j } (19) = \bigoplus_{j=1}^{M_p} C_{ij}, \quad \text{其中 } C_{ij} \triangleq \{p_{ijk} \mid k = 1, \dots, N_{ij}\} \tag{19} =j=1⨁Mp​​Cij​,其中 Cij​≜{pijk​∣k=1,…,Nij​}(19)

根据定义1（刚性变换）：

⨁ j = 1 M p ( T j ∘ C f i j ) , C f i j ≜ { p f i j k ∣ k = 1 , ⋯ , N i j } (20) \bigoplus_{j=1}^{M_p} \left(T_j \circ \mathcal{C}_{fij} \right), \quad \mathcal{C}_{fij} \triangleq \{ \mathbf{p}_{fijk} \mid k = 1, \cdots, N_{ij} \} \tag{20} j=1⨁Mp​​(Tj​∘Cfij​),Cfij​≜{pfijk​∣k=1,⋯,Nij​}(20)

其中，$\mathbf{p}\ast fijk$ 表示在激光雷达局部坐标系下的点（见公式 (2)），$\mathcal{C}\ast ij$ 是由所有在第 $j$ 个激光雷达位姿下观测到的第 $i$ 个特征（平面或边缘）上的点组成的点集，而 $\mathcal{C}\ast fij$ 是与 $\mathcal{C}\ast ij$ 相同的点集，只是表示在激光雷达的局部坐标系中（见图 4）。

点集 $\mathcal{C}\ast i$ 与局部坐标点集 $\mathcal{C}\ast fij$ 之间的关系（如式 (20) 所示）会导致它们的坐标之间的关系如下：

$${\mathcal{C}}_i=\frac{1}{M_p}\sum _{j=1}^{M_p}{\mathcal{C}}_{ij}=\frac{1}{M_p}\sum _{j=1}^{M_p}{T}_j{\mathcal{C}}_{fij}{T}_j^T\text{\hspace{1em}(21)}$$

因此，式 (9) 中的 BA（Bundle Adjustment）优化问题进一步简化为：

$$\underset{T_j\in SE(3),\forall j}{\min }\left(\sum _{i=1}^{M_f}{\lambda }_l\left(\mathcal{A}\left(\sum _{j=1}^{M_p}{T}_j{\mathcal{C}}_{fij}{T}_j^T\right)\right)\right)\text{\hspace{1em}(22)}$$

其中函数 $\mathcal{A}(\cdot )$ 定义见公式 (16)。注意，式 (22) 中的代价函数只需要点簇坐标 $\mathcal{C}\_fij$，而不需要逐点枚举每个点。

点簇坐标 $\mathcal{C}\_fij$ 的计算方法如下（参见式 (10)）：

$${\mathcal{C}}_{fij}=\left[\begin{array}{cc}{\mathbf{P}}_{fij}& {\mathbf{v}}_{fij}\\ {\mathbf{v}}_{fij}^T& N_{ij}\end{array}\right],{\mathbf{P}}_{fij}=\sum _{k=1}^{N_{ij}}{\mathbf{p}}_{fijk}{\mathbf{p}}_{fijk}^T,{\mathbf{v}}_{fij}=\sum _{k=1}^{N_{ij}}{\mathbf{p}}_{fijk}\text{\hspace{1em}(23)}$$

这些可以在特征关联阶段提前构建好，无需等到优化阶段。

特别地，如果第 $j$ 个位姿没有观测到第 $i$ 个特征上的任何点，则 $\mathcal{C}\ast fij=0\ast 4\times 4$，这样在公式 (22) 中，$i$ 对应的代价项就自然不会依赖于第 $j$ 个位姿。

---

定理 3：

给定一个矩阵函数：

$$\mathcal{A}(\mathcal{C})\triangleq {\mathbf{N}}^{-1}\mathbf{P}-{\mathbf{N}}^{-2}\mathbf{v}{\mathbf{v}}^T$$

其中：

$$\mathcal{C}=\left[\begin{array}{cc}\mathbf{P}& \mathbf{v}\\ {\mathbf{v}}^T& N\end{array}\right]\in {\mathbb{S}}^{4\times 4}$$

$\lambda \_l(\mathcal{A})$ 表示 $\mathcal{A}$ 的第 $l$ 大特征值。那么对于任意刚性变换 $T\_0\in SE(3)$，有：

$${\lambda }_l\left(\mathcal{A}\left(T_0\mathcal{C}{T}_0^T\right)\right)={\lambda }_l\left(\mathcal{A}(\mathcal{C})\right),\forall T_0\in SE(3)\text{\hspace{1em}(24)}$$

证明： 见附录 III-D [59]。

---

定理 3 的含义： 对所有位姿 $T\_j,\forall j$ 同时左乘一个相同的刚性变换 $T\_0$ 并不会改变优化目标。因此，这种 BA 优化问题对于全局参考系的变换是不变的——这正是 BA 问题中众所周知的规范自由度（gauge freedom）。

3. 二阶雅可比推导与一致性估计

论文推导出 cluster-based 的二阶闭式雅可比，保留稀疏结构，便于快速构建 Hessian；

如前一节所示，式（22）中的BA（束调整）优化问题与原始形式（1）完全等价，其中每个代价项代表点到平面（或边）的欧氏距离平方。这个平方距离本质上是一个二次优化问题，为了高效求解需要利用代价函数的二阶信息。本节我们推导该代价项的一阶和二阶导数。

不失一般性，我们仅讨论第 $i$ 个特征，它对应一个代价项，形式为

$$c_i(T)={\lambda }_l\left(A\left(\sum _{j=1}^{M_p}{T}_j{C}_{fij}{T}_j^T\right)\right)(25)$$

其中 $C_{fij}\in {\mathbb{R}}^{4\times 4}$ 是预先计算好的矩阵（见式（23））。

为了求代价项（25）关于位姿 $T_j$ 的导数，注意到 $T_j$ 是特殊欧几里得群 $SE(3)$ 的元素，我们用一种特殊的加法——称为盒加（boxplus，符号为 $⊞$）——来参数化它的扰动。对位姿向量

$$T=(\cdots ,T_j,\cdots ),$$

定义 $⊞$ 操作为：

$$T⊞\delta T\triangleq (\cdots ,T_j⊞\delta T_j,\cdots ),(26)$$

其中

$$T_j⊞\delta T_j\triangleq \left(\exp (\lfloor \delta {\varphi }_j\rfloor )R_j,\delta t_j+\exp (\lfloor \delta {\varphi }_j\rfloor )t_j\right),(27)$$

这里 $\delta T=(\cdots ,\delta T_j,\cdots )\in {\mathbb{R}}^{6M_p}$，每个

$$\delta T_j=(\delta {\varphi }_j,\delta t_j)\in {\mathbb{R}}^6,\forall j=1,\dots ,M_p$$

是对应位姿的扰动。

对于标量函数

$$f(T):SE(3)\times \cdots \times SE(3)\to \mathbb{R},$$

记其在输入点 $T_0$ 处关于 $T$ 的一阶导数为

$${\frac{\partial f(T)}{\partial T}|}_{T_0},$$

二阶导数为

$${\frac{{\partial }^{2}f(T)}{\partial T^2}|}_{T_0}.$$

盒加操作使我们能够用扰动 $\delta T$ 参数化函数 $f(\cdot )$ 的输入 $T$：

$$T=T_0⊞\delta T.$$

因为在 $\Vert \delta {\varphi }_j\Vert <\pi$ 时映射是双射，函数 $f(T)$ 可以等价地写成 $f(T_0⊞\delta T)$ 关于 $\delta T$ 的函数。因此，函数 $f(T)$ 在点 $T_0$ 处关于 $T$ 的导数可以定义为函数 $f(T_0⊞\delta T)$ 在 $\delta T=0$ 处的欧式向量导数，即：

$${\frac{\partial f}{\partial T}|}_{T_0}\triangleq {\frac{\partial f(T_0⊞\delta T)}{\partial \delta T}|}_{\delta T=0},(28)$$

$${\frac{{\partial }^{2}f}{\partial T^2}|}_{T_0}\triangleq {\frac{\partial }{\partial \delta T}{\left(\frac{\partial f(T_0⊞\delta T)}{\partial \delta T}\right)}^T|}_{\delta T=0}.(29)$$

对于后续讨论，为简化符号，我们直接用 $T$ 替代参考点 $T_0$，并省略标注。

基于上述导数定义（28）和（29），我们将得到代价项（25）的一阶和二阶导数。

定理4

已知：

(1) 矩阵

$$C_j=\left[\begin{array}{cc}{P}_j& v_j\\ v_j^T& N_j\end{array}\right]\in S_{4\times 4},j=1,\dots ,M_p;$$

(2) 位姿

$$T_j\in SE(3),j=1,\dots ,M_p;$$

(3) 矩阵

$$C=\sum _{j=1}^{M_p}{T}_j{C}_j{T}_j^T=\left[\begin{array}{cc}v& P\\ v^T& N\end{array}\right]\in S_{4\times 4},$$

它是 $C_j$ 的聚合矩阵，定义矩阵函数

$$A(C)\triangleq N^{-1}P-\frac{1}{N^2}v{v}^T\in S_{3\times 3};$$

(4) 函数 ${\lambda }_l(A(C))$，其中 ${\lambda }_l(A)$ 表示矩阵 $A$ 的第 $l$ 个（$l=1,2,3$）最大特征值，特征向量为 $u_l$；

则函数 ${\lambda }_l(A(C))$ 关于位姿 $T$ 的雅可比矩阵 $J_l$ 和 Hessian 矩阵 $H_l$ 为

$$J_l=g_{ll}\in {\mathbb{R}}^{1\times 6M_p},(30)$$

$$H_l=W_l+\sum _{\begin{array}{c}k=1\\ k\ne l\end{array}}^3\frac{2}{{\lambda }_l-{\lambda }_k}{g}_{kl}^T{g}_{kl}\in {\mathbb{R}}^{6M_p\times 6M_p},(31)$$

其中 $g_{ll}$ 是当 $k=l$ 时的 $g_{kl}$；矩阵 $g_{kl}$ 和 $W_l$ 分块表示为

$$g_{kl}=[\cdots ,g_{k{l}_j},\cdots ]\in {\mathbb{R}}^{1\times 6M_p},(32)$$

$$W_l=\left[\begin{array}{ccc}\ddots & ⋮& ⋮\\ \cdots & W_{l_{ij}}& \cdots \\ ⋮& ⋮& \ddots \end{array}\right]\in {\mathbb{R}}^{6M_p\times 6M_p},(33)$$

其块元素 $g_{k{l}_j}\in {\mathbb{R}}^{1\times 6}$，$W_{l_{ij}}\in {\mathbb{R}}^{6\times 6}$，对任意 i , j ∈ { 1 , … , M p } i,j \in \{1,\ldots, M_p\} i,j∈{1,…,Mp​} 定义为：

$$g_{k{l}_j}=\frac{1}{N}{u}_l^{T}SP\left(T_j-\frac{1}{N}CF\right)C_j{T}_j^T{V}_k^T+\frac{1}{N}{u}_k^{T}SP\left(T_j-\frac{1}{N}CF\right)C_j{T}_j^T{V}_l^T,(34)$$

$$W_{l_{ij}}=-\frac{2}{N^2}{V}_l^T{T}_i{C}_{i}F{C}_j{T}_j^T{V}_l^T+{\delta }_{ij}\cdot \left[\frac{N}{2}{V}_l^T{T}_j{C}_j{T}_j^T{V}_l^T+\left[\begin{array}{cc}{K}_j^l& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}\end{array}\right]\right],(35)$$

其中

$$K_j^l=\frac{1}{N}\lfloor SP{T}_j{C}_j{\left(T_j-\frac{1}{N}CF\right)}^T{S}^{T}P{u}_l\rfloor \lfloor u_l\rfloor +N\cdot \lfloor u_l\rfloor \lfloor SP{T}_j{C}_j{\left(T_j-\frac{1}{N}CF\right)}^T{S}^{T}P{u}_l\rfloor ,(36)$$

$$V_l=\left[\begin{array}{cc}-\lfloor u_l\rfloor & 0_{3\times 1}\\ 0_{3\times 3}& u_l\end{array}\right],F=\left[\begin{array}{cc}{0}_{3\times 3}& 0_{3\times 1}\\ 0_{1\times 3}& 1\end{array}\right],(37)$$

$$SP=\left[\begin{array}{cc}{I}_{3\times 3}& 0_{3\times 1}\end{array}\right],{\delta }_{ij}=\{\begin{array}{ll}1,& i=j\\ 0,& i\ne j\end{array}.(38)$$

证明：详见补充材料 III-E [59]。

推论4.1

定理4中的雅可比矩阵 $J_l$ 和 Hessian 矩阵 $H_l$ 满足，对于任意的 $l=1,2,3$，有

$$J_l\cdot \delta T=0,\delta T^T\cdot H_l\cdot \delta T=0,\forall \delta T\in W\triangleq \left\{\left[\begin{array}{c}w\\ w\\ ⋮\end{array}\right]\mid w\in {\mathbb{R}}^6\right\},(39)$$

且

$$J_l^j=0_{1\times 6},\text{若}{C}_j=0,(40)$$

$$H_l^{ij}=0_{6\times 6},\text{若}{C}_i=0\text{或}{C}_j=0,(41)$$

其中，$J_l^j$ 表示矩阵 $J_l$ 的第 $j$ 个列块，$H_l^{ij}$ 表示矩阵 $H_l$ 的第 $i$ 行第 $j$ 列块。

证明：详见补充材料 III-F [59]。

备注3

式（39）意味着雅可比矩阵和 Hessian 矩阵存在零空间，该零空间包含由 $W$ 所张成的空间。实际上，这表示 BA 优化中的代价函数（25）沿着所有位姿被相同扰动量 $w$ 同时扰动的方向是不变的，这与定理3中所述的规范自由（gauge freedom）相符。

---

备注4

式（40）和（41）表明，如果相关的位姿不观测当前特征（即 $C_i=0$ 或 $C_j=0$），则雅可比矩阵和 Hessian 矩阵对应的块是零矩阵，因此可以省略它们的计算。如果一个特征只被稀疏的位姿集合观测，这种稀疏结构能够节省大量计算时间。

---

备注5

定理4中的导数是基于（26）定义的位姿扰动得到的，即扰动 $\delta T$ 乘在当前位姿的左侧（即全局坐标系中的扰动）。如果使用其他扰动定义（记为 $\widetilde{\delta T}$，例如局部坐标系中的扰动以便与 IMU 预积分等其他测量集成），满足关系 $\delta T=L\widetilde{\delta T}$，其中 $L$ 是两种扰动参数化之间的雅可比，则其一阶和二阶导数可分别计算为

$$\tilde{J}=J\cdot L,\tilde{H}=L^T\cdot H\cdot L.$$

由此可见，$\tilde{J}$ 和 $\tilde{H}$ 保持了由 $L\cdot W$ 所张成的零空间，其中 $W$ 定义如（39）。

4. 高效二阶求解器设计

1.基于上述结构设计了专用的二阶 BA 求解器，高效且保持一致性

定理4中给出的雅可比矩阵和海森矩阵是针对一个代价项（式(25)），对应空间中的一个特征点。记第 $i$ 个特征（或代价项）的雅可比矩阵和海森矩阵分别为 $J_i$ 和 $H_i$，为了确定增量更新 $\Delta T$，我们利用总代价函数 $c(T)$ 在式(22)中的二阶近似：

$$c(T⊞\Delta T)\approx c(T)+J\Delta T+\frac{1}{2}\Delta T^{T}H\Delta T$$

其中

$$J=\sum _{i=1}^{M_f}{J}_i,H=\sum _{i=1}^{M_f}{H}_i.$$

对于任意 $d\in W$，由于 $J_{i}d=0$，且 $d^T{H}_{i}d=0,\forall i$，因此有 $Jd=0$ 且 $d^{T}Hd=0$，这意味着沿着 $d\in W$ 的任何额外更新都不会改变该近似。

解决规范自由度（gauge freedom）的一种方式是将第一个位姿固定在其初始值，不在优化过程中改变。也就是说，设定

然后，对（42）中关于 $\Delta T$（排除 $\Delta T_1$）的代价近似求导并设为零，得到最优的 $\Delta T_1=0$。

增量更新的解为：

$$\Delta T^{\ast }=-(H+\mu I{)}^{-1}{J}^T,$$

这里我们使用类似 Levenberg-Marquardt (LM) 算法的方法，通过阻尼参数 $\mu$ 对梯度和牛顿方向进行了加权调整。

完整算法在补充材料（算法1）中有总结，并且时间复杂度分析详见补充材料（第IV节）。整体求解器的复杂度为

$$O(M_f{M}_p+M_f{M}_p^2+M_p^3),$$

其中特征点数量为 $M_f$，位姿数量为 $M_p$，复杂度关于特征点数量是线性的，关于位姿数量是立方的，且与 $M_p$ 无关。式中 $M_f{M}_p$ 和 $M_f{M}_p^2$ 分别对应雅可比矩阵和海森矩阵的计算，而 $M_p^3$ 则是由于（43）中矩阵求逆的计算开销。

---

2.利用二阶信息评估位姿估计的不确定性，保证结果一致性,进行协方差估计

假设求解器收敛到了一个最优位姿 $T^{\ast }$，通常我们需要估计该估计位姿的置信度。令 $T_{gt}$ 为真实位姿（未知），定义估计误差为最优估计 $T^{\ast }$ 与真实位姿 $T_{gt}$ 之间的差异 $\delta T^{\ast }$，满足

$$T_{gt}=T^{\ast }⊞\delta T^{\ast }.$$

目标是估计误差 $\delta T^{\ast }$ 的协方差，记为 ${\Sigma }_{\delta T^{\ast }}$。

最终，估计误差 $\delta T^{\ast }$ 是由每个原始测量点的噪声引起的。记 $p_{gt}^{fijk}$ 为第 $i$ 个特征在第 $j$ 个激光雷达位姿观测到的第 $k$ 个点的真实位置，测量噪声为 $\delta p_{fijk}\sim \mathcal{N}(0,{\Sigma }_{p_{fijk}})$，则测量点位置 $p_{fijk}$ 为：

$$p_{fijk}=p_{gt}^{fijk}+\delta p_{fijk}.(44)$$

将真实点和测量点分别聚合形成真实点簇，记为 $C_{gt}^{fijk}$，测量点簇记为 $C_{fijk}$（见式(23)）：

$$C_{gt}^{fij}=\left[\begin{array}{c}{\sum }_{k=1}^{N_{ij}}{p}_{gt}^{fijk}\\ {\left({\sum }_{k=1}^{N_{ij}}{p}_{gt}^{fijk}\right)}^T{\sum }_{k=1}^{N_{ij}}{p}_{gt}^{fijk}\\ N_{ij}\end{array}\right](45)$$

近似为

$$C_{gt}^{fij}\approx C_{fij}-\delta C_{fij},(46)$$

其中

$$\delta C_{fij}=\sum _{k=1}^{N_{ij}}{B}_{fijk}\delta p_{fijk},\text{详见补充材料 III-G，式(47)},$$

$\delta C_{fij}$ 可以在特征关联阶段与点簇 $C_{fij}$ 一起提前构建。

在以下讨论中，为简化符号，记

C g t f = { C g t f i j } , C f = { C f i j } , δ C f = { δ C f i j } C_{gt}^f = \{ C_{gt}^{fij} \}, \quad C_f = \{ C_{fij} \}, \quad \delta C_f = \{ \delta C_{fij} \} Cgtf​={Cgtfij​},Cf​={Cfij​},δCf​={δCfij​}

分别表示在任意激光雷达位姿上任意特征观测到的所有点簇的真实值、测量值和噪声。

虽然真实位姿 $T_{gt}$ 和点簇 $C_{gt}^f$ 是未知的，但它们实际上是式（22）的最优解，因此在该处计算的雅可比矩阵应该为零，即：

$$J^T(T_{gt},C_{gt}^f)=0(48)$$

这里我们将雅可比矩阵写成位姿和点簇的显式函数。现在，我们用其一阶近似来代替式（48）左边：

$$J^T(T_{gt},C_{gt}^f)=J^T(T^{\ast }⊞\delta T^{\ast },C_f-\delta C_f)=J^T(T^{\ast },C_f)+\frac{\partial J^T(T^{\ast }⊞\delta T,C_f)}{\partial \delta T}\delta T^{\ast }-\frac{\partial J^T(T^{\ast },C_f)}{\partial C_f}\delta C_f.(49)$$

注意到

$$\frac{\partial J^T(T^{\ast }⊞\delta T,C_f)}{\partial \delta T}=H(T^{\ast },C_f),$$

即在 $T^{\ast }$ 处计算的式（22）的海森矩阵（也见式（42）），则有：

$$0=J^T(T_{gt},C_{gt}^f)=J^T(T^{\ast },C_f)+H\cdot \delta T^{\ast }-\frac{\partial J^T(T^{\ast },C_f)}{\partial C_f}\delta C_f,(50)$$

这意味着

$$\delta T^{\ast }=-H^{-1}{J}^T(T^{\ast },C_f)+H^{-1}\frac{\partial J^T(T^{\ast },C_f)}{\partial C_f}\delta C_f.(51)$$

由于 $T^{\ast }$ 是使用测量点簇 $C_f$ 收敛得到的解，它们应当导致零更新，即

$$H^{-1}{J}^T(T^{\ast },C_f)=0$$

（参考式（43）在收敛时阻尼参数 $\mu =0$ 的情况）。

因此，

$$\delta T^{\ast }=H^{-1}\frac{\partial J^T(T^{\ast },C_f)}{\partial C_f}\delta C_f\sim \mathcal{N}(0,{\Sigma }_{\delta T^{\ast }}),(52)$$

其协方差为

$${\Sigma }_{\delta T^{\ast }}=H^{-1}\frac{\partial J^T(T^{\ast },C_f)}{\partial C_f}{\Sigma }_{\delta C_f}{\left(\frac{\partial J^T(T^{\ast },C_f)}{\partial C_f}\right)}^T{H}^{-T}.(53)$$

具体关于

$$\frac{\partial J^T(T^{\ast },C_f)}{\partial C_f}\delta C_f,{\Sigma }_{\delta C_f},\text{以及}{\Sigma }_{\delta T^{\ast }}$$

的推导和结果详见补充材料 III-G [59]。

注意，计算 ${\Sigma }_{\delta T^{\ast }}$ 只需要先前根据式（47）构建好的 $\delta C_{fij}$ 的协方差，避免了在优化过程中枚举每个原始点的开销。

---

四、总结

问题	解决方案
点云数量庞大	通过点簇（cluster）聚合观测
特征变量过多	解析消元 edge/plane 特征
位姿估计不一致	利用二阶信息评估不确定性
优化效率低	保留稀疏 Hessian 结构，设计専用求解器

该论文以数学和工程实证结合的方式，实现了高效、可一致估计的 LiDAR 点云 BA，并为大规模 SLAM 与精细建图提供了新的路径。

---

五、主要创新点与优势总结

创新点	描述
点簇概念	将大量点压缩为低维簇参数，极大减少优化变量数量
解析几何闭式解	无需将几何特征作为优化变量，直接得到最优表达
全面导数推导	提供一阶 + 二阶雅可比，明确 null-space 和稀疏性
高效协方差估计	用二阶信息评估 pose uncertainty，无需逐点操作
实时高效实现	点少也不影响优化时间，适合大规模地图优化

---

参考文献

• Zheng Liu, Xiyuan Liu, Fu Zhang, “Efficient and Consistent Bundle Adjustment on Lidar Point Clouds”, IEEE Trans. Robotics, 2023