Eigen::MatrixXd FivePointsPolynomialConstraints()

in opensfm/src/geometry/src/essential.cc [56:111]

• 43 lines of code
• 9 McCabe index (conditional complexity)

Eigen::MatrixXd FivePointsPolynomialConstraints(
    const Eigen::MatrixXd &E_basis) {
  // Build the polynomial form of E (equation (8) in Stewenius et al. [1])
  Eigen::Matrix<double, -1, 1> E[3][3];
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      E[i][j] = Eigen::Matrix<double, -1, 1>::Zero(20);
      E[i][j](coef_x) = E_basis(3 * i + j, 0);
      E[i][j](coef_y) = E_basis(3 * i + j, 1);
      E[i][j](coef_z) = E_basis(3 * i + j, 2);
      E[i][j](coef_1) = E_basis(3 * i + j, 3);
    }
  }

  // The constraint matrix.
  Eigen::MatrixXd M(10, 20);
  int mrow = 0;

  // Determinant constraint det(E) = 0; equation (19) of Nister [2].
  M.row(mrow++) = o2(o1(E[0][1], E[1][2]) - o1(E[0][2], E[1][1]), E[2][0]) +
                  o2(o1(E[0][2], E[1][0]) - o1(E[0][0], E[1][2]), E[2][1]) +
                  o2(o1(E[0][0], E[1][1]) - o1(E[0][1], E[1][0]), E[2][2]);

  // Cubic singular values constraint.
  // Equation (20).
  Eigen::Matrix<double, -1, 1> EET[3][3];
  for (int i = 0; i < 3; ++i) {    // Since EET is symmetric, we only compute
    for (int j = 0; j < 3; ++j) {  // its upper triangular part.
      if (i <= j) {
        EET[i][j] =
            o1(E[i][0], E[j][0]) + o1(E[i][1], E[j][1]) + o1(E[i][2], E[j][2]);
      } else {
        EET[i][j] = EET[j][i];
      }
    }
  }

  // Equation (21).
  Eigen::Matrix<double, -1, 1>(&L)[3][3] = EET;
  Eigen::Matrix<double, -1, 1> trace =
      0.5 * (EET[0][0] + EET[1][1] + EET[2][2]);
  for (int i = 0; i < 3; ++i) {
    L[i][i] -= trace;
  }

  // Equation (23).
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      Eigen::Matrix<double, -1, 1> LEij =
          o2(L[i][0], E[0][j]) + o2(L[i][1], E[1][j]) + o2(L[i][2], E[2][j]);
      M.row(mrow++) = LEij;
    }
  }

  return M;
}