| 38 | namespace colmap { |
| 39 | |
| 40 | void DecomposeEssentialMatrix(const Eigen::Matrix3d& E, |
| 41 | Eigen::Matrix3d* R1, |
| 42 | Eigen::Matrix3d* R2, |
| 43 | Eigen::Vector3d* t) { |
| 44 | Eigen::JacobiSVD<Eigen::Matrix3d> svd( |
| 45 | E, Eigen::ComputeFullU | Eigen::ComputeFullV); |
| 46 | Eigen::Matrix3d U = svd.matrixU(); |
| 47 | Eigen::Matrix3d V = svd.matrixV().transpose(); |
| 48 | |
| 49 | if (U.determinant() < 0) { |
| 50 | U *= -1; |
| 51 | } |
| 52 | if (V.determinant() < 0) { |
| 53 | V *= -1; |
| 54 | } |
| 55 | |
| 56 | Eigen::Matrix3d W; |
| 57 | W << 0, 1, 0, -1, 0, 0, 0, 0, 1; |
| 58 | |
| 59 | *R1 = U * W * V; |
| 60 | *R2 = U * W.transpose() * V; |
| 61 | *t = U.col(2).normalized(); |
| 62 | } |
| 63 | |
| 64 | void PoseFromEssentialMatrix(const Eigen::Matrix3d& E, |
| 65 | const std::vector<Eigen::Vector3d>& cam_rays1, |
no outgoing calls