| 95 | } |
| 96 | |
| 97 | bool DecomposeProjectionMatrix(const Eigen::Matrix3x4d& P, |
| 98 | Eigen::Matrix3d* K, |
| 99 | Eigen::Matrix3d* R, |
| 100 | Eigen::Vector3d* T) { |
| 101 | Eigen::Matrix3d RR; |
| 102 | Eigen::Matrix3d QQ; |
| 103 | DecomposeMatrixRQ(P.leftCols<3>().eval(), &RR, &QQ); |
| 104 | |
| 105 | *R = ComputeClosestRotationMatrix(QQ); |
| 106 | |
| 107 | const double det_K = RR.determinant(); |
| 108 | if (det_K == 0) { |
| 109 | return false; |
| 110 | } else if (det_K > 0) { |
| 111 | *K = RR; |
| 112 | } else { |
| 113 | *K = -RR; |
| 114 | } |
| 115 | |
| 116 | for (int i = 0; i < 3; ++i) { |
| 117 | if ((*K)(i, i) < 0.0) { |
| 118 | K->col(i) = -K->col(i); |
| 119 | R->row(i) = -R->row(i); |
| 120 | } |
| 121 | } |
| 122 | |
| 123 | *T = K->triangularView<Eigen::Upper>().solve(P.col(3)); |
| 124 | if (det_K < 0) { |
| 125 | *T = -(*T); |
| 126 | } |
| 127 | |
| 128 | return true; |
| 129 | } |
| 130 | |
| 131 | Eigen::Vector3d RotationMatrixToAngleAxis(const Eigen::Matrix3d& R) { |
| 132 | const Eigen::AngleAxisd aa(R); |