SLAM后端处理

本文是学习笔记，原文来自计算机视觉life

背景

拓展卡尔曼滤波

SLAM的后端一般分为两种处理方法，一种是以扩展卡尔曼滤波（EKF）为代表的滤波方法，一种是以图优化为代表的非线性优化方法。不过，目前SLAM研究的主流热点几乎都是基于图优化的。
滤波方法尤其是EKF方法，在SLAM发展很长的一段历史中一直占据主导地位，它的一个大缺点就是存储量和状态量是平方增长关系，因为存储的是协方差矩阵，因此不适合大型场景。而现在基于视觉的SLAM方案，路标点（特征点）数据很大，滤波方法根本吃不消，所以此时滤波的方法效率非常低。

图优化

在图优化里，Bundle Adjustment（后面简称BA）起到了核心作用。但是之前LAM的研究者们发现包含大量特征点和相机位姿的BA计算量其实很大，根本没办法实时。
后来SLAM研究者们发现了其实在视觉SLAM中，虽然包含大量特征点和相机位姿，但其实BA是稀疏的，稀疏的就好办了，就可以加速了啊！比较代表性的就是2009年，几个大神发表了自己的研究成果《SBA：A software package for generic sparse bundle adjustment》，而且计算机硬件发展也很快，因此基于图优化的视觉SLAM也可以实时了！

在SLAM里，图优化一般分解为两个任务：

1、构建图。机器人位姿作为顶点，位姿间关系作为边。

2、优化图。调整机器人的位姿（顶点）来尽量满足边的约束，使得误差最小。

SparseOptimizer

整个图的核心SparseOptimizer 包含一个优化算法（OptimizationAlgorithm）的对象。OptimizationAlgorithm是通过OptimizationWithHessian 来实现的。其中迭代策略可以从Gauss-Newton（高斯牛顿法，简称GN）, Levernberg-Marquardt（简称LM法）, Powell’s dogleg 三者中间选择一个（我们常用的是GN和LM）

迭代策略

高斯牛顿法

优化问题中我们的目标是：找到一个变量x使得目标函数f(x)最小化。

Hessian 矩阵是目标函数 f(x)对变量 x 的二阶偏导数构成的矩阵，定义如下：

在极值点时
$$f^{\prime }(x)=▽f(x_k)+H(x_k)(x-x_k)=0$$

解得：
$$x_{k+1}=x_k-{H{X}_K}^{-1}▽f(x_k)$$

准Newton 方法： 在实际问题中，直接计算和存储 Hessian 矩阵可能过于昂贵。因此，准Newton 方法（如 BFGS 或 L-BFGS）通过近似 Hessian 矩阵或其逆矩阵，降低计算复杂度，同时保持高效性。

• 优势与限制

优势：
利用二阶导数信息，优化过程对目标函数曲率更加敏感，可以更快地收敛到极值。
特别适用于具有良好曲率结构的函数，能够准确找到极值点。

限制：
对于高维问题，计算 Hessian 矩阵的时间和内存复杂度很高。Hessian 矩阵可能不是正定的（例如目标函数的某些区域是凹的），需要特殊处理，如修正Hessian 矩阵或采用拟Hessian 近似。

注：凸函数的曲面是“开口向上”的，凹函数的曲面是“开口向下”的。

LM算法

Levenberg-Marquardt (LM) 是一种优化算法，用于解决非线性最小二乘问题，特别是在机器学习和数据拟合任务中。它结合了梯度下降法和高斯-牛顿法的优点，提供了一种在全局收敛性和局部优化效率之间取得平衡的方法。

核心思想
LM 算法通过在梯度下降和高斯-牛顿之间切换调整搜索方向，从而更有效地最小化非线性问题的残差平方和。其数学核心是解决下面的问题：

算法步骤

1. 初始条件设置：

• 初始参数x0
• 阻尼因子 λ 用于平衡算法的两种模式

2. 构建更新公式： 在每次迭代中，LM 的参数更新公式如下：
   
3. 调整阻尼因子 λ
   如果本次迭代减少了目标函数值（即误差降低），减小 λ（让搜索方向更接近高斯-牛顿法）。
4. 如果本次迭代增加了目标函数值，增大 λ（让搜索方向更接近梯度下降法）。
   更新参数： 使用新的 Δx 更新 x。
5. 检查收敛条件： 如果满足预设的收敛准则（如参数变化量或误差减小量小于阈值），停止迭代；否则返回第 2 步。

优点

结合了梯度下降法的全局收敛性和高斯-牛顿法的快速局部收敛性。
对于模型中的初始参数选择不敏感。
在小数据集的非线性最小二乘问题中非常高效。

缺点

需要计算雅可比矩阵，可能导致计算量较大。
对于非常高维度的优化问题，其效率可能不如其他方法。

应用场景

机器学习模型训练：非线性模型参数的优化，如神经网络训练（在某些早期应用中）。
数据拟合：科学计算中拟合实验数据，例如非线性回归。
图像处理：目标匹配或形状拟合。
总结来说，Levenberg-Marquardt 算法是一种稳健且高效的非线性优化工具，尤其适用于需要快速收敛的中小规模问题。

推导过程