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; }