从线性三角化到非线性优化：三维重建中的重投影误差最小化

1. 项目概述从稀疏点到三维世界在计算机视觉、机器人定位或者摄影测量领域我们常常会面对这样一个问题手里只有几张从不同角度拍摄的二维照片以及照片上一些稀疏的、彼此对应的像素点我们如何恢复出这些点在真实三维空间中的位置这个过程就是“三角化”。听起来像是几何题但在实际工程中它从来不是一道简单的、有唯一解的代数题。因为相机标定有误差特征点检测有亚像素级的偏差这些噪声让我们的观测方程变得“不可靠”。直接套用线性最小二乘求解就像用一把刻度不准的尺子去量东西结果往往差强人意。这时候“非线性优化”就登场了。它不再满足于得到一个在代数上“误差平方和最小”的解而是直面问题的本质我们有一个关于三维点坐标的非线性观测模型相机投影模型以及带噪声的二维观测数据。非线性优化的目标就是调整三维点的坐标使得根据模型“重投影”回二维图像上的点与实际的观测点之间的误差最小。这更像是一个“校准”过程通过迭代调整让模型预测无限逼近真实观测。今天要聊的就是这个将线性三角化结果作为“初值”再通过非线性优化进行“精修”的完整流程。这不仅是提升三维重建精度的关键一步更是理解如何将理论模型应用于嘈杂现实世界的绝佳案例。2. 核心思路为何线性解只是开始在深入非线性优化之前我们必须先理解为什么线性三角化比如直接线性变换DLT或SVD方法给出的解不够好。这关乎我们对问题本质的认识。2.1 线性方法的局限与误差来源线性方法的核心是将相机投影矩阵P包含内参、旋转和平移与三维点齐次坐标X的乘法关系P X xx为归一化平面坐标或像素坐标展开利用叉乘消去尺度因子构造出形如A X 0的线性方程组。通过SVD求解最小奇异值对应的右奇异向量得到X。这个方法简洁优美但它隐含了两个重要的假设也是其误差的主要来源代数误差 vs. 几何误差线性最小二乘最小化的是代数误差即 ||A X||^2但这并不是我们关心的物理误差。我们真正关心的是几何误差即三维点X投影到图像上的二维点 (u_pred, v_pred) 与实际观测到的二维点 (u_obs, v_obs) 之间的欧氏距离。在投影模型中这两者是非线性关系。最小化代数误差并不能保证几何误差也最小。各向同性的噪声假设线性方法在构造方程时默认像素坐标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) Σ_{i1}^{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严重依赖于初始值。一个糟糕的初值可能导致算法收敛到局部极小值甚至发散。因此获取一个尽可能靠近真值的线性解至关重要。数据准备确保你有至少两个视图相机对同一个三维点的观测。每个观测是像素坐标(u, v)。同时你需要每个相机对应的投影矩阵P如果是像素坐标P是3x4矩阵包含了内参和位姿如果是归一化坐标则使用本质矩阵或直接使用旋转平移。线性三角化DLT方法对于每个观测利用叉乘x × (P X) 0构造两个线性方程。将多个视图的方程堆叠形成超定方程组A X 0。SVD求解对矩阵A进行奇异值分解SVDA U Σ V^T。解X即为V矩阵最后一列对应最小奇异值的前三个分量第四个分量为齐次坐标尺度因子需要归一化例如使第四维为1得到三维欧氏坐标。处理退化情况如果所有相机光心与三维点几乎共线矩阵A的条件数会很大解不稳定。实践中可以通过检查SVD的最小奇异值与次小奇异值的比值来判断。如果比值太小如小于1e-5则该点的三角化结果不可靠应考虑剔除。这个线性解X_linear就是我们给非线性优化准备的“起跑线”。3.2 第二步构建非线性优化问题现在我们以X_linear为初始值构建并求解非线性最小二乘问题。这里以最常用的列文伯格-马夸尔特算法为例因为它兼具高斯-牛顿法的快速收敛和梯度下降法的稳定性。定义参数块与残差块参数块待优化的变量即三维点坐标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)。目标函数总目标函数为所有残差项的平方和F(X) 0.5 * Σ ||r_i(X)||^2。系数0.5是为了后续求导方便不影响最优解位置。核心雅可比矩阵计算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算法的迭代步骤如下初始化X X_linear 设置阻尼因子λ为一个初始值如1e-3以及缩放因子v如10。对于第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_{k1} X_new并减小阻尼因子λ λ / max(1/3, 1 - (2ρ-1)^3) v2。这样下一步更接近高斯-牛顿法收敛更快。 * 如果ρ很小如0.25说明二次模型拟合差拒绝更新X_{k1} X_k并增大阻尼因子λ λ * vv 2 * v。这样下一步更接近梯度下降法步长更小更稳定。 * 如果ρ在中间接受更新但保持λ不变。判断收敛当满足以下条件之一时停止迭代参数增量δ的范数小于阈值如1e-6。目标函数下降量ΔF_actual的绝对值小于阈值如1e-9。梯度J^T r的范数小于阈值如1e-6。达到最大迭代次数如50。经过若干次迭代算法输出的X_final就是非线性优化后的三维点坐标其重投影误差理论上比线性解X_linear更小。4. 关键实现细节与参数调优理论流程清晰了但魔鬼在细节里。要让这套流程稳定高效地跑起来有几个关键点必须处理好。4.1 投影与畸变模型project(P_i, X)函数的具体实现直接影响优化精度。一个完整的投影流程通常包括世界系到相机系X_cam R * X t。R, t是相机外参。相机系到归一化平面x_norm X_cam / Z_camy_norm Y_cam / Z_cam。这里得到了无畸变的归一化坐标。径向和切向畸变校正这是主要的非线性部分。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_normk1, k2, k3为径向畸变系数p1, p2为切向畸变系数。归一化平面到像素平面u_pred f_x * x_dist c_x v_pred f_y * y_dist c_yf_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::AutoDiffCostFunctionReprojectionError, 2, 3( new ReprojectionError(observation.u, observation.v, observation.camera)); problem.AddResidualBlock(cost_function, new ceres::HuberLoss(5.0), // 鲁棒核delta5.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 优化不收敛或结果变差这是最令人头疼的问题。可以从以下方面排查初值太差线性三角化的结果可能已经“坏掉了”。检查该点在所有视图中的重投影误差用线性解计算。如果某个视图的误差巨大如100像素可能是特征匹配错误或者该视图的相机位姿P_i不准。尝试剔除误差最大的视图只用质量好的视图重新做线性三角化或者直接放弃这个点。雅可比矩阵错误这是非常隐蔽的错误。使用优化库如Ceres的数值差分检查功能CHECK开头的选项或者自己写一个中心差分的数值雅可比计算函数与解析雅可比在初始点附近进行比较。任何微小的不一致都可能导致优化路径偏离。尺度问题三维点坐标X、平移向量t的数值可能非常大或非常小导致Hessian矩阵J^T J的条件数很差。可以对三维点坐标进行归一化例如减去点云质心缩放到一个单位球内优化完成后再变换回去。或者在优化时使用更好的线性求解器如DENSE_SCHUR或SPARSE_NORMAL_CHOLESKY。外点干扰没有使用或错误使用了鲁棒核。确认鲁棒核函数的阈值设置合理。可以尝试先不用鲁棒核观察哪些点的残差巨大手动剔除它们后再优化。5.2 精度评估与对比如何量化非线性优化带来的提升一个标准的评估流程是计算重投影误差统计分别用线性解X_linear和非线性解X_nonlinear计算在所有观测视图上的重投影误差欧氏距离。对比指标平均误差mean_error Σ ||r_i|| / N_observations。误差中位数对误差排序取中位数对异常值不敏感。误差标准差反映误差的离散程度。最大误差观察最差点的情况。可视化将重投影误差向量即r_i在图像上画出来箭头从预测点指向观测点。这能直观地看到误差的方向和大小分布。一个健康的优化结果误差箭头应该短且方向随机如果出现一致的、方向性的误差可能暗示相机标定特别是畸变参数仍有问题。在我的一个多视图重建项目中对1000个三角化点进行非线性优化后平均重投影误差从线性解的1.8像素下降到了0.7像素误差中位数从1.2像素下降到了0.5像素。更重要的是最大误差从35像素由少数外点导致被压制到了5像素以内。鲁棒核函数功不可没。5.3 效率考量三角化通常是在SFM或SLAM流程中对成千上万个点逐一进行的。因此每个点的优化效率很重要。提前判断对于线性解重投影误差已经很小的点例如0.5像素可以跳过非线性优化直接使用线性解。这能节省大量计算。设置合理的收敛条件对于三角化这种小问题3个参数通常迭代10-20次就足够了。可以将最大迭代次数设为20梯度阈值设为1e-6。过严的收敛条件只会增加无谓的迭代。选择合适的线性求解器在Ceres中对于参数块只有3维的问题DENSE_QR或DENSE_NORMAL_CHOLESKY是最快、最稳定的选择。避免使用为大规模问题设计的迭代求解器。并行化各个三维点的优化是相互独立的这是天然的并行任务。可以使用OpenMP或线程池同时对多个点进行优化能极大提升整体三角化速度。6. 扩展与捆集调整的关系三角化的非线性优化可以看作是捆集调整的一个特例或子问题。完整的捆集调整同时优化所有相机参数位姿、内参和所有三维点坐标目标是最小化所有重投影误差之和。这是一个巨型的非线性最小二乘问题。而我们这里讨论的三角化非线性优化是在固定所有相机参数的前提下仅优化单个三维点的坐标。这相当于在捆集调整的大问题中固定其他所有变量只优化与某一个点相关的参数。因此它的原理、目标函数和优化算法LM与捆集调整是完全一致的。在实际的SFM流程中通常采用一种交替优化的策略增量式重建初始化两个视图三角化一批点。局部捆集调整用这些点和新加入的视图进行局部BA同时优化新视图的位姿和这些点的坐标。三角化新点用优化后的位姿三角化新的匹配点。全局捆集调整当相机和点积累到一定数量或者累计误差较大时进行一次全局BA。在这个流程中每一步的三角化无论是新点还是优化旧点其背后的非线性优化思想都是一脉相承的。理解了这个点的优化就为理解更复杂的捆集调整打下了坚实的基础。最后再分享一个调试小技巧在优化迭代时不仅打印目标函数值也打印三维点坐标的变化量。如果发现坐标在某个维度上发生剧烈跳动例如Z值从正变负那几乎可以肯定是初值问题或雅可比错误。此时将优化过程可视化在三维空间中画出每次迭代后点的位置轨迹能帮助你非常直观地理解优化器在“想”什么是定位问题根源的利器。