void KRt_From_P(const Mat34 &P, Mat3 *Kp, Mat3 *Rp, Vec3 *tp) { // Decompose using the RQ decomposition HZ A4.1.1 pag.579. Mat3 K = P.block(0, 0, 3, 3); Mat3 Q; Q.setIdentity(); // Set K(2,1) to zero. if (K(2,1) != 0) { double c = -K(2,2); double s = K(2,1); double l = sqrt(c * c + s * s); c /= l; s /= l; Mat3 Qx; Qx << 1, 0, 0, 0, c, -s, 0, s, c; K = K * Qx; Q = Qx.transpose() * Q; } // Set K(2,0) to zero. if (K(2,0) != 0) { double c = K(2,2); double s = K(2,0); double l = sqrt(c * c + s * s); c /= l; s /= l; Mat3 Qy; Qy << c, 0, s, 0, 1, 0, -s, 0, c; K = K * Qy; Q = Qy.transpose() * Q; } // Set K(1,0) to zero. if (K(1,0) != 0) { double c = -K(1,1); double s = K(1,0); double l = sqrt(c * c + s * s); c /= l; s /= l; Mat3 Qz; Qz << c,-s, 0, s, c, 0, 0, 0, 1; K = K * Qz; Q = Qz.transpose() * Q; } Mat3 R = Q; //Mat3 H = P.block(0, 0, 3, 3); // RQ decomposition //Eigen::HouseholderQR<Mat3> qr(H); //Mat3 K = qr.matrixQR().triangularView<Eigen::Upper>(); //Mat3 R = qr.householderQ(); // Ensure that the diagonal is positive and R determinant == 1. if (K(2,2) < 0) { K = -K; R = -R; } if (K(1,1) < 0) { Mat3 S; S << 1, 0, 0, 0,-1, 0, 0, 0, 1; K = K * S; R = S * R; } if (K(0,0) < 0) { Mat3 S; S << -1, 0, 0, 0, 1, 0, 0, 0, 1; K = K * S; R = S * R; } // Compute translation. Eigen::PartialPivLU<Mat3> lu(K); Vec3 t = lu.solve(P.col(3)); if(R.determinant()<0) { R = -R; t = -t; } // scale K so that K(2,2) = 1 K = K / K(2,2); *Kp = K; *Rp = R; *tp = t; }
void DecomposeProjectionMatrix(const Mat& P, Mat3* K, Mat3* R, Vec3* t) { // This is a modified version of the function "KRt_From_P", found in libmv and openMVG. // It is subject to the terms of the Mozilla Public License, v. 2.0, a copy of the MPL // can be found under "license.openMVG" // Decompose using the RQ decomposition HZ A4.1.1 pag.579. Mat3 Kp = P.block(0, 0, 3, 3); Mat3 Q; Q.setIdentity(); // Set K(2, 1) to zero. if (Kp(2, 1) != 0) { double c = -Kp(2, 2); double s = Kp(2, 1); double l = sqrt(c * c + s * s); c /= l; s /= l; Mat3 Qx; Qx << 1, 0, 0, 0, c, -s, 0, s, c; Kp = Kp * Qx; Q = Qx.transpose() * Q; } // Set K(2, 0) to zero. if (Kp(2, 0) != 0) { double c = Kp(2, 2); double s = Kp(2, 0); double l = sqrt(c * c + s * s); c /= l; s /= l; Mat3 Qy; Qy << c, 0, s, 0, 1, 0, -s, 0, c; Kp = Kp * Qy; Q = Qy.transpose() * Q; } // Set K(1, 0) to zero. if (Kp(1, 0) != 0) { double c = -Kp(1, 1); double s = Kp(1, 0); double l = sqrt(c * c + s * s); c /= l; s /= l; Mat3 Qz; Qz << c, -s, 0, s, c, 0, 0, 0, 1; Kp = Kp * Qz; Q = Qz.transpose() * Q; } Mat3 Rp = Q; // Ensure that the diagonal is positive and R determinant == 1 if (Kp(2, 2) < 0) { Kp = -Kp; Rp = -Rp; } if (Kp(1, 1) < 0) { Mat3 S; S << 1, 0, 0, 0, -1, 0, 0, 0, 1; Kp = Kp * S; Rp = S * Rp; } if (Kp(0, 0) < 0) { Mat3 S; S << -1, 0, 0, 0, 1, 0, 0, 0, 1; Kp = Kp * S; Rp = S * Rp; } // Compute translation. Vec3 tp = Kp.colPivHouseholderQr().solve(P.col(3)); if(Rp.determinant() < 0) { Rp = -Rp; tp = -tp; } // Scale K so that K(2, 2) = 1 Kp = Kp / Kp(2, 2); *K = Kp; *R = Rp; *t = tp; }