本文是学习笔记，原文来自计算机视觉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 ′ ( x ) = ▽ f ( x k ) + H ( x k ) ( x − x k ) = 0 f'(x)=\triangledown f({x}_{k})+H({x}_{k})(x-{x}_{k})=0 f′(x)=▽f(xk)+H(xk)(x−xk)=0
解得：
x k + 1 = x k − H X K − 1 ▽ f ( x k ) {x}_{k+1} = {x}_{k}-{H{X}_{K}}^{-1}\bigtriangledown f({x}_{k}) xk+1=xk−HXK−1▽f(xk)
准Newton 方法： 在实际问题中，直接计算和存储 Hessian 矩阵可能过于昂贵。因此，准Newton 方法（如 BFGS 或 L-BFGS）通过近似 Hessian 矩阵或其逆矩阵，降低计算复杂度，同时保持高效性。
- 优势与限制
优势：
利用二阶导数信息，优化过程对目标函数曲率更加敏感，可以更快地收敛到极值。
特别适用于具有良好曲率结构的函数，能够准确找到极值点。
限制：
对于高维问题，计算 Hessian 矩阵的时间和内存复杂度很高。Hessian 矩阵可能不是正定的（例如目标函数的某些区域是凹的），需要特殊处理，如修正Hessian 矩阵或采用拟Hessian 近似。
注：凸函数的曲面是“开口向上”的，凹函数的曲面是“开口向下”的。
LM算法
Levenberg-Marquardt (LM) 是一种优化算法，用于解决非线性最小二乘问题，特别是在机器学习和数据拟合任务中。它结合了梯度下降法和高斯-牛顿法的优点，提供了一种在全局收敛性和局部优化效率之间取得平衡的方法。
核心思想
LM 算法通过在梯度下降和高斯-牛顿之间切换调整搜索方向，从而更有效地最小化非线性问题的残差平方和。其数学核心是解决下面的问题：
算法步骤
- 初始条件设置：
- 初始参数x0
- 阻尼因子 λ 用于平衡算法的两种模式
- 构建更新公式： 在每次迭代中，LM 的参数更新公式如下：
- 调整阻尼因子 λ
如果本次迭代减少了目标函数值（即误差降低），减小 λ（让搜索方向更接近高斯-牛顿法）。 - 如果本次迭代增加了目标函数值，增大 λ（让搜索方向更接近梯度下降法）。
更新参数： 使用新的 Δx 更新 x。 - 检查收敛条件： 如果满足预设的收敛准则（如参数变化量或误差减小量小于阈值），停止迭代；否则返回第 2 步。
优点
结合了梯度下降法的全局收敛性和高斯-牛顿法的快速局部收敛性。
对于模型中的初始参数选择不敏感。
在小数据集的非线性最小二乘问题中非常高效。
缺点
需要计算雅可比矩阵，可能导致计算量较大。
对于非常高维度的优化问题，其效率可能不如其他方法。
应用场景
机器学习模型训练：非线性模型参数的优化，如神经网络训练（在某些早期应用中）。
数据拟合：科学计算中拟合实验数据，例如非线性回归。
图像处理：目标匹配或形状拟合。
总结来说，Levenberg-Marquardt 算法是一种稳健且高效的非线性优化工具，尤其适用于需要快速收敛的中小规模问题。
推导过程
