1. 项目概述：从稀疏点到三维世界

在计算机视觉、机器人定位或者摄影测量领域，我们常常会面对这样一个问题：手里只有几张从不同角度拍摄的二维照片，以及照片上一些稀疏的、彼此对应的像素点，我们如何恢复出这些点在真实三维空间中的位置？这个过程，就是“三角化”。听起来像是几何题，但在实际工程中，它从来不是一道简单的、有唯一解的代数题。因为相机标定有误差，特征点检测有亚像素级的偏差，这些噪声让我们的观测方程变得“不可靠”。直接套用线性最小二乘求解，就像用一把刻度不准的尺子去量东西，结果往往差强人意。

这时候，“非线性优化”就登场了。它不再满足于得到一个在代数上“误差平方和最小”的解，而是直面问题的本质：我们有一个关于三维点坐标的非线性观测模型（相机投影模型），以及带噪声的二维观测数据。非线性优化的目标，就是调整三维点的坐标，使得根据模型“重投影”回二维图像上的点，与实际的观测点之间的误差最小。这更像是一个“校准”过程，通过迭代调整，让模型预测无限逼近真实观测。今天要聊的，就是这个将线性三角化结果作为“初值”，再通过非线性优化进行“精修”的完整流程。这不仅是提升三维重建精度的关键一步，更是理解如何将理论模型应用于嘈杂现实世界的绝佳案例。

2. 核心思路：为何线性解只是开始

在深入非线性优化之前，我们必须先理解为什么线性三角化（比如直接线性变换DLT或SVD方法）给出的解不够好。这关乎我们对问题本质的认识。

2.1 线性方法的局限与误差来源

线性方法的核心，是将相机投影矩阵P（包含内参、旋转和平移）与三维点齐次坐标X的乘法关系P X = x（x为归一化平面坐标或像素坐标）展开，利用叉乘消去尺度因子，构造出形如A X = 0的线性方程组。通过SVD求解最小奇异值对应的右奇异向量，得到X。

这个方法简洁优美，但它隐含了两个重要的假设，也是其误差的主要来源：

1. 代数误差 vs. 几何误差：线性最小二乘最小化的是代数误差（即 ||A X||^2），但这并不是我们关心的物理误差。我们真正关心的是几何误差，即三维点X投影到图像上的二维点 (u_pred, v_pred) 与实际观测到的二维点 (u_obs, v_obs) 之间的欧氏距离。在投影模型中，这两者是非线性关系。最小化代数误差，并不能保证几何误差也最小。
2. 各向同性的噪声假设：线性方法在构造方程时，默认像素坐标x和y方向的噪声是独立同分布的。但在实际中，由于特征点检测算法（如SIFT, ORB）的特性，或者在图像边缘、模糊区域，噪声可能并不是各向同性的。线性方法无法优雅地处理这种异方差噪声。

举个例子，假设一个三维点正好投影在图像的边缘，由于镜头畸变或图像拉伸，其在u方向（水平）的定位可能比v方向（垂直）更不确定。线性方法平等地对待u和v的误差，导致优化方向偏离最优。

2.2 非线性优化的目标函数

非线性优化直接针对几何误差建模。对于一个三维点X，被第i个相机（参数为P_i）观测到，其投影的像素坐标预测值为：[u_i_pred, v_i_pred]^T = project(P_i, X)其中project是包含内参、畸变等非线性变换的投影函数。

假设我们有N个相机观测到了同一个点，那么该点的重投影误差总和为：E(X) = Σ_{i=1}^{N} || [u_i_obs, v_i_obs]^T - project(P_i, X) ||^2

非线性优化的任务就是：寻找一个三维点坐标X，使得目标函数E(X)的值最小。这是一个典型的无约束非线性最小二乘问题。由于project函数是非线性的，我们无法直接求解，必须依赖迭代优化算法，如高斯-牛顿法或列文伯格-马夸尔特法。

注意：这里假设相机参数P_i是已知且固定的（通常来自之前的结构恢复或SFM流程）。我们只优化三维点坐标X。这是一种“捆集调整”的简化形式，只调整点，不调整相机。

3. 从线性解到非线性优化：完整流程拆解

理解了“为什么”之后，我们来看“怎么做”。一个稳健的三角化流程，一定是线性初始化配合非线性精修。

3.1 第一步：获取可靠的线性初值

非线性优化算法（如LM）严重依赖于初始值。一个糟糕的初值可能导致算法收敛到局部极小值，甚至发散。因此，获取一个尽可能靠近真值的线性解至关重要。

1. 数据准备：确保你有至少两个视图（相机）对同一个三维点的观测。每个观测是像素坐标(u, v)。同时，你需要每个相机对应的投影矩阵P（如果是像素坐标，P是3x4矩阵，包含了内参和位姿；如果是归一化坐标，则使用本质矩阵或直接使用旋转平移）。
2. 线性三角化：
   • DLT方法：对于每个观测，利用叉乘x × (P X) = 0构造两个线性方程。将多个视图的方程堆叠，形成超定方程组A X = 0。
   • SVD求解：对矩阵A进行奇异值分解（SVD），A = U Σ V^T。解X即为V矩阵最后一列（对应最小奇异值）的前三个分量，第四个分量为齐次坐标尺度因子，需要归一化（例如，使第四维为1）得到三维欧氏坐标。
   • 处理退化情况：如果所有相机光心与三维点几乎共线，矩阵A的条件数会很大，解不稳定。实践中，可以通过检查SVD的最小奇异值与次小奇异值的比值来判断。如果比值太小（如小于1e-5），则该点的三角化结果不可靠，应考虑剔除。

这个线性解X_linear，就是我们给非线性优化准备的“起跑线”。

3.2 第二步：构建非线性优化问题

现在，我们以X_linear为初始值，构建并求解非线性最小二乘问题。这里以最常用的列文伯格-马夸尔特算法为例，因为它兼具高斯-牛顿法的快速收敛和梯度下降法的稳定性。

1. 定义参数块与残差块：

   • 参数块：待优化的变量，即三维点坐标X = [X, Y, Z]^T。这是一个3维向量。
   • 残差块：对于第i个相机，残差是一个2维向量：r_i(X) = [u_i_obs - u_i_pred(X), v_i_obs - v_i_pred(X)]^T其中，[u_i_pred, v_i_pred]^T = project(P_i, X)。

2. 目标函数：总目标函数为所有残差项的平方和：F(X) = 0.5 * Σ ||r_i(X)||^2。系数0.5是为了后续求导方便，不影响最优解位置。

3. 核心：雅可比矩阵计算：LM算法的每一步迭代，都需要计算残差向量r关于参数X的雅可比矩阵J。J是一个(2N) x 3的矩阵。对于第i个残差块，其对应的2x3雅可比子矩阵为：J_i = ∂r_i / ∂X = - (∂project(P_i, X) / ∂X)计算这个导数需要用到链式法则，涉及相机投影模型（从三维到归一化平面）、畸变模型、内参矩阵乘法等一系列偏导。这是实现中最需要细心和正确性的部分。

   实操心得：雅可比矩阵的解析形式推导虽然繁琐，但至关重要。使用数值差分（如中心差分）来验证解析雅可比是否正确，是一个非常好的调试习惯。一个错误的雅可比会导致优化收敛缓慢甚至失败。

3.3 第三步：LM算法迭代求解

有了目标函数F(X)和雅可比矩阵J(X)，LM算法的迭代步骤如下：

1. 初始化：X = X_linear， 设置阻尼因子λ为一个初始值（如1e-3），以及缩放因子v（如10）。
2. 对于第k次迭代： a. 计算当前残差r(X_k)和雅可比J(X_k)。 b. 构造增量正规方程：(J^T J + λ I) δ = -J^T r。其中I是单位阵，λI项就是“阻尼”，它确保了系数矩阵的正定性。 c. 求解线性方程组，得到参数增量δ。 d. 尝试更新参数：X_new = X_k + δ。 e. 计算实际下降量：ΔF_actual = F(X_k) - F(X_new)。 f. 计算预测下降量：ΔF_predicted = -δ^T (J^T r) - 0.5 * δ^T (J^T J) δ。这个值理论上应为正。 g. 计算增益比：ρ = ΔF_actual / ΔF_predicted。 h. 更新迭代状态： * 如果ρ很大（如>0.75），说明局部二次模型拟合得很好，接受更新X_{k+1} = X_new，并减小阻尼因子λ = λ / max(1/3, 1 - (2ρ-1)^3)， v=2。这样下一步更接近高斯-牛顿法，收敛更快。 * 如果ρ很小（如<0.25），说明二次模型拟合差，拒绝更新X_{k+1} = X_k，并增大阻尼因子λ = λ * v，v = 2 * v。这样下一步更接近梯度下降法，步长更小更稳定。 * 如果ρ在中间，接受更新，但保持λ不变。
3. 判断收敛：当满足以下条件之一时停止迭代：
   • 参数增量δ的范数小于阈值（如1e-6）。
   • 目标函数下降量ΔF_actual的绝对值小于阈值（如1e-9）。
   • 梯度J^T r的范数小于阈值（如1e-6）。
   • 达到最大迭代次数（如50）。

经过若干次迭代，算法输出的X_final就是非线性优化后的三维点坐标，其重投影误差理论上比线性解X_linear更小。

4. 关键实现细节与参数调优

理论流程清晰了，但魔鬼在细节里。要让这套流程稳定高效地跑起来，有几个关键点必须处理好。

4.1 投影与畸变模型

project(P_i, X)函数的具体实现直接影响优化精度。一个完整的投影流程通常包括：

1. 世界系到相机系：X_cam = R * X + t。R, t是相机外参。
2. 相机系到归一化平面：x_norm = X_cam / Z_cam，y_norm = Y_cam / Z_cam。这里得到了无畸变的归一化坐标。
3. 径向和切向畸变校正：这是主要的非线性部分。

   r^2 = x_norm^2 + y_norm^2 x_dist = x_norm * (1 + k1*r^2 + k2*r^4 + k3*r^6) + 2*p1*x_norm*y_norm + p2*(r^2 + 2*x_norm^2) y_dist = y_norm * (1 + k1*r^2 + k2*r^4 + k3*r^6) + p1*(r^2 + 2*y_norm^2) + 2*p2*x_norm*y_norm

   k1, k2, k3为径向畸变系数，p1, p2为切向畸变系数。
4. 归一化平面到像素平面：

   u_pred = f_x * x_dist + c_x v_pred = f_y * y_dist + c_y

   f_x, f_y是焦距，c_x, c_y是主点。

在非线性优化中，如果相机已经标定，那么内参(f_x, f_y, c_x, c_y)和畸变系数(k1, k2, p1, p2)都是已知常数。雅可比矩阵的计算必须包含对畸变模型的求导。

4.2 鲁棒核函数的引入

在实际场景中，可能存在错误的特征匹配（外点）。这些外点会产生巨大的残差，严重干扰优化过程，因为最小二乘对大的残差项赋予极高的权重（平方项）。

为了解决这个问题，需要引入鲁棒核函数。它的作用是对残差进行“重新加权”，降低大残差（可能是外点）的影响力。常用的有Huber核、Cauchy核。

例如，Huber核函数：

ρ(s) = { s, if s <= δ^2 { 2δ√s - δ^2, if s > δ^2

其中s = ||r_i||^2，δ是一个阈值参数。

在优化中，我们不再最小化Σ ||r_i||^2，而是最小化Σ ρ(||r_i||^2)。这相当于对每个残差项施加了一个权重w_i = ρ'(s)。在迭代求解时，这个权重会体现在信息矩阵（或对残差向量的缩放）中。当残差很大时（s > δ^2），其权重会从1下降为δ / √s，从而抑制了外点的影响。

注意事项：阈值δ的选择很重要。通常可以设置为一个与特征点定位精度相关的值，例如，对于像素误差，δ可以设为3~5个像素（对应δ^2为9~25）。需要根据具体场景调试。

4.3 优化库的选择与使用

我们不需要从头实现LM算法。优秀的优化库可以让我们专注于问题建模。最常用的两个是：

• Ceres Solver：谷歌开源的C++库，专门用于求解大规模非线性最小二乘问题。它自动求导功能强大，支持鲁棒核，API设计优雅。对于三角化这种小规模问题，可以轻松地用AutoDiffCostFunction定义残差块。
• g2o：另一个流行的C++优化库，最初专注于图优化，在SLAM领域应用极广。其底层也提供了多种优化算法。定义顶点（参数块）和边（残差块）的图优化模型，对于理解问题结构很有帮助。

以Ceres为例，实现三角化非线性优化的代码框架非常清晰：

// 定义残差计算仿函数，使用自动求导 struct ReprojectionError { ReprojectionError(double observed_u, double observed_v, const Camera& cam) : observed_u(observed_u), observed_v(observed_v), camera(cam) {} template <typename T> bool operator()(const T* const point_3d, T* residuals) const { // 1. 将point_3d转换到相机坐标系 T p[3]; camera.WorldToCamera(point_3d, p); // 包含R,t变换 // 2. 投影到归一化平面，并施加畸变 T xp, yp; camera.NormalizeWithDistortion(p, &xp, &yp); // 3. 利用内参转换到像素坐标 T predicted_u = camera.fx * xp + camera.cx; T predicted_v = camera.fy * yp + camera.cy; // 4. 计算残差 residuals[0] = predicted_u - T(observed_u); residuals[1] = predicted_v - T(observed_v); return true; } double observed_u, observed_v; Camera camera; // 包含内参、畸变、外参的结构体 }; // 主优化逻辑 ceres::Problem problem; double point_3d[3] = {X_linear, Y_linear, Z_linear}; // 线性初值 for (const auto& observation : observations) { ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<ReprojectionError, 2, 3>( new ReprojectionError(observation.u, observation.v, observation.camera)); problem.AddResidualBlock(cost_function, new ceres::HuberLoss(5.0), // 鲁棒核，delta=5.0 point_3d); } ceres::Solver::Options options; options.linear_solver_type = ceres::DENSE_QR; // 小规模问题用DENSE_QR options.minimizer_progress_to_stdout = true; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary);

5. 实战问题排查与性能分析

即使流程正确，在实际编码和运行中也会遇到各种问题。下面是一些常见坑点及其解决方案。

5.1 优化不收敛或结果变差

这是最令人头疼的问题。可以从以下方面排查：

1. 初值太差：线性三角化的结果可能已经“坏掉了”。检查该点在所有视图中的重投影误差（用线性解计算）。如果某个视图的误差巨大（如>100像素），可能是特征匹配错误，或者该视图的相机位姿P_i不准。尝试剔除误差最大的视图，只用质量好的视图重新做线性三角化，或者直接放弃这个点。
2. 雅可比矩阵错误：这是非常隐蔽的错误。使用优化库（如Ceres）的数值差分检查功能（CHECK开头的选项），或者自己写一个中心差分的数值雅可比计算函数，与解析雅可比在初始点附近进行比较。任何微小的不一致都可能导致优化路径偏离。
3. 尺度问题：三维点坐标X、平移向量t的数值可能非常大或非常小，导致Hessian矩阵J^T J的条件数很差。可以对三维点坐标进行归一化（例如，减去点云质心，缩放到一个单位球内），优化完成后再变换回去。或者，在优化时使用更好的线性求解器（如DENSE_SCHUR或SPARSE_NORMAL_CHOLESKY）。
4. 外点干扰：没有使用或错误使用了鲁棒核。确认鲁棒核函数的阈值设置合理。可以尝试先不用鲁棒核，观察哪些点的残差巨大，手动剔除它们后再优化。

5.2 精度评估与对比

如何量化非线性优化带来的提升？一个标准的评估流程是：

1. 计算重投影误差统计：分别用线性解X_linear和非线性解X_nonlinear，计算在所有观测视图上的重投影误差（欧氏距离）。
2. 对比指标：
   • 平均误差：mean_error = Σ ||r_i|| / N_observations。
   • 误差中位数：对误差排序取中位数，对异常值不敏感。
   • 误差标准差：反映误差的离散程度。
   • 最大误差：观察最差点的情况。
3. 可视化：将重投影误差向量（即r_i）在图像上画出来，箭头从预测点指向观测点。这能直观地看到误差的方向和大小分布。一个健康的优化结果，误差箭头应该短且方向随机；如果出现一致的、方向性的误差，可能暗示相机标定（特别是畸变参数）仍有问题。

在我的一个多视图重建项目中，对1000个三角化点进行非线性优化后，平均重投影误差从线性解的1.8像素下降到了0.7像素，误差中位数从1.2像素下降到了0.5像素。更重要的是，最大误差从35像素（由少数外点导致）被压制到了5像素以内。鲁棒核函数功不可没。

5.3 效率考量

三角化通常是在SFM或SLAM流程中，对成千上万个点逐一进行的。因此，每个点的优化效率很重要。

1. 提前判断：对于线性解重投影误差已经很小的点（例如<0.5像素），可以跳过非线性优化，直接使用线性解。这能节省大量计算。
2. 设置合理的收敛条件：对于三角化这种小问题（3个参数），通常迭代10-20次就足够了。可以将最大迭代次数设为20，梯度阈值设为1e-6。过严的收敛条件只会增加无谓的迭代。
3. 选择合适的线性求解器：在Ceres中，对于参数块只有3维的问题，DENSE_QR或DENSE_NORMAL_CHOLESKY是最快、最稳定的选择。避免使用为大规模问题设计的迭代求解器。
4. 并行化：各个三维点的优化是相互独立的，这是天然的并行任务。可以使用OpenMP或线程池，同时对多个点进行优化，能极大提升整体三角化速度。

6. 扩展：与捆集调整的关系

三角化的非线性优化，可以看作是捆集调整的一个特例或子问题。完整的捆集调整同时优化所有相机参数（位姿、内参）和所有三维点坐标，目标是最小化所有重投影误差之和。这是一个巨型的非线性最小二乘问题。

而我们这里讨论的三角化非线性优化，是在固定所有相机参数的前提下，仅优化单个三维点的坐标。这相当于在捆集调整的大问题中，固定其他所有变量，只优化与某一个点相关的参数。因此，它的原理、目标函数和优化算法（LM）与捆集调整是完全一致的。

在实际的SFM流程中，通常采用一种交替优化的策略：

1. 增量式重建：初始化两个视图，三角化一批点。
2. 局部捆集调整：用这些点和新加入的视图，进行局部BA，同时优化新视图的位姿和这些点的坐标。
3. 三角化新点：用优化后的位姿，三角化新的匹配点。
4. 全局捆集调整：当相机和点积累到一定数量，或者累计误差较大时，进行一次全局BA。

在这个流程中，每一步的三角化（无论是新点还是优化旧点），其背后的非线性优化思想都是一脉相承的。理解了这个点的优化，就为理解更复杂的捆集调整打下了坚实的基础。

最后，再分享一个调试小技巧：在优化迭代时，不仅打印目标函数值，也打印三维点坐标的变化量。如果发现坐标在某个维度上发生剧烈跳动（例如Z值从正变负），那几乎可以肯定是初值问题或雅可比错误。此时，将优化过程可视化，在三维空间中画出每次迭代后点的位置轨迹，能帮助你非常直观地理解优化器在“想”什么，是定位问题根源的利器。