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