void MVS::DecomposeProjectionMatrix(const PMatrix& P, RMatrix& R, CMatrix& C)
{
#ifndef _RELEASE
KMatrix K;
DecomposeProjectionMatrix(P, K, R, C);
ASSERT(K.IsEqual(Matrix3x3::IDENTITY));
#endif
// extract camera center as the right null vector of P
const Vec4 hC(P.RightNullVector());
C = (const CMatrix&)hC * INVERT(hC[3]);
// get rotation
const cv::Mat mP(3,4,cv::DataType<REAL>::type,(void*)P.val);
mP(cv::Rect(0,0, 3,3)).copyTo(R);
ASSERT(R.IsValid());
} // DecomposeProjectionMatrix
// decomposition of projection matrix into KR[I|-C]: internal calibration ([3,3]), rotation ([3,3]) and translation ([3,1])
// (comparable with OpenCV: normalized cv::decomposeProjectionMatrix)
void MVS::DecomposeProjectionMatrix(const PMatrix& P, KMatrix& K, RMatrix& R, CMatrix& C)
{
// extract camera center as the right null vector of P
const Vec4 hC(P.RightNullVector());
C = (const CMatrix&)hC * INVERT(hC[3]);
// perform RQ decomposition
const cv::Mat mP(3,4,cv::DataType<REAL>::type,(void*)P.val);
cv::RQDecomp3x3(mP(cv::Rect(0,0, 3,3)), K, R);
// normalize calibration matrix
K *= INVERT(K(2,2));
// ensure positive focal length
if (K(0,0) < 0) {
ASSERT(K(1,1) < 0);
NEGATE(K(0,0));
NEGATE(K(1,1));
NEGATE(K(0,1));
NEGATE(K(0,2));
NEGATE(K(1,2));
(TMatrix<REAL,2,3>&)R *= REAL(-1);
}
ASSERT(R.IsValid());
} // DecomposeProjectionMatrix
