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