凸优化常用解法（GD / Newton / LM）在 BA、PnP、ICP 中的应用？

好的，我们来深入探讨凸优化中的经典迭代方法（梯度下降、牛顿法、LM法）及其在计算机视觉核心问题（BA、PnP、ICP）中的具体应用。

核心概念

非线性最小二乘（Non-linear Least Squares）是许多计算机视觉问题的数学本质，其目标是寻找一组参数 $\mathbf{x}$，使得一系列非线性残差函数 $\mathbf{f}_i(\mathbf{x})$ 的平方和最小。其目标函数可以写为： $F(\mathbf{x}) = \frac{1}{2} \sum_{i=1}^{m} \| \mathbf{f}_i(\mathbf{x}) \|^2 = \frac{1}{2} \| \mathbf{f}(\mathbf{x}) \|^2$ 梯度下降法（Gradient Descent, GD）、牛顿法（Newton's Method）和列文伯格-马夸尔特方法（Levenberg-Marquardt, LM）是求解此类问题的核心迭代优化算法。GD 是一阶方法，仅使用梯度信息；牛顿法是二阶方法，同时使用梯度和二阶导数（Hessian 矩阵）；LM 算法则是专门为最小二乘问题设计的一种信赖域（Trust Region）方法，它巧妙地融合了 GD 和高斯-牛顿法（Gauss-Newton, GN，牛顿法的一种近似）的优点。

原理与推导

假设我们当前对解的估计是 $\mathbf{x}_k$，我们希望找到一个更新量 $\Delta\mathbf{x}$，使得 $F(\mathbf{x}_k + \Delta\mathbf{x})$ 最小。我们将目标函数 $F(\mathbf{x})$ 在 $\mathbf{x}_k$ 处进行泰勒展开： $F(\mathbf{x}_k + \Delta\mathbf{x}) \approx F(\mathbf{x}_k) + \nabla F(\mathbf{x}_k)^T \Delta\mathbf{x} + \frac{1}{2} \Delta\mathbf{x}^T \mathbf{H}(\mathbf{x}_k) \Delta\mathbf{x}$ 其中，$\nabla F(\mathbf{x})$ 是梯度（Gradient），$\mathbf{H}(\mathbf{x})$ 是海森矩阵（Hessian Matrix）。对于最小二乘问题 $F(\mathbf{x}) = \frac{1}{2} \|\mathbf{f}(\mathbf{x})\|^2$，其梯度和海森矩阵分别为：

• 梯度: $\nabla F(\mathbf{x}) = \mathbf{J}(\mathbf{x})^T \mathbf{f}(\mathbf{x})$
• 海森矩阵: $\mathbf{H}(\mathbf{x}) = \mathbf{J}(\mathbf{x})^T \mathbf{J}(\mathbf{x}) + \sum_{i=1}^{m} f_i(\mathbf{x}) \mathbf{H}_i(\mathbf{x})$

其中 $\mathbf{J}(\mathbf{x})$ 是残差函数 $\mathbf{f}(\mathbf{x})$ 的雅可比矩阵（Jacobian Matrix），$\mathbf{H}_i(\mathbf{x})$ 是第 $i$ 个残差分量 $f_i(\mathbf{x})$ 的海森矩阵。

1. 梯度下降法 (Gradient Descent)

GD 只使用一阶泰勒展开，即 $F(\mathbf{x}_k + \Delta\mathbf{x}) \approx F(\mathbf{x}_k) + \nabla F(\mathbf{x}_k)^T \Delta\mathbf{x}$。为了使 $F$ 下降最快，$\Delta\mathbf{x}$ 应取在负梯度方向。

• 更新规则: $\Delta\mathbf{x} = -\alpha \nabla F(\mathbf{x}_k) = -\alpha \mathbf{J}(\mathbf{x}_k)^T \mathbf{f}(\mathbf{x}_k)$
• 动机: 沿最陡峭的下坡方向走一小步。$\alpha$ 是步长（学习率）。
• 复杂度: 每次迭代主要计算雅可比 $\mathbf{J}$ 和残差 $\mathbf{f}$，时间复杂度主要由雅可比矩阵的计算和矩阵向量乘法决定。
• 直观解释: 就像一个盲人下山，每次都沿着脚下最陡的方向走一小步。这种方法简单，但如果路径是狭长的山谷，会导致收敛非常缓慢，在谷底来回震荡。

2. 牛顿法 (Newton's Method)

牛顿法使用二阶泰勒展开，并通过令展开后函数的导数为零来求解最优的 $\Delta\mathbf{x}$。 $\nabla F(\mathbf{x}_k + \Delta\mathbf{x}) \approx \nabla F(\mathbf{x}_k) + \mathbf{H}(\mathbf{x}_k) \Delta\mathbf{x} = 0$

• 更新规则: $\mathbf{H}(\mathbf{x}_k) \Delta\mathbf{x} = -\nabla F(\mathbf{x}_k)$
• 动机: 用一个二次曲面去局部近似目标函数，然后直接跳到这个二次曲面的顶点。
• 困难:
  1. Hessian 计算复杂: $\mathbf{H}(\mathbf{x})$ 的第二项 $\sum f_i \mathbf{H}_i$ 包含二阶导数，计算非常困难。
  2. Hessian 可能非正定: 如果 $\mathbf{H}$ 不是正定矩阵，那么求解出的 $\Delta\mathbf{x}$ 可能不是下降方向，导致迭代发散。

3. 高斯-牛顿法 (Gauss-Newton, GN) 与 LM 法

高斯-牛顿法 是对牛顿法的一种实用近似，专门用于求解最小二乘问题。

• 核心近似: 假设残差 $\mathbf{f}(\mathbf{x})$ 足够小，或者其非线性程度较弱，我们可以忽略 Hessian 中的第二项。 $\mathbf{H}(\mathbf{x}) \approx \mathbf{J}(\mathbf{x})^T \mathbf{J}(\mathbf{x})$ 这个近似的 $\mathbf{H}$ 记为 $\mathbf{H}_{GN}$。它天然是半正定的，如果 $\mathbf{J}$ 列满秩，则为正定。
• GN 更新规则: $(\mathbf{J}^T \mathbf{J}) \Delta\mathbf{x} = -\mathbf{J}^T \mathbf{f}$
• GN 的问题: 如果 $\mathbf{J}^T \mathbf{J}$ 奇异或接近奇异（例如，雅可比矩阵的列线性相关），求解会非常不稳定，$\Delta\mathbf{x}$ 可能会过大，导致发散。

列文伯格-马夸尔特法 (Levenberg-Marquardt, LM) 解决了 GN 的不稳定性问题。

• LM 更新规则: $(\mathbf{J}^T \mathbf{J} + \lambda \mathbf{I}) \Delta\mathbf{x} = -\mathbf{J}^T \mathbf{f}$
• 动机与直观解释:
  • LM 引入了一个阻尼项（damping factor）$\lambda \mathbf{I}$。这可以看作是一种信赖域（Trust Region）思想的体现。
  • 当 $\lambda$ 很小（接近 0）时，LM 方程近似于 GN 方程，算法表现得像高斯-牛顿法，收敛速度快。
  • 当 $\lambda$ 很大时，$\mathbf{J}^T \mathbf{J}$ 相比 $\lambda \mathbf{I}$ 可以忽略，方程近似为 $\lambda \mathbf{I} \Delta\mathbf{x} = -\mathbf{J}^T \mathbf{f}$，即 $\Delta\mathbf{x} \approx -\frac{1}{\lambda} \mathbf{J}^T \mathbf{f}$。这本质上是一个步长很小的梯度下降。
  • 自适应调节 $\lambda$: LM 算法的核心在于动态调整 $\lambda$。如果当前步 $\Delta\mathbf{x}$ 使得总误差 $F(\mathbf{x})$ 显著下降，说明当前二次近似效果好，可以减小 $\lambda$，让算法更偏向 GN 以加速收敛。如果误差上升或下降不明显，说明当前近似不好，需要增大 $\lambda$，让算法更偏向 GD，步子更小更保守。
• 复杂度: 每次迭代需要求解一个线性方程组。对于稠密问题，复杂度为 $O(N^3)$，其中 $N$ 是参数数量。但在 BA 等稀疏问题中，可以利用稀疏性大大降低计算量。

在 BA, PnP, ICP 中的应用

• PnP (Perspective-n-Point):

  • 目标: 求解相机 6-DoF 位姿（3 个旋转，3 个平移）。参数量 $N=6$，非常小。
  • 残差: 3D 点在当前估计位姿下的重投影坐标与观测到的 2D 图像坐标之间的差（重投影误差）。
  • 适用算法: LM 是标准且最高效的选择。因为参数量小，雅可比矩阵 $\mathbf{J}$ 是稠密的，但尺寸不大（例如，对于 $k$ 个点，$\mathbf{J}$ 的尺寸是 $2k \times 6$）。$\mathbf{J}^T\mathbf{J}$ 是一个 $6 \times 6$ 的小矩阵，求逆或求解线性方程组的计算成本极低。GD 在此问题上收敛太慢，没有实用价值。

• ICP (Iterative Closest Point):

  • 目标: 求解一个点云到另一个点云的刚体变换（6-DoF 位姿）。
  • 残差: 对应点对之间的距离。在 point-to-plane ICP 变种中，残差是源点到目标点所在平面的距离。
  • 适用算法:
    1. 闭式解: 对于 point-to-point ICP，在确定了点对匹配后，求解最优位姿的最小二乘问题有一个基于 SVD 的闭式解。
    2. 迭代法: 对于 point-to-plane 等变种，或者当使用非欧式距离的度量时，通常需要迭代优化。LM 是常用方法，参数量同样是 6。

• BA (Bundle Adjustment):

  • 目标: 同时优化所有相机位姿和所有三维地图点。参数量巨大，可达数万甚至数百万维。
  • 残差: 依然是重投影误差。
  • 适用算法: LM 是绝对的主流，但必须利用问题的稀疏结构。
    • 稀疏性: BA 的雅可比矩阵是高度稀疏的。因为一个地图点通常只在少数几个相机帧中被看到，所以雅可比矩阵中只有相机参数和其看到的地图点参数对应的块才非零。
    • Schur 补 (Schur Complement): 直接求解巨大的 LM 方程 $(\mathbf{H} + \lambda \mathbf{I}) \Delta\mathbf{x} = \mathbf{g}$ 是不可行的。通过将参数分为相机位姿和地图点两部分，可以利用 Schur 补技巧，先求解相机位姿的更新量，然后反代入求解地图点的更新量。这极大地降低了求解线性方程组的计算复杂度，是所有现代 BA 库（如 Ceres, g2o）的核心。
    • GD 的角色: 尽管 LM 是主流，但在超大规模问题或需要快速得到一个粗略解的场景下，基于 GD 的变种（如 SGD）有时也会被使用，特别是在深度学习与 SLAM 结合的领域（如 BA-Net）。

代码实现

下面用 Python 和 NumPy 从零实现 GD, GN, LM 算法，来求解一个简单的非线性最小二乘问题：拟合曲线 $y = \exp(ax^2 + bx + c)$。

 1import numpy as np
 2import matplotlib.pyplot as plt
 3
 4# --- 1. 问题定义与数据生成 ---
 5# 定义非线性模型函数
 6def model_func(params, x):
 7    a, b, c = params
 8    return np.exp(a * x**2 + b * x + c)
 9
10# 定义真实参数
11true_params = np.array([0.3, -1.5, 1.0])
12N_points = 100
13x_data = np.linspace(-3, 3, N_points)
14# 生成带噪声的观测数据
15y_data = model_func(true_params, x_data) + np.random.normal(0, 0.5, N_points)
16
17# 初始参数猜测
18initial_params = np.array([0.1, -1.0, 0.5])
19
20# --- 2. 求解器实现 ---
21
22# 计算残差和雅可比矩阵
23def compute_residual_and_jacobian(params, x, y):
24    # 这是求解的核心，需要手动推导雅可比
25    # f(p) = exp(p0*x^2 + p1*x + p2) - y_obs
26    # d(f)/d(p0) = exp(...) * x^2
27    # d(f)/d(p1) = exp(...) * x
28    # d(f)/d(p2) = exp(...) * 1
29    a, b, c = params
30    y_pred = model_func(params, x)
31    residual = y_pred - y
32    
33    J = np.zeros((len(x), 3))
34    J[:, 0] = y_pred * x**2
35    J[:, 1] = y_pred * x
36    J[:, 2] = y_pred
37    
38    return residual, J
39
40# 梯度下降法 (GD)
41def solve_gd(x_data, y_data, initial_params, iterations=100, lr=1e-7):
42    params = initial_params.copy()
43    history = [params.copy()]
44    for i in range(iterations):
45        # 为什么这样做: 计算当前参数下的残差和雅可比
46        residual, J = compute_residual_and_jacobian(params, x_data, y_data)
47        # 为什么这样做: 梯度是 J^T * r
48        gradient = J.T @ residual
49        # 为什么这样做: 沿负梯度方向更新参数
50        params -= lr * gradient
51        history.append(params.copy())
52    return params, history
53
54# 高斯-牛顿法 (GN)
55def solve_gn(x_data, y_data, initial_params, iterations=20):
56    params = initial_params.copy()
57    history = [params.copy()]
58    for i in range(iterations):
59        residual, J = compute_residual_and_jacobian(params, x_data, y_data)
60        # 为什么这样做: 构建 H = J^T * J 和 g = J^T * r
61        H = J.T @ J
62        g = J.T @ residual
63        # 为什么这样做: 求解线性方程 H * delta_p = -g
64        try:
65            delta_p = np.linalg.solve(H, -g)
66        except np.linalg.LinAlgError:
67            # 为什么这样做: 如果H是奇异的，GN会失败，这里用伪逆作为备用方案
68            delta_p = np.linalg.pinv(H) @ -g
69        params += delta_p
70        history.append(params.copy())
71        if np.linalg.norm(delta_p) < 1e-6:
72            break
73    return params, history
74
75# 列文伯格-马夸尔特法 (LM)
76def solve_lm(x_data, y_data, initial_params, iterations=20, initial_lambda=1e-2):
77    params = initial_params.copy()
78    history = [params.copy()]
79    lmbda = initial_lambda
80    nu = 2  # 用于控制lambda增减的因子
81
82    residual, J = compute_residual_and_jacobian(params, x_data, y_data)
83    current_cost = 0.5 * np.sum(residual**2)
84
85    for i in range(iterations):
86        residual, J = compute_residual_and_jacobian(params, x_data, y_data)
87        H = J.T @ J
88        g = J.T @ residual
89
90        while True:
91            # 为什么这样做: 构建LM的增广线性系统 (H + lambda*I) * delta_p = -g
92            A = H + lmbda * np.identity(len(params))
93            try:
94                delta_p = np.linalg.solve(A, -g)
95            except np.linalg.LinAlgError:
96                # 为什么这样做: 即使加了阻尼，矩阵仍可能病态，用伪逆保证能求解
97                delta_p = np.linalg.pinv(A) @ -g
98
99            # 为什么这样做: 如果更新量很小，说明已经收敛
100            if np.linalg.norm(delta_p) < 1e-6:
101                return params, history
102
103            # 为什么这样做: 评估这次尝试的更新效果
104            new_params = params + delta_p
105            new_residual, _ = compute_residual_and_jacobian(new_params, x_data, y_data)
106            new_cost = 0.5 * np.sum(new_residual**2)
107
108            # 为什么这样做: 计算增益比 rho，判断当前信赖域（由lambda决定）是否合适
109            # rho = (实际下降) / (模型预测下降)
110            # 模型预测下降量 L(delta_p) - L(0) = -g^T*delta_p - 0.5*delta_p^T*H*delta_p
111            pred_reduction = - (g.T @ delta_p + 0.5 * delta_p.T @ H @ delta_p)
112            actual_reduction = current_cost - new_cost
113            
114            if pred_reduction > 0:
115                rho = actual_reduction / pred_reduction
116            else:
117                rho = -1 # 预测下降为负，说明模型很差
118
119            if rho > 0.1: # 实际下降符合预期，接受更新
120                params = new_params
121                current_cost = new_cost
122                # 为什么这样做: 减小lambda，让算法更接近GN，加速收敛
123                lmbda = max(lmbda / 3, 1e-7)
124                nu = 2
125                break
126            else: # 实际下降不理想，拒绝更新
127                # 为什么这样做: 增大lambda，让算法更接近GD，缩小步长，更保守
128                lmbda = lmbda * nu
129                nu = nu * 2
130        
131        history.append(params.copy())
132
133    return params, history
134
135
136# --- 3. 运行与可视化 ---
137params_gd, _ = solve_gd(x_data, y_data, initial_params)
138params_gn, _ = solve_gn(x_data, y_data, initial_params)
139params_lm, _ = solve_lm(x_data, y_data, initial_params)
140
141print(f"真实参数: {true_params}")
142print(f"初始猜测: {initial_params}")
143print(f"GD 结果: {params_gd}")
144print(f"GN 结果: {params_gn}")
145print(f"LM 结果: {params_lm}")
146
147plt.figure(figsize=(10, 6))
148plt.scatter(x_data, y_data, label='观测数据', s=10)
149plt.plot(x_data, model_func(true_params, x_data), 'k-', linewidth=3, label='真实模型')
150plt.plot(x_data, model_func(initial_params, x_data), 'r--', label='初始猜测')
151plt.plot(x_data, model_func(params_lm, x_data), 'g-', linewidth=2, label='LM 拟合结果')
152plt.legend()
153plt.title("非线性最小二乘拟合")
154plt.show()

工程实践

1. 参数化 (Parameterization): 对于旋转等有特殊约束的变量（如在 SO(3) 群上），直接优化欧拉角会遇到万向锁问题。工程上通常优化李代数 so(3)（一个三维向量）或四元数，然后通过指数映射或单位化转换回旋转矩阵。这被称为 "流形上的优化"，Ceres 和 g2o 都有很好的支持。

2. 鲁棒核函数 (Robust Cost Functions): 现实世界的数据充满了外点（outliers），例如错误的特征匹配。标准的 L2 范数对大误差的惩罚非常大，一个外点就可能毁掉整个优化结果。工程上会用鲁棒核函数（如 Huber loss, Cauchy loss）包裹残差项，当误差大到一定程度时，其惩罚力度从二次增长变为线性或常数增长，从而抑制外点的影响。

3. 初值敏感性: 所有这些都是局部优化算法，最终结果高度依赖于初始值的质量。

   • PnP: 通常先用非迭代的线性方法（如 DLT, EPnP）计算一个粗略的初值，再用 LM 精化。
   • BA: 在 SLAM 中，位姿和地图点的初值由前端的帧间匹配和三角化提供，BA 负责在后端进行全局优化。

4. LM 的阻尼策略: 代码中 (J^T J + lambda * I) 是最简单的阻尼策略。在 Ceres 等库中，会使用更高级的对角线阻尼 diag(J^T J)，即 (J^T J + lambda * diag(J^T J))。这可以更好地适应不同参数尺度不一的情况。

5. 求解器选择: 对于 BA 这样的大规模稀疏问题，选择合适的线性求解器至关重要。常用的有 SPARSE_SCHUR (利用 Schur 补) 和 CG (共轭梯度法)，配合 SCHUR_JACOBI 等预条件子。

常见误区与边界情况

1. 误区：GD/Adam 等一阶方法在 SLAM 中无用

   • 虽然 LM 是经典 SLAM 的王者，但随着深度学习的渗透，基于梯度的优化器在“端到端”或“可微分”的 SLAM/VO 系统中扮演重要角色。它们更容易处理复杂的、由神经网络构成的代价函数，并且在 GPU 上有极高的并行效率。

2. 误区：牛顿法一定比 GD 快

   • 牛顿法在接近最优点时具有二次收敛速度，非常快。但如果离最优点很远，或者 Hessian 矩阵非正定，它可能不收敛甚至发散。而 GD 只要步长合适，总能保证向着目标函数减小的方向前进。LM 正是结合了两者的优点。

3. 边界情况：BA 中的 Gauge Freedom (规范自由度)

   • 在单目 SLAM 中，整个场景的尺度是未知的。你可以将所有相机位移和地图点坐标同时放大两倍，重投影误差完全不变。这导致 BA 的雅可比矩阵存在一个零空间，J^T J 是奇异的。这被称为“尺度不确定性”或 Gauge Freedom。
   • 解决方法: 必须固定一个尺度基准来消除歧义。例如，固定第一帧相机位姿为原点，或固定第一帧与第二帧之间的距离为 1。

4. 边界情况：数值稳定性

   • J^T J 的计算和求逆是数值问题的重灾区。如果参数尺度差异巨大（例如，一个参数在 $10^3$ 量级，另一个在 $10^{-3}$ 量级），J^T J 会变得病态 (ill-conditioned)。
   • 解决方法: 数据归一化（如将像素坐标归一化到 [-1, 1]），以及使用 LM 的对角线阻尼策略，都能有效改善数值稳定性。

面试追问

• 问: 既然 LM 这么好，为什么深度学习中大家都在用 Adam 而不是 LM？
• 答: 主要原因有三：
  1. 问题性质: LM 专为最小二乘问题设计，它利用了 H ≈ J^T J 的结构。而深度学习的损失函数通常不是最小二乘形式（如交叉熵），无法直接套用。
  2. 计算成本: LM 需要计算和存储雅可比矩阵 J，对于有数百万参数的神经网络，J 的大小是 (样本数 * 输出维度) x (参数量)，这是天文数字，内存和计算都无法承受。而 Adam 等一阶方法只需要计算梯度，梯度和参数量同维度，存储和计算成本低得多。
  3. 随机性: 深度学习通常采用小批量随机梯度下降（Mini-batch SGD）。LM 基于全批量（full-batch）的雅可比计算，不适用于随机优化。虽然有随机版本的二阶方法，但实现复杂且效果不一定优于精心调参的 Adam。