企业官网建设流程全解析

函数功能

用带加权的高斯‑牛顿法精细优化相机位姿（旋转 + 平移），最小化由零空间约束构建的残差。

输入参数：

• x：位姿参数向量（6×1），前 3 维为罗德里格斯旋转向量，后 3 维为平移向量。既是输入初值，也是输出结果。

• pts：3D 点数组。

• nullspaces：每个 bearing vector 的零空间矩阵N_i（3×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::vector<Eigen::MatrixXd> &nullspaces, const Eigen::SparseMatrix<double> 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::LDLT<Eigen::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 返回 }

核心公式详解

整体流程

1. 接收初值x（由上一步 SVD 阶段提供）。

2. 循环至多 5 次：

   • 计算残差和雅可比。

   • 组装法方程矩阵和右端项。

   • 求解线性方程组得增量。

   • 若增量太大（疑似错误）则跳出。

   • 更新位姿，检查收敛。

3. 输出优化后的x。

该函数是 MLPnP 算法的最后一步，将闭式解提升到极大似然意义下的最优解，极大提高了精度和鲁棒性。