Levenberg-Marquardt算法详解和C++代码示例

Levenberg-Marquardt（LM）算法是非线性最小二乘问题中常用的一种优化算法，它融合了高斯-牛顿法和梯度下降法的优点，在数值计算与SLAM、图像配准、机器学习等领域中应用广泛。

一、Levenberg-Marquardt算法基本原理

1.1 问题定义

我们希望最小化一个非线性残差平方和目标函数：

$\min_{\mathbf{x}} \, f(\mathbf{x}) = \frac{1}{2} \sum_{i=1}^m r_i(\mathbf{x})^2 = \frac{1}{2} \| \mathbf{r}(\mathbf{x}) \|^2$

其中，

$\mathbf{x} \in \mathbb{R}^n$ ：参数向量
$\mathbf{r}(\mathbf{x}) = [r_1(\mathbf{x}), \ldots, r_m(\mathbf{x})]^T$ ：残差向量

我们要最小化的是残差的平方和。

二、高斯-牛顿法回顾

在当前点 $\mathbf{x}_k$ 处，对残差函数进行一阶泰勒展开：

$\mathbf{r}(\mathbf{x}_k + \Delta \mathbf{x}) \approx \mathbf{r}(\mathbf{x}_k) + J(\mathbf{x}_k) \Delta \mathbf{x}$

其中 $\in \mathbb{R}^{m \times n}$ 是 Jacobian：

$J_{ij} = \frac{\partial r_i}{\partial x_j}$

代入目标函数：

$\min_{\Delta \mathbf{x}} \frac{1}{2} \| \mathbf{r} + J \Delta \mathbf{x} \|^2$

导出正规方程（Normal Equation）：

$J^T J \Delta \mathbf{x} = - J^T \mathbf{r}$

这就是高斯-牛顿法。

三、LM算法推导：阻尼的高斯-牛顿

LM法通过引入一个阻尼因子 $\lambda$ 来平衡 Gauss-Newton 与 Gradient Descent：

$(J^T J + \lambda I) \Delta \mathbf{x} = - J^T \mathbf{r}$

当 $\lambda \to 0$ ，接近高斯-牛顿法；
当 $\lambda \to \infty$ ，趋于梯度下降法。

为了更稳定地调整 $\lambda$ ，可以采用如下对角矩阵：

$(J^T J + \lambda \cdot \text{diag}(J^T J)) \Delta \mathbf{x} = - J^T \mathbf{r}$

这种处理使 LM 更具有数值稳定性。

四、LM算法伪代码

x = x0
lambda = lambda_initwhile not converged:r = residual(x)J = jacobian(x)H = J^T * Jg = J^T * rsolve (H + lambda * diag(H)) * dx = -gif cost(x + dx) < cost(x):x = x + dxlambda = lambda / factorelse:lambda = lambda * factor

五、Levenberg-Marquardt 算法实现步骤

步骤 1：初始化

初始化参数向量 $\mathbf{x}_0$
设置初始阻尼系数 $\lambda$ ，通常取 $10^{-3} \sim 10^{-1}$
设置调整系数（如增长因子 $\nu = 2$ ）
设置收敛条件（如最大迭代次数、步长阈值、误差阈值）

步骤 2：计算当前残差与 Jacobian

在当前参数 $\mathbf{x}$ 处计算残差向量 $\mathbf{r}(\mathbf{x})$
计算残差的 Jacobian 矩阵 $J(\mathbf{x})$

步骤 3：构建 LM 修正的正规方程

构造修正的线性系统：

$(J^T J + \lambda \cdot \text{diag}(J^T J)) \Delta \mathbf{x} = -J^T \mathbf{r}$

$J^T J$ ：近似 Hessian 矩阵
$\lambda \cdot \text{diag}(J^T J)$ ：用于平滑（阻尼），避免步长过大

步骤 4：求解增量 $\Delta \mathbf{x}$

使用 Cholesky / LDLT 分解求解线性方程组，得到参数增量 $\Delta \mathbf{x}$
可选：添加线性求解器条件数检查以保证稳定性

步骤 5：评估新参数点

更新新参数： $\mathbf{x}_{\text{new}} = \mathbf{x} + \Delta \mathbf{x}$
计算新误差 $\| \mathbf{r}(\mathbf{x}_{\text{new}}) \|^2$
如果误差变小，接受更新，并降低 $\lambda$ ：

$\lambda \leftarrow \lambda / \text{factor}$
否则，拒绝更新，提高阻尼系数以减小步长：

$\lambda \leftarrow \lambda \times \text{factor}$

步骤 6：检查收敛条件

如果满足以下任一条件，则终止：
- 残差变化非常小： $\| \Delta \mathbf{x} \| < \epsilon$
- 最大迭代次数达到
- 梯度足够小： $\| J^T \mathbf{r} \| < \epsilon$
否则返回步骤 2，继续迭代

六、总结为流程图结构

初始化 x, lambda↓
计算残差 r(x), Jacobian J↓
构建系统 (JᵀJ + λI)Δx = -Jᵀr↓
求解 Δx↓
计算新误差 cost(x + Δx)↓
误差减少？┌─────────────┐↓             ↓
Yes           No
↓              ↓
x ← x + Δx     λ ← λ × factor
λ ← λ / factor ↓↓
满足终止条件？↓Yes → 退出No  → 回到迭代

七、应用示例：拟合二次函数 $y = ax^2 + bx + c$

我们以拟合二次函数为例，给定数据点 $x_i, y_i)$ ，最小化以下残差：

$r_i(a, b, c) = y_i - (a x_i^2 + b x_i + c)$

Jacobian：

$J_i = \left[ -x_i^2, -x_i, -1 \right]$

八、C++代码实现

#include <iostream>
#include <vector>
#include <Eigen/Dense>using namespace Eigen;
using namespace std;struct DataPoint {double x, y;
};struct LMResult {Vector3d params;double final_cost;int iterations;
};LMResult LevenbergMarquardt(const vector<DataPoint>& data, Vector3d init, int max_iter = 100) {Vector3d x = init;double lambda = 1e-3;double v = 2.0;int n = data.size();double last_cost = 1e20;for (int iter = 0; iter < max_iter; ++iter) {MatrixXd J(n, 3);VectorXd r(n);for (int i = 0; i < n; ++i) {double xi = data[i].x;double yi = data[i].y;double yi_est = x(0) * xi * xi + x(1) * xi + x(2);r(i) = yi - yi_est;J(i, 0) = -xi * xi;J(i, 1) = -xi;J(i, 2) = -1.0;}Matrix3d H = J.transpose() * J;Vector3d g = J.transpose() * r;Matrix3d H_lm = H + lambda * H.diagonal().asDiagonal();Vector3d dx = H_lm.ldlt().solve(-g);Vector3d x_new = x + dx;// compute new costdouble new_cost = 0.0;for (int i = 0; i < n; ++i) {double xi = data[i].x;double yi = data[i].y;double yi_est = x_new(0) * xi * xi + x_new(1) * xi + x_new(2);double ri = yi - yi_est;new_cost += ri * ri;}if (new_cost < last_cost) {x = x_new;lambda *= 0.8;last_cost = new_cost;} else {lambda *= 2.0;}if (dx.norm() < 1e-6) break;}return {x, last_cost, max_iter};
}

九、输出与测试

int main() {vector<DataPoint> data;for (int i = 0; i <= 10; ++i) {double x = i;double y = 2.0 * x * x + 3.0 * x + 1.0 + ((rand() % 100) / 50.0 - 1.0); // 加噪声data.push_back({x, y});}Vector3d init(0.0, 0.0, 0.0);auto result = LevenbergMarquardt(data, init);cout << "Estimated parameters: " << result.params.transpose() << endl;cout << "Final cost: " << result.final_cost << endl;return 0;
}

十、总结

方法	特点
梯度下降法	收敛稳定但慢
高斯-牛顿法	快速但易发散
Levenberg-Marquardt	二者结合，自动调节，收敛稳定

实用建议

阻尼初始化值 $\lambda$ ：设置为初始 Hessian 的最大对角元素的某个比例（如 $\lambda = \tau \cdot \max(\text{diag}(J^T J))$ ）
梯度与步长判断条件：
- 使用 $\| \Delta \mathbf{x} \| < 1e{-6}$ 或 $g \| < 1e{-6}$