static void Solve(const Mat &x1, const Mat &x2, std::vector<Mat3> *pvec_E)
{
assert(3 == x1.rows());
assert(8 <= x1.cols());
assert(x1.rows() == x2.rows());
assert(x1.cols() == x2.cols());
MatX9 A(x1.cols(), 9);
EncodeEpipolarEquation(x1, x2, &A);
Vec9 e;
Nullspace(&A, &e);
Mat3 E = Map<RMat3>(e.data());
// Find the closest essential matrix to E in frobenius norm
// E = UD'VT
if (x1.cols() > 8) {
Eigen::JacobiSVD<Mat3> USV(E, Eigen::ComputeFullU | Eigen::ComputeFullV);
Vec3 d = USV.singularValues();
double a = d[0];
double b = d[1];
d << (a+b)/2., (a+b)/2., 0.0;
E = USV.matrixU() * d.asDiagonal() * USV.matrixV().transpose();
}
pvec_E->push_back(E);
}
void FourPointSolver::Solve(const Mat &x, const Mat &y, vector<Mat3> *Hs) {
assert(2 == x.rows());
assert(4 <= x.cols());
assert(x.rows() == y.rows());
assert(x.cols() == y.cols());
Mat::Index n = x.cols();
Vec9 h;
if (n == 4) {
// In the case of minimal configuration we use fixed sized matrix to let
// Eigen and the compiler doing the maximum of optimization.
typedef Eigen::Matrix<double, 16, 9> Mat16_9;
Mat16_9 L = Mat::Zero(16, 9);
BuildActionMatrix(L, x, y);
Nullspace(&L, &h);
}
else {
MatX9 L = Mat::Zero(n * 2, 9);
BuildActionMatrix(L, x, y);
Nullspace(&L, &h);
}
Mat3 H = Map<RMat3>(h.data()); // map the linear vector as the H matrix
Hs->push_back(H);
}