陕访惹玫在前两篇文章《最小二乘问题详解10PnP问题求解》和《最小二乘问题详解11基于李代数的PnP优化》中我们分别通过常规思想与李代数思想深入探讨了计算机视觉中 SFMStructure from Motion系统的核心子问题之一——PnP 问题。该问题建模于针孔成像原理本质上是利用单视图中的2D-3D对应关系求解相机位姿常被归入单视图几何的范畴。然而仅靠单视图无法恢复场景的三维结构深度信息在投影过程中永久丢失。要重建真实世界必须引入多视图几何Multi-view Geometry。而在多视图框架下最基础、最关键的优化问题之一便是三角化Triangulation——即在已知多个相机位姿的前提下通过多视角下的同名点观测反推空间中对应 3D 点的位置。三角化看似简单却是 SfM 和 SLAM 系统中“结构恢复”的基础本文要讲解就是三角化问题的非线性优化求解方法。2 问题建模2.1 三角化定义在计算机视觉中三角化Triangulation是指已知多个相机的位姿和同一空间点在各图像中的观测位置求解该点在世界坐标系下的三维坐标。其名称源于几何直观从两个或多个相机光心向对应的图像点作射线这些射线理论上应交于空间中的同一点——形成一个“三角形”。在理想无噪声情况下两射线精确相交但在实际中由于位姿误差、特征匹配噪声等射线往往不交此时需通过优化找到“最佳”交点。需要注意的是三角化假设相机位姿已知通常由 PnP 或 SfM 前端提供因此它是一个纯结构恢复问题与 PnP已知结构求位姿互为对偶。2.2 成像模型回顾回顾一下《最小二乘问题详解10PnP问题求解》和《最小二乘问题详解11基于李代数的PnP优化》中提到的针孔相机成像模型。设某空间点在世界坐标系下的位置为X[X,Y,Z]?∈R3对于第i个相机其位姿由旋转矩阵Ri∈SO(3)和平移向量ti∈R3描述即世界到相机的变换。该点在第i幅图像上的投影像素坐标ui[ui,vi]?满足si???uivi1???Ki(RiXti)(1)其中Ki为第i个相机的内参矩阵通常假设已标定且恒定记为Ksi为未知尺度因子深度。去齐次化后得到重投影函数π(X;Ri,ti,K)???????fx?r?i1Xtixr?i3Xtizcxfy?r?i2Xtiyr?i3Xtizcy???????(2)其中r?ij表示Ri的第j行。2.3 优化目标函数设某空间点被N≥2个视角观测到对应图像坐标为{u1,u2,…,uN}相机位姿为{(R1,t1),…,(RN,tN)}。我们的目标是找到一个X∈R3使得其在所有视角下的重投影结果尽可能接近观测值。由此定义残差向量ri(X)π(X;Ri,ti,K)?ui∈R2(3)最终的非线性最小二乘问题为minX∈R3N∑i1∥ri(X)∥2minXN∑i1∥π(X;Ri,ti,K)?ui∥2(4)可以看到与 PnP 问题相比三角化问题的优化还相对简单一点PnP 优化变量是T∈SE(3)需李代数处理三角化优化变量是X∈R3在普通欧氏空间即可求解无需流形。当然由于π(?)中存在分母r?i3Xtiz整体为非线性、非凸函数需借助迭代优化方法求解。3 线性三角化DLT 方法尽管三角化的本质是非线性的见式 (4)但在实际应用中我们常先使用一种线性近似方法快速获得初值这就是直接线性变换Direct Linear Transform, DLT。DLT 的核心思想是将透视投影方程转化为齐次线性方程组并通过 SVD 求解。3.1 齐次方程的构造回顾成像方程 (1)siuhomiK(RiXti)其中uhomi[ui,vi,1]?。令PiK[Ri∣ti]∈R3×4为第i个相机的投影矩阵并将世界点表示为齐次坐标~X[X,Y,Z,1]?∈R4则上式可简写为siuhomiPi~X(5)由于等式两边相差一个未知尺度si我们可以利用叉积消去尺度uhomi×(Pi~X)0(6)展开叉积设p?i1,p?i2,p?i3为Pi的三行????vi(p?i3~X)?(p?i2~X)(p?i1~X)?ui(p?i3~X)ui(p?i2~X)?vi(p?i1~X)????0注意到第三行是前两行的线性组合因叉积秩为2因此只需取前两行作为独立约束(uip?i3?p?i1)~X0(vip?i3?p?i2)~X0(7)对每个视角i我们得到两个线性方程。若共有N个视角则可堆叠成一个2N×4的设计矩阵AA~X0,A??????????u1p?13?p?11v1p?13?p?12?uNp?N3?p?N1vNp?N3?p?N2??????????∈R2N×4(8)3.2 求解与归一化方程A~X0构成一个齐次线性系统。在《最小二乘问题详解2线性最小二乘求解》中我们已系统讨论了线性最小二乘问题的一般形式min∥Ax?b∥2及其求解方法。然而DLT 所面对的是该框架下的一个特殊情形b0。由于齐次方程的解在尺度上不确定若~X是解则任意缩放λ~X也是解直接最小化∥A~X∥会退化为无意义的平凡解~X0。因此我们必须施加单位范数约束∥~X∥1以在单位球面上寻找使∥A~X∥最小的非零向量——即具有单位长度的最优方向。这一目标等价于min∥~X∥1∥A~X∥2对A∈R2N×4进行奇异值分解AUΣV?,Σdiag(σ1,σ2,σ3,σ4),σ1≥σ2≥σ3≥σ4≥0由于V[v1,v2,v3,v4]是正交矩阵其列向量构成R4的一组标准正交基。因此任何满足∥~X∥1的候选解均可唯一表示为~X4∑i1αivi,其中4∑i1α2i1由于U是正交矩阵满足∥UΣ∥∥Σ∥因此范数计算中U可被消去。此时有∥A~X∥2∥∥UΣV?~X∥∥2∥∥∥∥∥∥Σ?????α1α2α3α4?????∥∥∥∥∥∥24∑i1σ2iα2i为使该式最小在∑α2i1约束下应将全部权重分配给最小奇异值对应的分量即取α41其余αi0。因此最优解为~Xv4V(:,4)?? 为何必须用 SVD而不能用 QRQR 分解适用于非齐次最小二乘问题b≠0其核心是通过正交变换将问题转化为上三角系统求解。但在齐次情形A~X0下QR 无法直接揭示矩阵的零空间结构。只有 SVD 能显式给出所有奇异值及其对应的奇异向量从而可靠地提取出使∥A~X∥最小的单位向量——这正是 DLT 所需的解。最后需对解进行去齐次化dehomogenization。这是因为~X[~Xx,~Xy,~Xz,~Xw]?是齐次坐标仅定义射影空间中的方向而实际 3D 点位于欧氏空间。根据齐次坐标的定义其对应的欧氏坐标为XDLT1~Xw[~Xx,~Xy,~Xz]?(9)若~Xw≈0说明点在无穷远处通常应舍弃若~Xw0则可能对应负深度点位于相机后方需结合相机位姿进行符号校正。3.3 DLT 的缺陷分析尽管 DLT 实现简单、计算高效但它存在几个根本性缺陷限制了其精度优化目标不合理DLT 最小化的是∥A~X∥2即代数残差。该残差混合了像素坐标、深度和尺度没有明确的几何或物理意义。相比之下我们真正关心的是重投影误差单位像素如式 (4) 所示。符号模糊性由于A(?~X)?A~X0若~X是解则?~X也是解。这导致 DLT 可能输出负深度的点位于相机后方必须通过深度符号校正检查(RiXti)z0才能得到合理结果。对噪声高度敏感代数误差对图像噪声缺乏鲁棒性。即使添加少量像素噪声如 1 像素DLT 解也可能严重偏离真值。这是因为 DLT 未考虑透视投影的非均匀性——相同的空间误差在不同深度产生的像素误差不同但 DLT 对所有方程平等对待。因此在实际应用中DLT 通常仅作为非线性最小二乘优化的初值——它计算快速但精度有限。要获得高精度的三维点我们必须回到几何本质最小化重投影误差。4 非线性三角化最小化重投影误差如第2节所述三角化的理想目标是最小化重投影误差式 (4)minX∈R3S(X)N∑i1∥π(X;Ri,ti,K)?ui∥2该问题是典型的非线性最小二乘问题。根据《最小二乘问题详解4非线性最小二乘》和《最小二乘问题详解8Levenberg-Marquardt方法》中的框架其求解依赖于对残差函数ri(X)的一阶泰勒展开而展开的核心正是雅可比矩阵Ji?ri?X?。尽管现代优化库如 Ceres支持自动微分但手动推导雅可比不仅能加深对几何模型的理解还能在自定义优化器或性能敏感场景中提供关键优势。下面我们详细推导该雅可比矩阵。4.1 重投影函数的显式形式为简化记号令第i个相机的外参变换为X(i)cRiXti???xcyczc???则重投影函数式 (2)可写为π(X)[uv][fxxczccxfyyczccy]残差为ri(X)[rurv][fxxczccx?uifyyczccy?vi]4.2 雅可比矩阵推导我们需要计算Ji?ri?X????ru?X?ru?Y?ru?Z?rv?X?rv?Y?rv?Z??∈R2×3利用链式法则?ru?X?ru?xc?xc?X?ru?zc?zc?X首先计算中间偏导?ru?xcfxzc,?ru?zc?fxxcz2c?rv?ycfyzc,?rv?zc?fyycz2c而?xc?Xr?i1,?yc?Xr?i2,?zc?Xr?i3其中r?ij是Ri的第j行。因此最终雅可比矩阵为Ji???fxzcr?i1?fxxcz2cr?i3fyzcr?i2?fyycz2cr?i3???(10)或等价地写成Ji???fxz2c(zcr?i1?xcr?i3)fyz2c(zcr?i2?ycr?i3)???雅可比矩阵描述了3D 点微小扰动如何影响图像观测。在这里可以看到分母z2c表明深度越大zc越大图像对 3D 扰动越不敏感——这正是透视投影的非均匀性体现也是 DLT 忽略的关键信息。4.3 整体优化流程有了残差ri(X)和雅可比Ji即可构建整体残差向量r(X)∈R2N和雅可比矩阵J(X)∈R2N×3将所有Ji垂直堆叠。随后可采用 Gauss-Newton 或 Levenberg-Marquardt 方法迭代求解详见《最小二乘问题详解4非线性最小二乘》和《最小二乘问题详解8Levenberg-Marquardt方法》(J?J)ΔX?J?r(GN)或(J?JλI)ΔX?J?r(LM)5 实例根据前文的理论推导我们完整实现了基于 DLT 初值估计与非线性优化的三角化流程。具体代码如下#include#include#include#include#include#include#includeconstexpr double PI 3.14159265358979323846;// 投影函数将世界坐标 X 投影到图像平面对应式 (1)Eigen::Vector2d Project(const Eigen::Matrix3d K, const Eigen::Matrix3d R_i,const Eigen::Vector3d t_i,const Eigen::Vector3d X_world) {// 相机坐标系下的点: X_c R_i * X t_i 式 (1) 中间步骤Eigen::Vector3d X_cam R_i * X_world t_i;// 像素齐次坐标: s * [u, v, 1]^T K * X_camEigen::Vector3d px_hom K * X_cam;// 去齐次化式 (2)return Eigen::Vector2d(px_hom.x() / px_hom.z(), px_hom.y() / px_hom.z());}// DLT 三角化基于式 (5)-(8)Eigen::Vector3d TriangulateDLT(const std::vector Rs,const std::vector ts,const std::vector observations,const Eigen::Matrix3d K) {size_t N Rs.size();Eigen::MatrixXd A(2 * N, 4); // 式 (8): A ∈ ?^{2N×4}for (size_t i 0; i N; i) {// 构造完整的投影矩阵 P_i K [R_i | t_i] ∈ ?^{3×4} 式 (5)Eigen::Matrix P_i;P_i.block3, 3(0, 0) K * Rs[i]; // K * R_iP_i.col(3) K * ts[i]; // K * t_idouble u_i observations[i].x(); // 观测像素 u_idouble v_i observations[i].y(); // 观测像素 v_i// 构造 DLT 约束式 (7):// (u_i * p_{i3}^T - p_{i1}^T) * X_tilde 0// (v_i * p_{i3}^T - p_{i2}^T) * X_tilde 0A.row(2 * i) u_i * P_i.row(2) - P_i.row(0);A.row(2 * i 1) v_i * P_i.row(2) - P_i.row(1);}// SVD 求解 min ||A X_tilde|| s.t. ||X_tilde||1 式 (9) 前Eigen::JacobiSVD svd(A, Eigen::ComputeFullV);Eigen::Vector4d X_tilde svd.matrixV().col(3); // 对应 v_4// 齐次坐标 X_tilde [X, Y, Z, W]^T → 欧氏坐标 [X/W, Y/W, Z/W]^Tif (std::abs(X_tilde.w()) 1e-8) {// 点在无穷远处无法有效三角化返回原点或可抛异常return Eigen::Vector3d::Zero();}Eigen::Vector3d X_euclid(X_tilde.x() / X_tilde.w(), X_tilde.y() / X_tilde.w(),X_tilde.z() / X_tilde.w());// 符号校正由于 X_tilde 和 -X_tilde 都是齐次解// 去齐次化后对应 X_euclid 和 -X_euclid需选择使更多相机看到正深度的解Eigen::Vector3d X1 X_euclid;Eigen::Vector3d X2 -X_euclid;auto count_positive_depth [](const Eigen::Vector3d X) - int {int cnt 0;for (size_t i 0; i N; i) {double z_cam (Rs[i] * X ts[i]).z(); // (R_i X t_i)_zif (z_cam 0) cnt;}return cnt;};Eigen::Vector3d X_dlt (count_positive_depth(X1) count_positive_depth(X2)) ? X1 : X2;// 最终保障至少第一个相机深度为正if ((Rs[0] * X_dlt ts[0]).z() 0) {X_dlt -X_dlt;}return X_dlt;}// Ceres 残差块重投影误差对应式 (3)struct ReprojectionError {ReprojectionError(const Eigen::Vector2d u_obs, const Eigen::Matrix3d K,const Eigen::Matrix3d R_i, const Eigen::Vector3d t_i): u_obs_(u_obs), K_(K), R_i_(R_i), t_i_(t_i) {}templatebool operator()(const T* const X_world, T* residuals) const {// X_world: 优化变量对应式 (4) 中的 X ∈ ?^3Eigen::Map X(X_world);// 转换到相机坐标系: X_cam R_i * X t_iEigen::Matrix R_i_T R_i_.template cast();Eigen::Matrix t_i_T t_i_.template cast();Eigen::Matrix X_cam R_i_T * X t_i_T;// 投影到像素平面式 (2)Eigen::Matrix K_T K_.template cast();Eigen::Matrix px_hom K_T * X_cam;T u_proj px_hom[0] / px_hom[2]; // f_x * x_c / z_c c_xT v_proj px_hom[1] / px_hom[2]; // f_y * y_c / z_c c_y// 残差 投影值 - 观测值式 (3)residuals[0] u_proj - T(u_obs_.x());residuals[1] v_proj - T(u_obs_.y());return true;}static ceres::CostFunction* Create(const Eigen::Vector2d u_obs,const Eigen::Matrix3d K,const Eigen::Matrix3d R_i,const Eigen::Vector3d t_i) {return new ceres::AutoDiffCostFunction(new ReprojectionError(u_obs, K, R_i, t_i));}private:Eigen::Vector2d u_obs_; // 观测像素坐标 [u_i, v_i]^TEigen::Matrix3d K_, R_i_; // 内参、旋转Eigen::Vector3d t_i_; // 平移};int main() {// 相机内参 K式 (1)double f_x 800.0, f_y 800.0, c_x 320.0, c_y 240.0;Eigen::Matrix3d K;K f_x, 0, c_x, 0, f_y, c_y, 0, 0, 1;// 真实3D点 X_gt世界坐标Eigen::Vector3d X_gt(1.2, -0.5, 3.0);// 相机位姿 {R_i, t_i} std::vector Rs;std::vector ts;// 相机1: 单位位姿Rs.push_back(Eigen::Matrix3d::Identity());ts.push_back(Eigen::Vector3d::Zero());// 相机2: 绕Y轴旋转30度平移(0.5, 0, 0)double angle PI / 6.0;Eigen::AngleAxisd rot(angle, Eigen::Vector3d::UnitY());Rs.push_back(rot.toRotationMatrix());ts.push_back(Eigen::Vector3d(0.5, 0.0, 0.0));// 相机3: 绕Y轴旋转 -20 度平移 (-0.3, 0.1, 0.2)Rs.push_back(Eigen::AngleAxisd(-PI / 9.0, Eigen::Vector3d::UnitY()).toRotationMatrix());ts.push_back(Eigen::Vector3d(-0.3, 0.1, 0.2));// 生成带噪声观测 {u_i} std::vector observations_clean;for (size_t i 0; i Rs.size(); i) {Eigen::Vector2d proj Project(K, Rs[i], ts[i], X_gt);observations_clean.push_back(proj);}std::mt19937 gen(42);std::normal_distribution noise(0.0, 2); // 2像素高斯噪声std::vector observations;for (const auto obs : observations_clean) {double u_noisy obs.x() noise(gen);double v_noisy obs.y() noise(gen);observations.emplace_back(u_noisy, v_noisy);}std::cout std::fixed std::setprecision(6);std::cout Ground Truth 3D Point X_gt std::endl;std::cout [ X_gt.x() , X_gt.y() , X_gt.z() ] std::endl;// 1. DLT 初值 Eigen::Vector3d X_dlt TriangulateDLT(Rs, ts, observations, K);std::cout \n DLT Estimate X_dlt std::endl;std::cout [ X_dlt.x() , X_dlt.y() , X_dlt.z() ] std::endl;// 2. 非线性优化最小化式 (4)double X_opt[3] {X_dlt.x(), X_dlt.y(), X_dlt.z()}; // 初值ceres::Problem problem;for (size_t i 0; i Rs.size(); i) {problem.AddResidualBlock(ReprojectionError::Create(observations[i], K, Rs[i], ts[i]), nullptr,X_opt);}ceres::Solver::Options options;options.linear_solver_type ceres::DENSE_QR;options.minimizer_progress_to_stdout true;options.max_num_iterations 20;ceres::Solver::Summary summary;ceres::Solve(options, problem, summary);std::cout \n summary.BriefReport() \n;Eigen::Vector3d X_est(X_opt[0], X_opt[1], X_opt[2]);std::cout Nonlinear Optimization Estimate X_est std::endl;std::cout [ X_est.x() , X_est.y() , X_est.z() ] std::endl;// 3. 评估 double dlt_error (X_gt - X_dlt).norm();double opt_error (X_gt - X_est).norm();double total_reproj_err_sq 0.0;for (size_t i 0; i Rs.size(); i) {Eigen::Vector2d proj Project(K, Rs[i], ts[i], X_est);double err (proj - observations[i]).norm();total_reproj_err_sq err * err;}double reproj_rmse std::sqrt(total_reproj_err_sq / Rs.size());std::cout \n Evaluation std::endl;std::cout DLT 3D error: dlt_error meters std::endl;std::cout Optimized 3D error: opt_error meters std::endl;std::cout Final reprojection RMSE: reproj_rmse pixels std::endl;return 0;}该实现遵循典型的视觉三维重建范式先通过 DLTDirect Linear Transform快速获得一个闭式初值再以该初值为起点利用 Ceres Solver 对重投影误差进行非线性最小二乘优化从而在几何意义上获得更精确的 3D 点估计。整个流程与《最小二乘问题详解9使用Ceres求解非线性最小二乘》中介绍的通用优化框架完全一致。值得注意的是DLT 求解的是齐次坐标下的代数最小二乘问题其解在符号上具有天然的二义性——即若~X是解则?~X同样满足方程。然而只有其中一个符号对应物理上合理的 3D 点即在所有或大多数相机前方。因此代码中专门加入了符号校正机制通过统计各视角下点的深度z分量是否为正选择使更多相机看到“正深度”的解并进一步确保第一个相机的深度为正以消除歧义。本实验设置了三个视角主相机位于原点第二、第三相机分别绕 Y 轴旋转 ±30°/20° 并施加小幅平移构成良好的三角化几何结构。同时我们在理想投影上叠加了标准差为 2 像素的高斯噪声以模拟实际特征匹配中的观测误差。程序运行输出如下 Ground Truth 3D Point X_gt [1.200000, -0.500000, 3.000000] DLT Estimate X_dlt [1.195321, -0.500514, 2.982901]iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time0 3.862688e00 0.00e00 6.30e02 0.00e00 0.00e00 1.00e04 0 9.30e-05 2.91e-041 3.171789e00 6.91e-01 4.62e-01 0.00e00 1.00e00 3.00e04 1 1.12e-04 5.51e-04Ceres Solver Report: Iterations: 2, Initial cost: 3.862688e00, Final cost: 3.171789e00, Termination: CONVERGENCE Nonlinear Optimization Estimate X_est [1.195772, -0.498440, 2.983621] Evaluation DLT 3D error: 0.017735 metersOptimized 3D error: 0.016988 metersFinal reprojection RMSE: 1.454141 pixels可以看到此处 DLT 的 3D 误差略大于优化结果0.0177 0.0169表明非线性优化确实带来了改进。不过DLT 的初值已经非常接近真值非线性优化带来的 3D 位置改进相对微小约 4%。这是因为当前三角化条件良好视角夹角适中、基线合理、噪声水平较低。另外从输出可见优化后的重投影 RMSE 已成功降低说明优化器确实更好地拟合了带噪观测数据。在真实应用中三角化精度高度依赖于多视图几何构型。当面临小基线、大噪声、弱纹理或极端视角等挑战性场景时DLT 的代数误差会显著放大导致深度估计严重失真而基于重投影误差的非线性优化则能通过合理的几何约束保持鲁棒性与精度优势将更加明显。6. 问题尽管三角化包括 DLT 和非线性优化是多视图几何中的核心工具但在实际应用中其可靠性受到多重因素的制约视角夹角过小小基线当两个相机光心距离很近或观察方向几乎平行时反投影光线近乎平行导致深度方向上的不确定性急剧增大即“三角化病态”。此时即使使用非线性优化3D 估计也会对噪声极度敏感误差可能高达数米。观测点位于图像边缘或遮挡区域特征匹配在这些区域本身不可靠容易引入大偏差观测从而误导优化过程。纹理缺失或重复纹理区域在这些区域特征点定位不准等效于引入了大噪声输入同样破坏了三角化的前提条件。动态物体或非刚性场景这类场景违反了“静态点在多视图中一致”的基本假设使得三角化结果无意义。基于以上且不局限于以上的现实工程环境单纯依赖两视图或多视图三角化可能失效。实践中常采用以下策略来提高三角化的鲁棒性和精度设置三角化角度阈值如 1°~2°仅对满足几何条件的点进行三角化。这有助于筛选出那些具有足够立体信息的点避免处理病态情况。结合先验信息例如单目深度估计、立体匹配或 RGB-D 传感器提供的深度图。这些额外的信息可以在三角化之前提供可靠的初始估计增强系统的稳定性。引入鲁棒核函数如 Huber、Cauchy以抑制异常观测的影响。这种方法能够在一定程度上减轻大偏差观测对最终解的影响提高整体的鲁棒性。将三角化嵌入更大的优化框架如全局 Bundle AdjustmentBA。通过联合优化相机位姿与 3D 点提升整个系统的几何一致性进一步减少累积误差。对于无法三角化的点延迟初始化或直接放弃避免错误的3D点污染地图。这种做法可以防止由于不准确的三角化结果带来的连锁错误确保系统输出的高质量。