视觉SLAM中的Bundle Adjustment问题详解

视觉SLAM里的Bundle Adjustment问题

Bundle Adjustment(BA)是视觉SLAM中用于同时优化相机位姿和三维点位置的核心算法。其本质是一个大规模非线性优化问题：

• 状态量初始值：包括所有关键帧的相机位姿（6自由度，3个旋转+3个平移参数）和所有观测到的特征点的三维坐标（3D位置）
• 系统测量值：特征点在不同图像坐标系中的像素坐标（u,v），通常来自特征匹配的结果

问题描述

如何从带噪声的观测数据中估计相机位姿和三维点坐标的最优值？具体来说，给定：

1. m个相机位姿参数：R_i, t_i (i=1,...,m)
2. n个三维点坐标：P_j (j=1,...,n)
3. 观测数据：z_ij表示第i个相机观测到第j个点的像素坐标

解决方式

构建重投影误差函数，利用非线性最小二乘方法得到状态量的最优估计。误差函数通常表示为： E = Σ||z_ij - π(R_iP_j + t_i)||² 其中π是相机投影模型函数。

最小二乘基础概念

最小二乘问题的数学表述：找到一个n维变量x* ∈ Rⁿ，使得损失函数F(x)取局部最小值： F(x) = 1/2 Σ(f_i(x))² (i=1到m) 其中：

• f_i是残差函数（如测量值与预测值之差）
• 通常m ≥ n（观测数量多于待估参数）
• 局部最小值指对任意||x-x*|| < δ有F(x*) ≤ F(x)

损失函数泰勒展开

假设F(x)二阶可导且平滑，在x附近二阶泰勒展开： F(x+Δx) ≈ F(x) + JΔx + 1/2 ΔxᵀHΔx 其中：

• J是Jacobian矩阵（一阶导数）
• H是Hessian矩阵（二阶导数）
• O(||Δx||³)表示高阶小量

损失函数性质分析

在稳定点x_s（导数为0）处：

1. H正定（所有特征值>0）→ x_s是局部极小值
2. H负定（所有特征值<0）→ x_s是局部极大值
3. H不定（有正有负特征值）→ x_s是鞍点

迭代下降法求解

基本思路

通过迭代逐步减小损失函数：

1. 寻找下降方向d（单位向量）
2. 确定步长α
3. 更新：x_{k+1} = x_k + αd

一阶泰勒近似条件：Jd < 0 步长通过线搜索确定：α* = argmin F(x+αd)

最速下降法

特点：初期收敛快，后期震荡 下降方向：d = -Jᵀ/||J||（负梯度方向） 缺点：在最优值附近出现锯齿形震荡，收敛速度慢

牛顿法

特点：最优值附近收敛快 利用二阶导数信息： Δx = -H⁻¹Jᵀ 优点：二次收敛性 缺点：需要计算和存储Hessian矩阵，计算量大

阻尼法（Levenberg-Marquardt）

结合最速下降和牛顿法的优点： (H + λI)Δx = -Jᵀ 其中λ是阻尼因子：

• λ大→接近最速下降
• λ小→接近牛顿法

非线性最小二乘

Gauss-Newton法

针对最小二乘问题的简化： 用JᵀJ近似Hessian矩阵 求解：(JᵀJ)Δx = -Jᵀf

Levenberg-Marquardt(LM)法

改进的Gauss-Newton： (JᵀJ + λI)Δx = -Jᵀf 自适应调整λ保证矩阵正定

鲁棒核函数的实现

为处理外点，引入鲁棒核函数ρ(·)： E = Σ ρ(||f_i(x)||²) 常用核函数：

• Huber
• Cauchy
• Tukey

VIO残差函数的构建

1. 视觉重投影误差

视觉重投影误差定义了特征点在相机坐标系下的观测值与预测值之间的差异。具体表达式为：

e_vis = z_ij - π(T_cw * P_w)

其中：

• z_ij：特征点在图像坐标系下的实际观测像素坐标（u,v），通过特征检测（如SIFT、ORB等）获得
• π()：相机投影函数，将3D点投影到2D图像平面。对于针孔相机模型：π([X,Y,Z]^T) = [f_xX/Z + c_x, f_yY/Z + c_y]^T
• T_cw：相机位姿的SE(3)变换矩阵，包含旋转R_cw和平移t_cw
• P_w：世界坐标系下的3D地图点坐标

实际应用中，通常会采用归一化平面坐标或使用逆深度参数化来改善数值稳定性。

2. IMU测量值积分

IMU测量值通过预积分技术进行处理，避免重复积分：

预积分模型：

ΔR, Δv, Δp = ∫(ω, a) dt

其中ω和a分别表示陀螺仪和加速度计的原始测量值。

IMU误差由三部分组成：

e_imu = [e_R, e_v, e_p]

具体定义：

• e_R：旋转误差，log(ΔR^T * R_{ij}^T * R_i * R_j)
• e_v：速度误差，v_j - v_i - gΔt - Δv
• e_p：位置误差，p_j - p_i - v_iΔt - 0.5gΔt^2 - Δp

预积分过程需要考虑IMU零偏（bias）的影响，通常采用中值积分法进行离散化处理。

3. 状态误差线性递推公式

基于误差随时间变化的递推方程：

δx_{k+1} = F_k δx_k + G_k n_k

其中：

• F：状态转移矩阵（15×15），描述误差状态的传播规律，包含：

  • 姿态误差（3×3）
  • 速度误差（3×3）
  • 位置误差（3×3）
  • IMU零偏误差（6×6）

• G：噪声Jacobian矩阵（15×12），将噪声映射到状态空间

• n：噪声向量（12×1），包含：

  • 陀螺仪噪声（3×1）
  • 加速度计噪声（3×1）
  • 陀螺仪随机游走（3×1）
  • 加速度计随机游走（3×1）

该递推公式用于EKF-SLAM中的协方差矩阵传播，或在优化框架中构建IMU误差的Jacobian。在实际实现时，需要考虑离散化处理（如零阶保持或一阶近似）。