引用

视觉SLAM中Bundle Adjustment（BA）问题旨在同时优化相机位姿和三维地图点，以最小化重投影误差。与之前文章中提到的最小化重投影的区别是：BA同时优化相机位姿和三维空间点，并且最小化k时刻之前所有数据的重投影误差；而视觉里程计中的方法仅优化空间点，且只有两帧之间的数据。

详细推导过程：

1. 问题定义与目标函数

• 重投影误差：
  对于第$i$个相机位姿（参数化为李代数${\mathbf{\xi }}_i\in \mathfrak{se}(3)$）和第$j$个三维点${\mathbf{X}}_j\in {\mathbb{R}}^3$，其投影到图像的观测像素坐标为${\mathbf{u}}_{ij}$。
  投影过程为：
  $${\mathbf{p}}_{ij}=\pi \left(\exp ({\mathbf{\xi }}_i^{\wedge }){\mathbf{X}}_j\right)$$
  其中$\pi (\cdot )$为针孔相机模型，重投影误差为：
  $${\mathbf{e}}_{ij}={\mathbf{u}}_{ij}-{\mathbf{p}}_{ij}$$

• 目标函数：
  最小化所有观测的重投影误差平方和：
  $$\underset{{\mathbf{\xi }}_i,{\mathbf{X}}_j}{\min }\sum _{i,j}||{\mathbf{e}}_{ij}|{|}^2$$

  ---

2. 非线性优化方法

采用高斯-牛顿法或列文伯格-马夸尔特法进行迭代优化，步骤如下：
2.1 误差函数的线性化
对误差项进行一阶泰勒展开：
$${\mathbf{e}}_{ij}({\mathbf{\xi }}_i+\Delta {\mathbf{\xi }}_i,{\mathbf{X}}_j+\Delta {\mathbf{X}}_j)\approx {\mathbf{e}}_{ij}+{\mathbf{J}}_i^{\text{pose}}\Delta {\mathbf{\xi }}_i+{\mathbf{J}}_j^{\text{point}}\Delta {\mathbf{X}}_j$$
其中：

• ${\mathbf{J}}_i^{\text{pose}}=\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{\xi }}_i}$是误差对相机位姿的雅可比矩阵。
• ${\mathbf{J}}_j^{\text{point}}=\frac{\partial {\mathbf{e}}_{ij}}{\partial {\mathbf{X}}_j}$ 是误差对三维点的雅可比矩阵。
  2.2 雅可比矩阵计算
• 相机位姿导数（左扰动模型）：
  设${\mathbf{\xi }}_i$的左扰动为$\Delta \mathbf{\xi }$
  投影点对位姿的导数为：
  $${\mathbf{J}}_i^{\text{pose}}=-\frac{\partial \pi }{\partial \mathbf{p}}\cdot \frac{\partial ({\mathbf{T}}_i{\mathbf{X}}_j)}{\partial \Delta \mathbf{\xi }}{|}_{\Delta \mathbf{\xi }=0}$$
  其中$\frac{\partial \pi }{\partial \mathbf{p}}$为投影函数对三维点的导数，与直接法类似；
  $\frac{\partial ({\mathbf{T}}_i{\mathbf{X}}_j)}{\partial {\mathbf{\xi }}_i}=[I_{3\times 3,-({\mathbf{T}}_i{\mathbf{X}}_J{)}^{\wedge }}]$在直接法等文章中已经推导过。
• 三维点导数：
  $${\mathbf{J}}_j^{\text{point}}=-\frac{\partial \pi }{\partial \mathbf{p}}\cdot {\mathbf{R}}_i$$
  其中${\mathbf{R}}_i$为相机的旋转矩阵。

3. 构建增量方程

将优化变量组合为
Δx={Δξ1,…,Δξm,ΔX1,…,ΔXn}T\Delta\mathbf{x} = \{ \Delta\boldsymbol{\xi}_1, \dots, \Delta\boldsymbol{\xi}_m, \Delta\mathbf{X}_1, \dots, \Delta\mathbf{X}_n \}^TΔx={Δξ1​,…,Δξm​,ΔX1​,…,ΔXn​}T
目标函数变为：
$$\sum _{i,j}||{\mathbf{e}}_{ij}+{\mathbf{J}}_{ij}\Delta \mathbf{x}|{|}^2$$
其中${\mathbf{J}}_{ij}$为对应误差项的雅可比块。
整体增量方程为：$${\mathbf{J}}^T\mathbf{J}\Delta \mathbf{x}=-{\mathbf{J}}^T\mathbf{e}$$
其中$\mathbf{J}$为稀疏雅可比矩阵，$\mathbf{e}$为所有误差项堆叠的向量。

4. 利用稀疏性加速求解

由于每个误差项仅关联一个相机和一个点，${\mathbf{J}}^T\mathbf{J}$具有分块对角结构，可分解为：
$$\mathbf{H}=\left[\begin{array}{cc}{\mathbf{H}}_{\text{cc}}& {\mathbf{H}}_{\text{cp}}\\ {\mathbf{H}}_{\text{pc}}& {\mathbf{H}}_{\text{pp}}\end{array}\right]$$
代入增量方程得到方程组：
$$\{\begin{array}{l}{\mathbf{H}}_{cc}\Delta \xi +{\mathbf{H}}_{cp}\Delta P=-{\mathbf{b}}_c\\ {\mathbf{H}}_{pc}\Delta \xi +{\mathbf{H}}_{pp}\Delta P=-{\mathbf{b}}_p\end{array}$$
通过舒尔补（Schur Complement）消去三维点变量$\Delta P$：
$$\left({\mathbf{H}}_{\text{cc}}-{\mathbf{H}}_{\text{cp}}{\mathbf{H}}_{\text{pp}}^{-1}{\mathbf{H}}_{\text{pc}}\right)\Delta \mathbf{\xi }={\mathbf{b}}_{\text{c}}-{\mathbf{H}}_{\text{cp}}{\mathbf{H}}_{\text{pp}}^{-1}{\mathbf{b}}_{\text{p}}$$
求解后回代得到点变量的增量：
$$\Delta \mathbf{P}={\mathbf{H}}_{\text{pp}}^{-1}\left({\mathbf{b}}_{\text{p}}-{\mathbf{H}}_{\text{pc}}\Delta \mathbf{\xi }\right)$$
这一步将计算复杂度从 $O(n^3)$ 降低到$O(m^3+n)$,( $m$ 为相机数量，$n$ 为地图点数量）

5. 迭代更新与收敛

重复以下步骤直至收敛：

1. 线性化误差：计算雅可比矩阵和残差。
2. 构建并求解增量方程：利用稀疏求解器（如Cholesky分解）高效计算。
3. 更新变量：
   $${\mathbf{\xi }}_i\leftarrow {\mathbf{\xi }}_i+\Delta {\mathbf{\xi }}_i,{\mathbf{X}}_j\leftarrow {\mathbf{X}}_j+\Delta {\mathbf{X}}_j$$

6. 关键优化技巧- 鲁棒核函数：

使用Huber或Cauchy核函数抑制外点影响。

• 参数化选择：三维点可选用逆深度参数化以改善数值稳定性。
• 稀疏求解库：借助Ceres Solver、g2o等工具实现高效计算。

总结

Bundle Adjustment通过非线性优化联合调整相机位姿与三维点，是视觉SLAM后端优化的核心。其核心步骤包括误差建模、雅可比计算、稀疏矩阵分解及迭代更新。理解BA的数学推导与实现细节，对提升SLAM系统的精度与鲁棒性至关重要。