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

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

---

一、Levenberg-Marquardt算法基本原理

1.1 问题定义

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

$$\underset{\mathbf{x}}{\min }f(\mathbf{x})=\frac{1}{2}\sum _{i=1}^m{r}_i(\mathbf{x}{)}^2=\frac{1}{2}\Vert \mathbf{r}(\mathbf{x}){\Vert }^2$$

其中，

• $\mathbf{x}\in {\mathbb{R}}^n$：参数向量
• $\mathbf{r}(\mathbf{x})=[r_1(\mathbf{x}),\dots ,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}$$

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

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

代入目标函数：

$$\underset{\Delta \mathbf{x}}{\min }\frac{1}{2}\Vert \mathbf{r}+J\Delta \mathbf{x}{\Vert }^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}$

• 计算新误差 $\Vert \mathbf{r}({\mathbf{x}}_{\text{new}}){\Vert }^2$

• 如果误差变小，接受更新，并降低 $\lambda$：

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

• 否则，拒绝更新，提高阻尼系数以减小步长：

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

---

步骤 6：检查收敛条件

• 如果满足以下任一条件，则终止：

  • 残差变化非常小：$\Vert \Delta \mathbf{x}\Vert <ϵ$
  • 最大迭代次数达到
  • 梯度足够小：$\Vert J^T\mathbf{r}\Vert <ϵ$

• 否则返回步骤 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=a{x}^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))$）

• 梯度与步长判断条件：

  • 使用 $\Vert \Delta \mathbf{x}\Vert <1e-6$ 或 $\Vert g\Vert <1e-6$