本质矩阵（本质矩阵与基础矩阵）

由对极几何[1] 可得, P1TEP2=0P_1^T E P_2 = 0，写成矩阵的形式如下:

[x1y11][e11e12e13e21e22e23e31e32e33][x2y21]=0\begin{equation} \begin{bmatrix} x_1 & y_1 & 1 \end{bmatrix} \begin{bmatrix} e_{11} & e_{12} & e_{13} \\ e_{21} & e_{22} & e_{23} \\ e_{31} & e_{32} & e_{33}\\ \end{bmatrix} \begin{bmatrix} x_2 \\ y_2 \\ 1 \end{bmatrix} = 0 \end{equation} \\

可以化简成:

x1x2e11+x1y2e12+x1e13+y1x2e21+y1y2e22+y1e23+x2e31+y2e32+e33=0x_1 x_2 e_{11} + x_1 y_2 e_{12} + x_1 e_{13} + y_1 x_2 e_{21} + y_1 y_2 e_{22} + y_1 e_{23} + x_2 e_{31} + y_2 e_{32} + e_{33} = 0 \\

写成矩阵的形式

[x1x2x1y2x1y1x2y1y2y1x2y21][e11e12e13e21e22e23e31e32e33]=0\begin{bmatrix} x_1 x_2 & x_1 y_2 & x1 & y_1 x_2 & y_1 y_2 & y_1 & x_2 & y_2 & 1 \end{bmatrix} \begin{bmatrix} e_{11} \\ e_{12} \\ e_{13}\\ e_{21}\\ e_{22}\\ e_{23}\\ e_{31}\\ e_{32}\\ e_{33}\\ \end{bmatrix} = 0 \\

对于多个点，则可以写成

Qe=[x1x2x1y2x1y1x2y1y2y1x2y21..................][e11e12e13e21e22e23e31e32e33]=0Qe = \begin{bmatrix} x_1 x_2 & x_1 y_2 & x1 & y_1 x_2 & y_1 y_2 & y_1 & x_2 & y_2 & 1 \\ .. & .. & .. & ..& .. & .. & .. & .. & .. \\ \end{bmatrix} \begin{bmatrix} e_{11} \\ e_{12} \\ e_{13}\\ e_{21}\\ e_{22}\\ e_{23}\\ e_{31}\\ e_{32}\\ e_{33}\\ \end{bmatrix} = 0 \\

求解E矩阵需要5个点，故Q是 5×95 \times 9 的矩阵,用C++代码实现如下:

Eigen::Matrix<double, 5, 9> Q; for (size_t i = 0; i < 5; ++i) { const double x1_0 = points1[i](0); const double x1_1 = points1[i](1); const double x2_0 = points2[i](0); const double x2_1 = points2[i](1); Q(i, 0) = x1_0 * x2_0; Q(i, 1) = x1_1 * x2_0; Q(i, 2) = x2_0; Q(i, 3) = x1_0 * x2_1; Q(i, 4) = x1_1 * x2_1; Q(i, 5) = x2_1; Q(i, 6) = x1_0; Q(i, 7) = x1_1; Q(i, 8) = 1; }

E矩阵有9个参数，5个自由度，故有4个nullspace, 故E矩阵的通用形式可以写成:

E=xX+yY+zZ+wWE = x X + yY + zZ + wW \\

C++代码的实现如下：

// Extract the 4 Eigen vectors corresponding to the *** allest singular values. const Eigen::JacobiSVD<Eigen::Matrix<double, Eigen::Dynamic, 9>> svd( Q, Eigen::ComputeFullV); const Eigen::Matrix<double, 9, 4> E = svd.matrixV().block<9, 4>(0, 5);

E矩阵满足以下约束:

det(E)=0det(E) = 0 \\

EETE−12trace(EET)E=0E E^TE-\frac{1}{2} trace(EE^T)E = 0 \\

可以用Gauss-Jordan公式去做消元，代码实现如下:

Eigen::Matrix<double, 10, 20> A; #include "estimators/essential_matrix_poly.h" Eigen::Matrix<double, 10, 10> AA = A.block<10, 10>(0, 0).partialPivLu().solve(A.block<10, 10>(0, 10));

行列式扩展和root extraction这部分比较复杂，暂略掉,代码如下:

// Step 4: Expansion of the determinant polynomial of the 3x3 polynomial // matrix B to obtain the tenth degree polynomial. Eigen::Matrix<double, 13, 3> B; for (size_t i = 0; i < 3; ++i) { B(0, i) = 0; B(4, i) = 0; B(8, i) = 0; B.block<3, 1>(1, i) = AA.block<1, 3>(i * 2 + 4, 0); B.block<3, 1>(5, i) = AA.block<1, 3>(i * 2 + 4, 3); B.block<4, 1>(9, i) = AA.block<1, 4>(i * 2 + 4, 6); B.block<3, 1>(0, i) -= AA.block<1, 3>(i * 2 + 5, 0); B.block<3, 1>(4, i) -= AA.block<1, 3>(i * 2 + 5, 3); B.block<4, 1>(8, i) -= AA.block<1, 4>(i * 2 + 5, 6); } // Step 5: Extraction of roots from the degree 10 polynomial. Eigen::Matrix<double, 11, 1> coeffs; #include "estimators/essential_matrix_coeffs.h" Eigen::VectorXd roots_real; Eigen::VectorXd roots_imag; if (!FindPolynomialRootsCompanionMatrix(coeffs, &roots_real, &roots_imag)) { return {}; } std::vector<M_t> models; models.reserve(roots_real.size()); for (Eigen::VectorXd::Index i = 0; i < roots_imag.size(); ++i) { const double kMaxRootImag = 1e-10; if (std::abs(roots_imag(i)) > kMaxRootImag) { continue; } const double z1 = roots_real(i); const double z2 = z1 * z1; const double z3 = z2 * z1; const double z4 = z3 * z1; Eigen::Matrix3d Bz; for (size_t j = 0; j < 3; ++j) { Bz(j, 0) = B(0, j) * z3 + B(1, j) * z2 + B(2, j) * z1 + B(3, j); Bz(j, 1) = B(4, j) * z3 + B(5, j) * z2 + B(6, j) * z1 + B(7, j); Bz(j, 2) = B(8, j) * z4 + B(9, j) * z3 + B(10, j) * z2 + B(11, j) * z1 + B(12, j); } const Eigen::JacobiSVD<Eigen::Matrix3d> svd(Bz, Eigen::ComputeFullV); const Eigen::Vector3d X = svd.matrixV().block<3, 1>(0, 2); const double kMaxX3 = 1e-10; if (std::abs(X(2)) < kMaxX3) { continue; } Eigen::MatrixXd essential_vec = E.col(0) * (X(0) / X(2)) + E.col(1) * (X(1) / X(2)) + E.col(2) * z1 + E.col(3); essential_vec /= essential_vec.norm(); const Eigen::Matrix3d essential_matrix = Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>( essential_vec.data());

E矩阵可以分解出旋转矩阵和平移矩阵，代码实现如下:

Eigen::JacobiSVD<Eigen::Matrix3d> svd( E, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix3d U = svd.matrixU(); Eigen::Matrix3d V = svd.matrixV().transpose(); if (U.determinant() < 0) { U *= -1; } if (V.determinant() < 0) { V *= -1; } Eigen::Matrix3d W; W << 0, 1, 0, -1, 0, 0, 0, 0, 1; *R1 = U * W * V; *R2 = U * W.transpose() * V; *t = U.col(2).normalized();

参考资料:

Essential Matrix公式https://www.cs.cmu.edu/~16385/s17/Slides/12.2_Essential_Matrix.pdf