MLPnPsolver::mlpnp_gn

函数功能用带加权的高斯‑牛顿法精细优化相机位姿旋转 平移最小化由零空间约束构建的残差。输入参数x位姿参数向量6×1前 3 维为罗德里格斯旋转向量后 3 维为平移向量。既是输入初值也是输出结果。pts3D 点数组。nullspaces每个 bearing vector 的零空间矩阵N_i3×2满足N_i^T f_i 0。Kll信息矩阵加权矩阵形状2n×2n分块对角。use_cov是否使用协方差加权。x x - dx说明其内部雅可比或残差定义了相反的符号我们只需跟随代码逻辑。// 高斯‑牛顿优化函数 void MLPnPsolver::mlpnp_gn(Eigen::VectorXd x, const points_t pts, const std::vectorEigen::MatrixXd nullspaces, const Eigen::SparseMatrixdouble Kll, bool use_cov) { const int numObservations pts.size(); // 观测点数量 n const int numUnknowns 6; // 未知数维度旋转3 平移3 // 检查冗余方程数 2n 必须大于未知数 6 assert((2 * numObservations - numUnknowns) 0); // // 初始化矩阵和向量 // Eigen::VectorXd r(2 * numObservations); // 残差向量 r (2n) Eigen::VectorXd rd(2 * numObservations); // 未使用可能是调试用 Eigen::MatrixXd Jac(2 * numObservations, numUnknowns); // 雅可比矩阵 J (2n×6) Eigen::VectorXd g(numUnknowns, 1); // 梯度向量 g J^T P r (6×1) Eigen::VectorXd dx(numUnknowns, 1); // 增量 Δx (6×1) Jac.setZero(); // 雅可比清零 r.setZero(); // 残差清零 dx.setZero(); // 增量清零 g.setZero(); // 梯度清零 int it_cnt 0; // 迭代计数器 bool stop false; // 收敛标志 const int maxIt 5; // 最大迭代次数MLPnP 初值已较好少量迭代即可 double epsP 1e-5; // 增量步长阈值检查 dl J*dx 的变化 Eigen::MatrixXd JacTSKll; // J^T P (若 use_cov) 或 J^T Eigen::MatrixXd A; // 近似 Hessian 矩阵 H J^T P J (6×6) // 高斯‑牛顿迭代主循环 while (it_cnt maxIt !stop) { // 计算当前位姿下的残差 r 和雅可比 Jac // 函数内部根据 x(旋转向量) 和 x(平移) 计算 R, t然后对每个点计算 // r_i N_i^T (R*P_i t) (2×1) // Jac_i ∂r_i/∂x (2×6) mlpnp_residuals_and_jacs(x, pts, nullspaces, r, Jac, true); // 构造加权雅可比转置JacTSKll J^T P (如果使用协方差) 否则 J^T if (use_cov) JacTSKll Jac.transpose() * Kll; // J^T * P else JacTSKll Jac.transpose(); // J^T A JacTSKll * Jac; // 法方程矩阵H J^T P J // 计算右端项g J^T P r 梯度的一半 g JacTSKll * r; // 求解增量dx H^{-1} g 注意代码中为 dx但更新为 x x - dx等价于 -H^{-1}g见下文解释 Eigen::LDLTEigen::MatrixXd chol(A); // LDLT 分解对称正定 dx chol.solve(g); // 解 H dx g // 防止错误收敛如果增量过大旋转5 rad 或平移1可能是错误的线性估计跳出 if (dx.array().abs().maxCoeff() 5.0 || dx.array().abs().minCoeff() 1.0) break; // 计算观测空间的增量dl J * dx (用于判断收敛) Eigen::MatrixXd dl Jac * dx; if (dl.array().abs().maxCoeff() epsP) { // 若残差变化足够小标记收敛 stop true; x x - dx; // 更新位姿 break; } else x x - dx; // 更新位姿 it_cnt; // 迭代计数加1 }//while // 结果通过引用 x 返回 }核心公式详解整体流程接收初值x由上一步 SVD 阶段提供。循环至多 5 次计算残差和雅可比。组装法方程矩阵和右端项。求解线性方程组得增量。若增量太大疑似错误则跳出。更新位姿检查收敛。输出优化后的x。该函数是 MLPnP 算法的最后一步将闭式解提升到极大似然意义下的最优解极大提高了精度和鲁棒性。