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 重投影误差更合理的评判标准什么才是更合理的误差答案是重投影误差。它的定义非常直观对于一个估计的三维点我们把它按照相机模型重新投影到每一张观测到它的图片上得到一个预测的像素坐标。然后计算这个预测坐标与实际观测到的像素坐标之间的欧氏距离以像素为单位。这个距离就是重投影误差。非线性优化的目标正是最小化所有视图上重投影误差的平方和。这被称为捆绑调整在单一点上的特例。与线性方法相比它的优势显而易见几何意义明确误差直接反映了三维点估计的准确度在图像上的表现。对噪声更鲁棒通过迭代优化能够部分修正由于相机参数误差或观测噪声带来的影响。兼容各种相机模型无论是简单的针孔模型还是包含径向、切向畸变的复杂模型都可以轻松纳入重投影误差的计算框架。用一个简单的类比线性方法好比用一把刻度不均匀的尺子去测量然后求平均而非线性优化则是先校准尺子优化过程再用它去找到一个让所有测量读数看起来最协调的长度。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) ½ Σ_{i1}^{M} || r_i(P) ||^2这里的½是为了后续求导方便而添加的系数。这就是一个无约束的非线性最小二乘问题。在存在外点错误匹配的情况下我们通常会使用鲁棒核函数如Huber核、Cauchy核来降低外点的影响此时目标函数变为F(P) ½ Σ_{i1}^{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_{k1} 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 第二步定义残差和雅可比计算这是最关键的一步。我们需要编程实现两个函数重投影函数π(P)将世界点P变换到相机坐标系再投影到归一化平面最后应用内参得到像素坐标。残差计算r z - π(P)。雅可比矩阵计算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::AutoDiffCostFunctionReprojectionError, 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很大时给这个残差项赋予一个较小的权重甚至饱和到一个固定值从而减弱外点的影响。核函数公式近似特点Huberρ(s) { s (if s≤δ); 2√δ√s - δ (if sδ) }线性增长对中等外点有效计算简单。Cauchyρ(s) δ² log(1 s/δ²)对数增长对严重外点抑制力更强但可能导致收敛变慢。Tukeyρ(s) { (δ²/6)[1-(1-s/δ²)³] (if s≤δ²); δ²/6 (if 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计算量太大。通常采用分层策略前端三角化对每一对新匹配的特征点使用线性方法或快速的非线性优化只优化该点得到一个初始三维点。局部BA维护一个滑动窗口当新帧到来时对窗口内的所有相机位姿和能观测到的地图点进行联合优化。这一步会用到我们讨论的非线性最小二乘方法并利用稀疏性加速。全局BA/Pose Graph优化在回环检测发生后或者定期地进行规模更大的优化以消除累积误差。此时可能会用更简化的位姿图模型来替代完整的BA以提升速度。三角化的非线性优化是这个庞大优化系统中的基础原子操作。它的稳定性和精度直接决定了上层建筑的质量。最后再分享一个调试中的小技巧当你怀疑三角化或BA的结果有问题时可视化重投影误差是最直接的诊断方法。把优化后的三维点重新投影到图像上并用不同颜色如绿色表示误差小红色表示误差大画出连线。你能一眼看出哪些匹配可能是错误的哪些相机位姿估计有问题。这种视觉反馈对于快速定位问题至关重要。 非线性优化在三维重建三角化中的应用：从重投影误差到LM算法
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 重投影误差更合理的评判标准什么才是更合理的误差答案是重投影误差。它的定义非常直观对于一个估计的三维点我们把它按照相机模型重新投影到每一张观测到它的图片上得到一个预测的像素坐标。然后计算这个预测坐标与实际观测到的像素坐标之间的欧氏距离以像素为单位。这个距离就是重投影误差。非线性优化的目标正是最小化所有视图上重投影误差的平方和。这被称为捆绑调整在单一点上的特例。与线性方法相比它的优势显而易见几何意义明确误差直接反映了三维点估计的准确度在图像上的表现。对噪声更鲁棒通过迭代优化能够部分修正由于相机参数误差或观测噪声带来的影响。兼容各种相机模型无论是简单的针孔模型还是包含径向、切向畸变的复杂模型都可以轻松纳入重投影误差的计算框架。用一个简单的类比线性方法好比用一把刻度不均匀的尺子去测量然后求平均而非线性优化则是先校准尺子优化过程再用它去找到一个让所有测量读数看起来最协调的长度。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) ½ Σ_{i1}^{M} || r_i(P) ||^2这里的½是为了后续求导方便而添加的系数。这就是一个无约束的非线性最小二乘问题。在存在外点错误匹配的情况下我们通常会使用鲁棒核函数如Huber核、Cauchy核来降低外点的影响此时目标函数变为F(P) ½ Σ_{i1}^{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_{k1} 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 第二步定义残差和雅可比计算这是最关键的一步。我们需要编程实现两个函数重投影函数π(P)将世界点P变换到相机坐标系再投影到归一化平面最后应用内参得到像素坐标。残差计算r z - π(P)。雅可比矩阵计算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::AutoDiffCostFunctionReprojectionError, 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很大时给这个残差项赋予一个较小的权重甚至饱和到一个固定值从而减弱外点的影响。核函数公式近似特点Huberρ(s) { s (if s≤δ); 2√δ√s - δ (if sδ) }线性增长对中等外点有效计算简单。Cauchyρ(s) δ² log(1 s/δ²)对数增长对严重外点抑制力更强但可能导致收敛变慢。Tukeyρ(s) { (δ²/6)[1-(1-s/δ²)³] (if s≤δ²); δ²/6 (if 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计算量太大。通常采用分层策略前端三角化对每一对新匹配的特征点使用线性方法或快速的非线性优化只优化该点得到一个初始三维点。局部BA维护一个滑动窗口当新帧到来时对窗口内的所有相机位姿和能观测到的地图点进行联合优化。这一步会用到我们讨论的非线性最小二乘方法并利用稀疏性加速。全局BA/Pose Graph优化在回环检测发生后或者定期地进行规模更大的优化以消除累积误差。此时可能会用更简化的位姿图模型来替代完整的BA以提升速度。三角化的非线性优化是这个庞大优化系统中的基础原子操作。它的稳定性和精度直接决定了上层建筑的质量。最后再分享一个调试中的小技巧当你怀疑三角化或BA的结果有问题时可视化重投影误差是最直接的诊断方法。把优化后的三维点重新投影到图像上并用不同颜色如绿色表示误差小红色表示误差大画出连线。你能一眼看出哪些匹配可能是错误的哪些相机位姿估计有问题。这种视觉反馈对于快速定位问题至关重要。