MLPnPsolver::computePose

//MLPnP methods // 函数功能用最大似然 PnP 估计相机位姿旋转 R 和平移 t // 输入 // f : 归一化图像 bearing vectors (单位方向向量) // p : 对应的 3D 点坐标 // covMats : 每个 bearing vector 的 3x3 协方差矩阵表示测量不确定性 // indices : 本次使用的对应点索引 // 输出 // result : 4x4 变换矩阵 [R | t; 0 0 0 1]注意 opengv 惯例这里存储的是相机到世界的变换的逆实际是相机位姿见末尾 void MLPnPsolver::computePose(const bearingVectors_t f, const points_t p, const cov3_mats_t covMats, const std::vectorint indices, transformation_t result) { size_t numberCorrespondences indices.size(); // 参与计算的对应点数量 assert(numberCorrespondences 5); // 至少需要 6 个点平面情况可 5 个程序要求 5 bool planar false; // 场景是否为平面标志 // 计算每个 bearing vector 的零空间矩阵用于消去深度 λ std::vectorEigen::MatrixXd nullspaces(numberCorrespondences); Eigen::MatrixXd points3(3, numberCorrespondences); // 存储 3D 点列向量 points_t points3v(numberCorrespondences); // 同时备份一份点用于后续重投影 points4_t points4v(numberCorrespondences); for (size_t i 0; i numberCorrespondences; i) { bearingVector_t f_current f[indices[i]]; // 当前 bearing vector points3.col(i) p[indices[i]]; // 存入点矩阵用于平面检测 // 零空间计算对 f_current^T 1x3做 SVD // 公式f U * S * V^Tf 是 3x1S 只有一个非零奇异值。 // V 的后两列对应奇异值 0张成 f 的零空间即 N 满足 N^T f 0。 Eigen::JacobiSVDEigen::MatrixXd, Eigen::HouseholderQRPreconditioner svd_f(f_current.transpose(), Eigen::ComputeFullV); nullspaces[i] svd_f.matrixV().block(0, 1, 3, 2); // 取出后两列组成 3x2 矩阵 N_i points3v[i] p[indices[i]]; // 备份点 } ////////////////////////////////////// // 1. 测试是否为平面场景 ////////////////////////////////////// // 计算 3D 点协方差矩阵去中心化可忽略因为点未去均值但秩检测仍然有效 Eigen::Matrix3d planarTest points3 * points3.transpose(); Eigen::FullPivHouseholderQREigen::Matrix3d rankTest(planarTest); Eigen::Matrix3d eigenRot; eigenRot.setIdentity(); // 若平面用于将点转到特征坐标系 // 如果秩为 2说明点分布在平面上第三维退化 if (rankTest.rank() 2) { planar true; // 自伴特征分解planarTest V * D * V^TV 的列是特征向量 Eigen::SelfAdjointEigenSolverEigen::Matrix3d eigen_solver(planarTest); eigenRot eigen_solver.eigenvectors().real(); // 特征向量矩阵按特征值升序排列 eigenRot.transposeInPlace(); // 取转置使得变换后第三维对应最小特征值方向平面法向 // 将点变换到特征坐标系points3 eigenRot * points3 for (size_t i 0; i numberCorrespondences; i) points3.col(i) eigenRot * points3.col(i); } ////////////////////////////////////// // 2. 随机模型协方差加权 ////////////////////////////////////// // 构建信息矩阵 P权重矩阵对应每对点的两个方程2x2 块对角 Eigen::SparseMatrixdouble P(2 * numberCorrespondences, 2 * numberCorrespondences); bool use_cov false; P.setIdentity(); // 默认单位矩阵各向同性 if (covMats.size() numberCorrespondences) { use_cov true; int l 0; for (size_t i 0; i numberCorrespondences; i) { // 将原始 3x3 测量协方差变换到零空间约束下的 2x2 协方差 // 公式Cov_2D N_i^T * Cov_3D_i * N_i cov2_mat_t temp nullspaces[i].transpose() * covMats[i] * nullspaces[i]; temp temp.inverse().eval(); // 信息矩阵为协方差的逆 // 填入稀疏矩阵 P 的对应 2x2 块 P.coeffRef(l, l) temp(0, 0); P.coeffRef(l, l 1) temp(0, 1); P.coeffRef(l 1, l) temp(1, 0); P.coeffRef(l 1, l 1) temp(1, 1); l 2; } } ////////////////////////////////////// // 3. 构建设计矩阵 A ////////////////////////////////////// const int rowsA 2 * numberCorrespondences; // 每个点贡献两个方程 int colsA 12; // 非平面旋转9个 平移3个 12 Eigen::MatrixXd A; if (planar) { colsA 9; // 平面自由度减少只需要9个参数 A Eigen::MatrixXd(rowsA, 9); } else A Eigen::MatrixXd(rowsA, 12); A.setZero(); // 填充设计矩阵 // 原理对每个点方程 N_i^T (R*P_i t) 0 写成 A_i * x 0 // 其中 x 包含旋转矩阵元素按行或按列展开和平移向量。 // 此处将旋转矩阵元素按列优先排列[r11,r12,r13, r21,r22,r23, r31,r32,r33, t1,t2,t3]^T 非平面 // N_i 是 3x2N_i^T 的第一行与第二行分别提供一个方程。 if (planar) { // 平面情况经过特征变换后点的第三维坐标为0或常数因此涉及第三维的旋转项消失。 // 保留的旋转项为 r12,r13, r22,r23, r32,r336个以及平移3个共9个参数。 for (size_t i 0; i numberCorrespondences; i) { point_t pt3_current points3.col(i); // 变换后的点 [x,y,0]^T 实际第三维不一定严格0但近于0 // 按顺序填充 // r12, r13, r22, r23, r32, r33, t1, t2, t3 // 对应 x 向量的索引 0~8 // 第一行方程N_i(0,:) 与 (R*Pt) 的点积 // 第二行方程N_i(1,:) 与 (R*Pt) 的点积 A(2*i, 0) nullspaces[i](0,0) * pt3_current[1]; // r12 A(2*i1, 0) nullspaces[i](0,1) * pt3_current[1]; A(2*i, 1) nullspaces[i](0,0) * pt3_current[2]; // r13 A(2*i1, 1) nullspaces[i](0,1) * pt3_current[2]; A(2*i, 2) nullspaces[i](1,0) * pt3_current[1]; // r22 A(2*i1, 2) nullspaces[i](1,1) * pt3_current[1]; A(2*i, 3) nullspaces[i](1,0) * pt3_current[2]; // r23 A(2*i1, 3) nullspaces[i](1,1) * pt3_current[2]; A(2*i, 4) nullspaces[i](2,0) * pt3_current[1]; // r32 A(2*i1, 4) nullspaces[i](2,1) * pt3_current[1]; A(2*i, 5) nullspaces[i](2,0) * pt3_current[2]; // r33 A(2*i1, 5) nullspaces[i](2,1) * pt3_current[2]; // 平移项N_i 直接乘 t没有点坐标 A(2*i, 6) nullspaces[i](0,0); // t1 A(2*i1, 6) nullspaces[i](0,1); A(2*i, 7) nullspaces[i](1,0); // t2 A(2*i1, 7) nullspaces[i](1,1); A(2*i, 8) nullspaces[i](2,0); // t3 A(2*i1, 8) nullspaces[i](2,1); } } else { // 非平面完整的 12 参数 for (size_t i 0; i numberCorrespondences; i) { point_t pt3_current points3.col(i); // 顺序r11, r12, r13, r21, r22, r23, r31, r32, r33, t1, t2, t3 A(2*i, 0) nullspaces[i](0,0) * pt3_current[0]; // r11 A(2*i1, 0) nullspaces[i](0,1) * pt3_current[0]; A(2*i, 1) nullspaces[i](0,0) * pt3_current[1]; // r12 A(2*i1, 1) nullspaces[i](0,1) * pt3_current[1]; A(2*i, 2) nullspaces[i](0,0) * pt3_current[2]; // r13 A(2*i1, 2) nullspaces[i](0,1) * pt3_current[2]; A(2*i, 3) nullspaces[i](1,0) * pt3_current[0]; // r21 A(2*i1, 3) nullspaces[i](1,1) * pt3_current[0]; A(2*i, 4) nullspaces[i](1,0) * pt3_current[1]; // r22 A(2*i1, 4) nullspaces[i](1,1) * pt3_current[1]; A(2*i, 5) nullspaces[i](1,0) * pt3_current[2]; // r23 A(2*i1, 5) nullspaces[i](1,1) * pt3_current[2]; A(2*i, 6) nullspaces[i](2,0) * pt3_current[0]; // r31 A(2*i1, 6) nullspaces[i](2,1) * pt3_current[0]; A(2*i, 7) nullspaces[i](2,0) * pt3_current[1]; // r32 A(2*i1, 7) nullspaces[i](2,1) * pt3_current[1]; A(2*i, 8) nullspaces[i](2,0) * pt3_current[2]; // r33 A(2*i1, 8) nullspaces[i](2,1) * pt3_current[2]; // 平移 A(2*i, 9) nullspaces[i](0,0); A(2*i1, 9) nullspaces[i](0,1); A(2*i, 10) nullspaces[i](1,0); A(2*i1,10) nullspaces[i](1,1); A(2*i, 11) nullspaces[i](2,0); A(2*i1,11) nullspaces[i](2,1); } } ////////////////////////////////////// // 4. 最小二乘求解约束 ||x|| 1 ////////////////////////////////////// // 加权法方程矩阵AtPA A^T * P * A (若 use_cov)否则 AtPA A^T * A Eigen::MatrixXd AtPA; if (use_cov) AtPA A.transpose() * P * A; else AtPA A.transpose() * A; // 解齐次线性系统 AtPA * x 0在 ||x||1 下的解为 AtPA 最小特征值对应的特征向量 Eigen::JacobiSVDEigen::MatrixXd svd_A(AtPA, Eigen::ComputeFullV); Eigen::MatrixXd result1 svd_A.matrixV().col(colsA - 1); // V 的最后一列为最小奇异值对应的右奇异向量 // 从解向量中恢复旋转和平移 rotation_t Rout; translation_t tout; if (planar) // 平面情况 { rotation_t tmp; // 解向量结构[r12, r13, r22, r23, r32, r33, t1, t2, t3] // 构建一个初始旋转矩阵缺少第一列和第三列的部分 // tmp 的列为第1列暂为0待叉乘计算第2列由 r12, r22, r32 构成第3列由 r13, r23, r33 构成 tmp 0.0, result1(0,0), result1(1,0), 0.0, result1(2,0), result1(3,0), 0.0, result1(4,0), result1(5,0); // 由前两列叉乘得到第一列旋转矩阵行/列注意这里 tmp 是按行填充的后续转置与重组 // 实际代码逻辑tmp 的行对应旋转矩阵的列下面注释将纠正 // 从后面 tmp.col(0) tmp.col(1).cross(tmp.col(2)) 可知tmp 的列含义 // col(1) (r12, r22, r32)^Tcol(2) (r13, r23, r33)^Tcol(0) 暂为 (0,0,0) tmp.col(0) tmp.col(1).cross(tmp.col(2)); // 叉乘得到正交的第三列理论上为旋转矩阵的第三个列向量 tmp.transposeInPlace(); // 现在 tmp 的行成了列变为通常的旋转矩阵排列 // 此时 tmp 行排列第1行对应原来 col(0)第2行对应原来 col(1)第3行对应原来 col(2) // 但顺序较为混乱本意是获得一个近似旋转矩阵尺度未知 double scale 1.0 / std::sqrt(std::abs(tmp.col(1).norm() * tmp.col(2).norm())); // 利用列模长估计尺度 // 将 tmp 投影到 SO(3)Frobenius 范数下最接近的旋转矩阵 Eigen::JacobiSVDEigen::MatrixXd svd_R_frob(tmp, Eigen::ComputeFullU | Eigen::ComputeFullV); rotation_t Rout1 svd_R_frob.matrixU() * svd_R_frob.matrixV().transpose(); // 确保行列式为 1 if (Rout1.determinant() 0) Rout1 * -1.0; // 旋转回原始世界坐标系因为之前将点变换到了特征坐标系 Rout1 eigenRot.transpose() * Rout1; // 从解中提取平移部分并乘以尺度 translation_t t scale * translation_t(result1(6,0), result1(7,0), result1(8,0)); Rout1.transposeInPlace(); Rout1 * -1; // 符号调整根据后面重投影选择最佳 if (Rout1.determinant() 0.0) Rout1.col(2) * -1; // 创建四种可能的符号组合因为旋转列可能有整体符号歧义 rotation_t R1, R2; R1.col(0) Rout1.col(0); R1.col(1) Rout1.col(1); R1.col(2) Rout1.col(2); R2.col(0) -Rout1.col(0); R2.col(1) -Rout1.col(1); R2.col(2) Rout1.col(2); vectortransformation_t, Eigen::aligned_allocatortransformation_t Ts(4); Ts[0].block3,3(0,0) R1; Ts[0].block3,1(0,3) t; Ts[1].block3,3(0,0) R1; Ts[1].block3,1(0,3) -t; Ts[2].block3,3(0,0) R2; Ts[2].block3,1(0,3) t; Ts[3].block3,3(0,0) R2; Ts[3].block3,1(0,3) -t; // 通过前6个点的重投影误差选择最佳组合 vectordouble normVal(4); for (int i 0; i 4; i) { point_t reproPt; double norms 0.0; for (int p 0; p 6; p) { reproPt Ts[i].block3,3(0,0) * points3v[p] Ts[i].block3,1(0,3); reproPt reproPt / reproPt.norm(); norms (1.0 - reproPt.transpose() * f[indices[p]]); } normVal[i] norms; } std::vectordouble::iterator findMinRepro std::min_element(std::begin(normVal), std::end(normVal)); int idx std::distance(std::begin(normVal), findMinRepro); Rout Ts[idx].block3,3(0,0); tout Ts[idx].block3,1(0,3); } else // 非平面 { rotation_t tmp; // 解向量结构[r11, r12, r13, r21, r22, r23, r31, r32, r33, t1, t2, t3] // 将旋转部分排列成 3x3 矩阵按列优先注意代码是逐行填入最终 tmp 的元素排列 // tmp r11, r21, r31, // r12, r22, r32, // r13, r23, r33 // 即 tmp 的列是原本旋转矩阵的行仔细看 // result1(0) - tmp(0,0) r11 // result1(3) - tmp(0,1) r21 // result1(6) - tmp(0,2) r31 // 所以 tmp 的第一行是旋转矩阵的第一列转置即 tmp 实际上是旋转矩阵的转置需进一步看投影恢复。 tmp result1(0,0), result1(3,0), result1(6,0), result1(1,0), result1(4,0), result1(7,0), result1(2,0), result1(5,0), result1(8,0); // 尺度估计几何平均列模长的倒数 double scale 1.0 / std::pow(std::abs(tmp.col(0).norm() * tmp.col(1).norm() * tmp.col(2).norm()), 1.0/3.0); // 投影到 SO(3) Eigen::JacobiSVDEigen::MatrixXd svd_R_frob(tmp, Eigen::ComputeFullU | Eigen::ComputeFullV); Rout svd_R_frob.matrixU() * svd_R_frob.matrixV().transpose(); if (Rout.determinant() 0) Rout * -1.0; // 平移恢复t scale * R * t_est? 注意公式推导 // 原方程中未知向量是 [r_vec; t] 在求解时旋转部分恢复为 Rout平移部分解出的 t_est 需要乘上尺度 // 但实际平移应为 -R^T * t? 我们看代码 // tout Rout * (scale * t_est) 说明 t_est 是世界坐标系下平移后面逆变换组合会再调整。 tout Rout * (scale * translation_t(result1(9,0), result1(10,0), result1(11,0))); // 确定平移的符号构建两种可能t 和 -t比较重投影误差 vectordouble error(2); vectorEigen::Matrix4d, Eigen::aligned_allocatorEigen::Matrix4d Ts(2); for (int s 0; s 2; s) { error[s] 0.0; Ts[s] Eigen::Matrix4d::Identity(); Ts[s].block3,3(0,0) Rout; if (s 0) Ts[s].block3,1(0,3) tout; else Ts[s].block3,1(0,3) -tout; Ts[s] Ts[s].inverse().eval(); // 取逆得到相机位姿opengv 惯例需要 for (int p 0; p 6; p) { bearingVector_t v Ts[s].block3,3(0,0) * points3v[p] Ts[s].block3,1(0,3); v v / v.norm(); error[s] (1.0 - v.transpose() * f[indices[p]]); } } if (error[0] error[1]) tout Ts[0].block3,1(0,3); // 注意此时 tout 实际是逆变换后的平移 else tout Ts[1].block3,1(0,3); Rout Ts[0].block3,3(0,0); } ////////////////////////////////////// // 5. 高斯-牛顿精细优化 ////////////////////////////////////// rodrigues_t omega rot2rodrigues(Rout); // 旋转矩阵转罗德里格斯向量 Eigen::VectorXd minx(6); minx[0] omega[0]; minx[1] omega[1]; minx[2] omega[2]; minx[3] tout[0]; minx[4] tout[1]; minx[5] tout[2]; mlpnp_gn(minx, points3v, nullspaces, P, use_cov); // 执行极大似然高斯-牛顿优化 // 优化后更新位姿 Rout rodrigues2rot(rodrigues_t(minx[0], minx[1], minx[2])); tout translation_t(minx[3], minx[4], minx[5]); // 最终结果填入输出矩阵opengv 规定的变换方向 result.block3,3(0,0) Rout; result.block3,1(0,3) tout; }小结这段代码严格实现了MLPnP的核心步骤零空间消深度通过 SVD 构造与 bearing vector 正交的矩阵得到线性约束。平面检测若 3D 点共面变换到特征坐标系以降低未知数维度。协方差加权将 bearing 测量不确定性引入法方程。齐次最小二乘构建 ATPA并取最小特征向量。位姿恢复与歧义消除从解向量重构旋转矩阵SVD 投影到 SO(3)和平移并用重投影误差确定符号。高斯‑牛顿精细以当前解为初值优化极大似然代价。注释中的公式与代码逻辑完全对应可供理解这一经典算法。