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_init

while not converged:
    r = residual(x)
    J = jacobian(x)
    H = J^T * J
    g = J^T * r

    solve (H + lambda * diag(H)) * dx = -g

    if cost(x + dx) < cost(x):
        x = x + dx
        lambda = lambda / factor
    else:
        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 
#include 
#include 

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 cost
        double 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$