Mat3 operator*(const Mat3& n, const Mat3& m)
{
Mat3 A;
for(int i=0;i<3;i++)
for(int j=0;j<3;j++)
A(i,j) = n[i]*m.col(j);
return A;
}
bool FindExtrinsics(const Mat3& E, const Point2Vec& pts1, const Point2Vec& pts2, Mat3* R, Vec3* t)
{
const auto num_correspondences = pts1.size();
// Put first camera at origin
Mat3 R0; R0.setIdentity();
Vec3 t0; t0.setZero();
// Find the SVD of E
Eigen::JacobiSVD<Mat3> svd{E, Eigen::ComputeFullU | Eigen::ComputeFullV};
Mat3 U = svd.matrixU();
Mat3 Vt = svd.matrixV().transpose();
// Find R and t
Mat3 D;
D << 0, 1, 0,
-1, 0, 0,
0, 0, 1;
Mat3 Ra = U * D * Vt;
Mat3 Rb = U * D.transpose() * Vt;
if (Ra.determinant() < 0.0) Ra *= -1.0;
if (Rb.determinant() < 0.0) Rb *= -1.0;
Vec3 tu = U.col(2);
// Figure out which configuration is correct using the supplied points
int c1_pos = 0, c2_pos = 0, c1_neg = 0, c2_neg = 0;
for (std::size_t i = 0; i < num_correspondences; i++)
{
const Point3 Q = Triangulate(Observations{Observation{pts1[i], R0, t0}, Observation{pts2[i], Ra, tu}});
const Point3 PQ = (Ra * Q) + tu;
if (Q.z() > 0) c1_pos++;
else c1_neg++;
if (PQ.z() > 0) c2_pos++;
else c2_neg++;
}
if (c1_pos < c1_neg && c2_pos < c2_neg)
{
*R = Ra;
*t = tu;
} else if (c1_pos > c1_neg && c2_pos > c2_neg)
{
*R = Ra;
*t = -tu;
} else
{
// Triangulate again
c1_pos = c1_neg = c2_pos = c2_neg = 0;
for (std::size_t i = 0; i < num_correspondences; i++)
{
const Point3 Q = Triangulate(Observations{Observation{pts1[i], R0, t0}, Observation{pts2[i], Rb, tu}});
const Point3 PQ = (Rb * Q) + tu;
if (Q.z() > 0) c1_pos++;
else c1_neg++;
if (PQ.z() > 0) c2_pos++;
else c2_neg++;
}
if (c1_pos < c1_neg && c2_pos < c2_neg)
{
*R = Rb;
*t = tu;
} else if (c1_pos > c1_neg && c2_pos > c2_neg)
{
*R = Rb;
*t = -tu;
} else
{
std::cerr << "[FindExtrinsics] Error: no case found!";
return false;
}
}
return true;
}
