1. 从“三角化”说起：一个三维重建中的经典问题

在计算机视觉、摄影测量或者机器人定位领域，我们经常会遇到一个看似简单却至关重要的任务：三角化。想象一下，你站在一个陌生的十字路口，想确定远处一个广告牌的具体位置。你从两个不同的位置（比如路口的两个角）分别拍了一张照片，广告牌在两张照片上都有出现。通过这两张照片，结合你拍照时相机的位置和朝向，理论上你就能推算出那个广告牌在真实世界中的三维坐标。这个过程，就是三角化的核心思想——利用多视图几何，从二维图像点反推三维空间点。

然而，现实远比理想骨感。你拍的照片可能有噪点，你对自己相机位置的估计可能有误差，镜头本身还有畸变。这些因素叠加起来，导致我们通过简单的线性代数方法（比如直接线性变换DLT）计算出的三维点，往往不是最优解，甚至可能因为数值不稳定而完全错误。这就引出了我们今天要深入探讨的核心：如何通过非线性优化的方法，来求解更精确、更鲁棒的三角化结果？这本质上就是一个典型的非线性最小二乘问题。

我处理过不少三维重建和SLAM（同步定位与地图构建）的项目，三角化是其中避不开的基础模块。早期图省事直接用线性方法，结果在长基线、大视角或者噪声较大的场景下，重建出的点云飘忽不定，后续的优化和回环检测简直是一场灾难。后来系统地转向非线性优化，整个系统的稳定性和精度才有了质的提升。这篇文章，我就结合自己的实战经验，把三角化中的非线性优化给你掰开揉碎了讲清楚。

2. 为什么线性三角化不够用？非线性优化的必要性

在深入非线性方法之前，我们必须先理解线性方法为什么会在某些情况下“失灵”。线性三角化，例如经典的DLT算法或者SVD方法，其思想非常直接。对于每一个观测，我们都可以根据相机投影模型列出一个方程。对于两个视图，我们至少能得到四个方程（每个视图两个方程，对应x和y坐标），来求解三个未知数（空间点的X, Y, Z）。这是一个超定方程组，可以用最小二乘法求解。

2.1 线性方法的理想假设与残酷现实

线性方法求解的是下面这个方程的最小二乘解：(u, v)是图像上的像素坐标，P是相机的投影矩阵（包含了内参和外参），(X, Y, Z, 1)是齐次形式的三维点坐标。通过叉乘消去尺度因子，我们可以得到两个线性方程。把所有视图的方程堆叠起来，形成一个形如AX = 0的齐次线性方程组，然后通过SVD求解。

听起来很完美，对吧？问题出在它的“最小二乘”是在代数距离上进行的。它最小化的是投影方程本身的代数误差，而不是最符合我们物理直觉的误差。

注意：代数距离的几何意义并不直观。它没有直接对应到图像平面上“预测点”和“实际观测点”之间的像素距离。当噪声存在时，最小化代数距离得到的结果，并不是视觉上最“像”的那个三维点。

2.2 重投影误差：更合理的评判标准

什么才是更合理的误差？答案是重投影误差。它的定义非常直观：对于一个估计的三维点，我们把它按照相机模型重新投影到每一张观测到它的图片上，得到一个预测的像素坐标。然后，计算这个预测坐标与实际观测到的像素坐标之间的欧氏距离（以像素为单位）。这个距离，就是重投影误差。

非线性优化的目标，正是最小化所有视图上重投影误差的平方和。这被称为捆绑调整在单一点上的特例。与线性方法相比，它的优势显而易见：

1. 几何意义明确：误差直接反映了三维点估计的准确度在图像上的表现。
2. 对噪声更鲁棒：通过迭代优化，能够部分修正由于相机参数误差或观测噪声带来的影响。
3. 兼容各种相机模型：无论是简单的针孔模型，还是包含径向、切向畸变的复杂模型，都可以轻松纳入重投影误差的计算框架。

用一个简单的类比：线性方法好比用一把刻度不均匀的尺子去测量，然后求平均；而非线性优化则是先校准尺子（优化过程），再用它去找到一个让所有测量读数看起来最协调的长度。

3. 构建三角化的非线性最小二乘模型

理解了“为什么”要优化，接下来我们看“怎么做”。构建一个完整的非线性最小二乘问题，需要三个核心要素：优化变量、残差项和目标函数。

3.1 优化变量：我们要求解的是什么？

在单纯的三角化问题中，优化变量通常就是那个未知的三维空间点的坐标P = [X, Y, Z]^T。这是最简单的情形。但在更实际的场景中，尤其是系统辨识或自标定场景下，优化变量可能会扩展。

例如，在同时进行相机位姿估计和三角化时（即SfM或SLAM中的局部BA），优化变量会包括多个三维点坐标和多个相机位姿（旋转和平移）。甚至，如果相机内参（如焦距、主点、畸变系数）也不确定，它们也会被加入优化。这时的模型就是典型的光束法平差。

3.2 残差计算：重投影误差的数学表达

对于第i个相机视图，假设其相机投影函数为π_i（这个函数包含了内参、外参和畸变模型），观测到的二维像素坐标为z_i = [u_i, v_i]^T。

那么，对于当前估计的三维点P，在第i个视图上的重投影误差（残差）r_i定义为：r_i(P) = z_i - π_i(P)

这是一个二维向量。我们的目标是最小化所有M个视图上残差的平方和。

3.3 目标函数与损失函数

标准的最小二乘目标函数是：F(P) = ½ Σ_{i=1}^{M} || r_i(P) ||^2

这里的½是为了后续求导方便而添加的系数。这就是一个无约束的非线性最小二乘问题。在存在外点（错误匹配）的情况下，我们通常会使用鲁棒核函数（如Huber核、Cauchy核）来降低外点的影响，此时目标函数变为：F(P) = ½ Σ_{i=1}^{M} ρ( || r_i(P) ||^2 )其中ρ(·)是鲁棒核函数。这能有效防止一个错误匹配的观测把整个优化“拉偏”。

4. 求解利器：非线性最小二乘优化算法剖析

模型建好了，怎么求解这个最小化问题？这里不可能展开讲所有优化理论，但我会重点介绍在三角化乃至整个视觉SLAM/VIO中最核心、最实用的两种方法：高斯-牛顿法和列文伯格-马夸尔特法。它们都是迭代求解的方法。

4.1 高斯-牛顿法：在局部用线性模型替代

高斯-牛顿法的思想很巧妙。它不对目标函数F(x)进行二阶泰勒展开，而是对残差函数r(x)进行一阶泰勒展开。因为F(x)是残差的平方和，利用这个结构可以避免计算二阶导数（海森矩阵），大大减少了计算量。

假设当前迭代点为x_k，残差r(x_k)的雅可比矩阵为J_k。那么，高斯-牛顿法通过求解如下线性方程来得到增量Δx：(J_k^T J_k) Δx = -J_k^T r(x_k)这个方程被称为正规方程。解出Δx后，更新估计值：x_{k+1} = x_k + Δx。

在三角化中的具体体现： 对于我们的三维点P，残差r_i(P)是一个2维向量，它对P的雅可比是一个2x3的矩阵。这个雅可比矩阵描述了三维点坐标微小变化时，其在图像上投影点移动的方向和速度。J^T J就形成了一个3x3的信息矩阵（或近似海森矩阵），它集中了所有观测对这个三维点的约束强度。在三角化中，这个矩阵如果病态（特征值很小），说明观测角度不好或者噪声太大，三角化的结果就会非常不确定。

4.2 列文伯格-马夸尔特法：带信任区域的高斯-牛顿

LM算法是高斯-牛顿法的增强版，可以看作是融合了梯度下降法思想的高斯-牛顿法。它引入了一个“信任区域”的概念：在当前点x_k附近的一个区域Δ内，我们相信线性模型（即泰勒展开）是足够好的。

LM算法求解的是带约束的优化问题：在||Δx|| ≤ Δ的范围内，最小化线性模型下的近似目标。通过拉格朗日乘子法，这等价于求解：(J_k^T J_k + λ I) Δx = -J_k^T r(x_k)其中λ是一个阻尼因子，I是单位矩阵。

• 当λ很大时，λI占主导，方程近似为λI Δx = -J_k^T r(x_k)，即Δx ≈ - (1/λ) J_k^T r(x_k)，这接近于最速下降方向，步长很小，适合在初始阶段或远离最优解时使用。
• 当λ很小时，方程退化为高斯-牛顿方程，适合在接近最优解时快速收敛。

LM算法会根据每次迭代后目标函数实际下降值与模型预测下降值的比值ρ，来动态调整阻尼因子λ和信任区域半径Δ。ρ大说明模型拟合得好，可以增大信任区域（减小λ）；ρ小说明模型拟合得差，需要缩小信任区域（增大λ）。

为什么三角化常用LM算法？因为三角化的初始值（通常由线性方法提供）可能离真值较远，且问题可能因观测不足而病态。高斯-牛顿法在病态或初始值差时容易发散。LM算法通过阻尼因子保证了矩阵(J^T J + λI)的正定性，使得求解更稳定，能够从更差的初始点开始收敛。在实际的视觉库如g2o、Ceres Solver中，LM算法是求解这类BA问题的默认或推荐选择。

5. 实战：从理论到代码的三角化优化

光说不练假把式。我们用一个简化的例子，来看看如何在实践中实现三角化的非线性优化。这里假设相机内参已知且已去畸变，我们使用两视图三角化。

5.1 第一步：获取初始值

非线性优化需要一个起点。这个起点通常由线性三角化方法（如DLT）提供。我们记这个初始三维点为P_0。

// 伪代码示例：使用OpenCV的triangulatePoints获取初始值 cv::Mat points4D; // 齐次坐标 [X, Y, Z, W] cv::triangulatePoints(projMatrix1, projMatrix2, points2D_1, points2D_2, points4D); // 转换为非齐次坐标，得到 P_0 cv::Point3f P_initial = convertFromHomogeneous(points4D);

5.2 第二步：定义残差和雅可比计算

这是最关键的一步。我们需要编程实现两个函数：

1. 重投影函数π(P)：将世界点P变换到相机坐标系，再投影到归一化平面，最后应用内参得到像素坐标。
2. 残差计算r = z - π(P)。
3. 雅可比矩阵计算J = ∂r/∂P：即残差对三维点坐标的导数。

对于针孔相机模型，投影过程为：u = f_x * (X_c / Z_c) + c_xv = f_y * (Y_c / Z_c) + c_y其中[X_c, Y_c, Z_c]^T = R * P + t是点在相机坐标系下的坐标。

雅可比矩阵可以通过链式法则求得：∂r/∂P = ∂r/∂[X_c, Y_c, Z_c] * ∂[X_c, Y_c, Z_c]/∂P第一部分是像素坐标对相机坐标的导数（一个2x3矩阵），第二部分是相机坐标对世界坐标的导数（对于点P，就是旋转矩阵R，一个3x3矩阵）。具体推导涉及一些求导，这里给出结果中的一个元素示例：∂u/∂X = f_x * (r11 * Z_c - r31 * X_c) / Z_c^2（假设R的第一行是[r11, r12, r13]...）

在实际使用优化库（如Ceres Solver）时，我们可以使用自动求导，无需手动推导雅可比，这大大降低了开发难度。

5.3 第三步：配置并运行优化器

以Ceres Solver为例，构建问题的流程非常清晰：

// 伪代码示例：使用Ceres Solver进行三角化优化 ceres::Problem problem; for (int i = 0; i < num_views; ++i) { // 创建代价函数。使用自动求导，模板参数：残差维度，优化变量维度 ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<ReprojectionError, 2, 3>( new ReprojectionError(observed_pixel[i], camera_intrinsic[i], camera_pose[i])); // 向问题中添加残差块。优化变量是三维点P problem.AddResidualBlock(cost_function, nullptr, P_estimated); } // 配置求解器选项 ceres::Solver::Options options; options.linear_solver_type = ceres::DENSE_QR; // 对于小问题（单点），使用DENSE_QR足够 options.minimizer_progress_to_stdout = true; // 输出迭代信息 options.max_num_iterations = 50; // 最大迭代次数 // 可以启用鲁棒核函数，例如Huber核，用于抑制外点 // ceres::LossFunction* loss_function = new ceres::HuberLoss(1.0); // 在AddResidualBlock时传入loss_function ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); std::cout << summary.BriefReport() << std::endl;

5.4 一个关键的实操心得：尺度归一化

在优化迭代中，三维点P的坐标值可能变化很大。直接使用欧氏距离的平方作为代价，可能会让优化过程对数值尺度敏感。一个非常有效的技巧是进行尺度归一化。

我常用的做法是，在计算重投影误差之前，对三维点P的坐标进行归一化。例如，可以用第一次线性三角化得到的所有点的平均深度作为尺度因子，将所有点的坐标除以这个尺度因子，让它们分布在1.0附近。这样，优化器中的步长、阻尼因子等参数就有一个相对统一的尺度，更容易设置，收敛也更稳定。优化完成后，再将坐标乘回原来的尺度。

6. 性能、精度与鲁棒性：深入优化细节

把优化跑起来只是第一步。要让它在实际系统中稳定、高效、精确地工作，还需要关注很多细节。

6.1 迭代终止条件：如何知道优化“够了”？

优化不能无限迭代下去。常见的终止条件有：

• 梯度足够小：||J^T r||_∞ < ε1，表示当前点已经接近驻点（梯度为零）。
• 参数变化足够小：||Δx|| < ε2 * (||x|| + ε2)，表示迭代步长非常小，优化进展缓慢。
• 代价函数下降足够小：|ΔF| < ε3 * (|F| + ε3)，表示目标函数值几乎不再降低。
• 达到最大迭代次数：防止无限循环。

在Ceres或g2o中，这些条件都有对应的参数可以设置。对于单点三角化这种小问题，通常迭代10-20次就足够了。

6.2 精度评估：信息矩阵与不确定性

优化完成后，我们不仅得到了一个三维点坐标，还能评估这个估计的不确定性。这在SLAM中对于后续的数据关联、地图管理至关重要。

在非线性最小二乘框架下，优化收敛后，近似海森矩阵H ≈ J^T J的逆，就是优化变量协方差矩阵的一个近似。对于我们的三维点P，Σ_P = (J^T J)^{-1}是一个3x3的矩阵，它描述了P在各个方向上的不确定性。

这个信息矩阵的特征值和特征向量非常有用：

• 特征值的大小：反映了在该特征向量方向上的不确定程度。特征值越大，不确定性越小（约束越强）。
• 特征向量的方向：最大特征值对应的方向，通常是沿着平均观测方向的，不确定性最小；最小特征值对应的方向，通常是沿着观测基线的，不确定性最大。这就是为什么观测基线越长，三角化精度越高的数学体现。

我们可以计算一个位置误差的椭球来直观表示不确定性。如果某个点的最大不确定性（椭球长轴）超过阈值，可以认为该点三角化质量差，予以剔除。

6.3 处理外点：鲁棒核函数的作用

图像匹配不可能100%正确。错误匹配会产生巨大的重投影误差，如果不加处理，它会像“黑洞”一样把优化结果拉偏。这就是外点问题。

鲁棒核函数ρ(s)（其中s = ||r||^2）的作用是，当残差s很大时，给这个残差项赋予一个较小的权重，甚至饱和到一个固定值，从而减弱外点的影响。

选择哪个核函数，以及如何设置其参数δ（通常与测量噪声标准差相关），需要根据具体场景实验。我的经验是，在视觉SLAM的BA中，Huber核是一个不错的默认选择，它在鲁棒性和收敛性之间取得了较好的平衡。

7. 超越单点：系统辨识与大规模三角化

文章开头提到的热词“系统辨识”和“最小二乘辨识模型”，在这里可以联系起来。当我们的优化变量不仅仅是三维点，还包括相机参数（内参、外参）时，三角化问题就升级为了一个系统辨识问题——我们通过观测数据（二维图像点）来辨识整个系统的参数（三维结构和传感器模型）。

7.1 局部束调整

在SLAM中，我们经常不是三角化一个点，而是同时三角化一批新观测到的点，并且可能还会优化与这些点相关的最近几帧相机位姿。这个过程称为局部束调整。此时的优化变量维数会急剧上升（例如，优化10个位姿和100个点，变量维数就是106 + 1003 = 360维）。

对于这种问题，正规方程(J^T J) Δx = -J^T r中的J^T J矩阵会呈现出特殊的稀疏块结构。因为一个残差只依赖于一个点和看到它的相机，所以雅可比矩阵J的大部分块是零。利用这种稀疏性，可以极大地提高求解效率。这就是为什么g2o、Ceres等库都提供稀疏求解器（如SPARSE_NORMAL_CHOLESKY, SPARSE_SCHUR）的原因。

7.2 工程实现中的分层策略

在实际的视觉SLAM系统中，对所有点进行全局BA计算量太大。通常采用分层策略：

1. 前端三角化：对每一对新匹配的特征点，使用线性方法或快速的非线性优化（只优化该点）得到一个初始三维点。
2. 局部BA：维护一个滑动窗口，当新帧到来时，对窗口内的所有相机位姿和能观测到的地图点进行联合优化。这一步会用到我们讨论的非线性最小二乘方法，并利用稀疏性加速。
3. 全局BA/Pose Graph优化：在回环检测发生后，或者定期地，进行规模更大的优化以消除累积误差。此时可能会用更简化的位姿图模型来替代完整的BA以提升速度。

三角化的非线性优化，是这个庞大优化系统中的基础原子操作。它的稳定性和精度，直接决定了上层建筑的质量。

最后，再分享一个调试中的小技巧：当你怀疑三角化或BA的结果有问题时，可视化重投影误差是最直接的诊断方法。把优化后的三维点重新投影到图像上，并用不同颜色（如绿色表示误差小，红色表示误差大）画出连线。你能一眼看出哪些匹配可能是错误的，哪些相机位姿估计有问题。这种视觉反馈对于快速定位问题至关重要。