将投影矩阵P利用QR分解分解出摄像机内外参数（Opencv）

将投影矩阵P利用QR分解分解出摄像机内外参数（Opencv）

/***************************************************************
    将投影矩阵P利用QR分解分解出摄像机内外参数
****************************************************************
输入：
P：投影矩阵，3*4
输出：
K：内参数矩阵，3*3
R：旋转矩阵，3*3
T：平移向量，3*1
程序设计：Wenbin WANG
设计时间：2009/06/21
参考文献：蔡涛，《单目视觉三维重建方法与应用研究》，2004，pp36

说明：本程序中的qrHouse可以在本博客中找到相应的Opencv代码，本程序已经经过验证是正确的
****************************************************************/
void KRT_From_P_QR(CvMat* P, CvMat* K, CvMat* R, CvMat* T)
{
int i,j;

//第一步
CvMat* H = cvCreateMat(3,3,CV_64FC1);
CvMat* h = cvCreateMat(3,1,CV_64FC1);
for (i = 0; i < 3; i++)
{
   for (j = 0; j < 3; j++)
   {
    cvmSet(H,i,j,cvmGet(P,i,j));
   }
   cvmSet(h,i,0,cvmGet(P,i,3));
}

//第二步
CvMat* Hinv = cvCreateMat(3,3,CV_64FC1);
cvInvert(H,Hinv,CV_SVD);
CvMat* Qd   = cvCreateMat(H->rows,H->cols,CV_64FC1);
CvMat* Rd   = cvCreateMat(H->rows,H->rows,CV_64FC1);

cvReleaseMat(&H);
qrHouse(Hinv,Qd,Rd);
cvInvert(Rd,K,CV_SVD);
cvTranspose(Qd,R);
cvReleaseMat(&Hinv);
cvReleaseMat(&Qd);

//第三步：
double s1[9] = {-1,0,0,0,1,0,0,0,-1};
double s2[9] = {-1,0,0,0,-1,0,0,0,1};
double s3[9] = {1,0,0,0,-1,0,0,0,-1};
double s4[9] = {1,0,0,0,1,0,0,0,1};

CvMat* Rn = cvCreateMat(3,3,CV_64FC1);
CvMat* Kn = cvCreateMat(3,3,CV_64FC1);
CvMat* S1 = cvCreateMat(3,3,CV_64FC1);
CvMat* S2 = cvCreateMat(3,3,CV_64FC1);
CvMat* S3 = cvCreateMat(3,3,CV_64FC1);
CvMat* S4 = cvCreateMat(3,3,CV_64FC1);
cvInitMatHeader(S1,3,3,CV_64FC1,s1);
cvInitMatHeader(S2,3,3,CV_64FC1,s2);
cvInitMatHeader(S3,3,3,CV_64FC1,s3);
cvInitMatHeader(S4,3,3,CV_64FC1,s4);

double a = cvmGet(K,0,0);
double b = cvmGet(K,1,1);
double c = cvmGet(K,2,2);

CvMat* t0 = cvCreateMat(3,3,CV_64FC1);

if ((a>0 && b<0 && c>0) || (a<0 && b>0 && c<0))
{
   cvMatMul(K,S1,Kn);
   cvMatMul(S1,R,Rn);
   cvMatMul(S1,Rd,t0);
}
else if ((a>0 && b>0 && c<0) || (a<0 && b<0 && c>0))
{
   cvMatMul(K,S2,Kn);
   cvMatMul(S2,R,Rn);
   cvMatMul(S2,Rd,t0);
}
else if ((a>0 && b<0 && c<0) || (a<0 && b>0 && c>0))
{
   cvMatMul(K,S3,Kn);
   cvMatMul(S3,R,Rn);
   cvMatMul(S2,Rd,t0);
}
else if ((a>0 && b>0 && c>0) || (a<0 && b<0 && c<0))
{
   cvMatMul(K,S4,Kn);
   cvMatMul(S4,R,Rn);
   cvMatMul(S2,Rd,t0);
}

cvMatMul(t0,h,T);

cvReleaseMat(&t0);
cvReleaseMat(&S1);
cvReleaseMat(&S2);
cvReleaseMat(&S3);
cvReleaseMat(&S4);
cvReleaseMat(&Rd);
cvReleaseMat(&h);

for (i = 0; i < 3; i++)
{
   for (j = 0; j < 3; j++)
   {
    cvmSet(K,i,j,cvmGet(Kn,i,j)/cvmGet(Kn,2,2));
   }
}
cvReleaseMat(&Kn);
cvCopy(Rn,R);
cvReleaseMat(&Rn);
}

void main()
{
//用Matlab仿真的真实图像实验的投影矩阵
double P0[12]={
0.00191854517446,-0.00174998332538,0.00009670477631,-0.63861687292509,
0.00015184786215,0.00005945815592,0.00252273614700,-0.76951593410321,
   -0.00000042931672,-0.00000093480944,0.00000027152848,-0.00075721579556
};

CvMat* P = cvCreateMat(3,4,CV_64FC1);
cvInitMatHeader(P,3,4,CV_64FC1,P0);
CvMat* K = cvCreateMat(3,3,CV_64FC1);
CvMat* R = cvCreateMat(3,3,CV_64FC1);
CvMat* T = cvCreateMat(3,1,CV_64FC1);

KRT_From_P_QR(P,K,R,T);

//输出求解出的内外参数矩阵
int i,j;
cout<<"K:"<<endl;
for (i = 0; i < 3; i++)
{
   for (j = 0; j < 3; j++)
   {
    cout<<cvmGet(K,i,j)<<" ";
   }
   cout<<endl;
}
cout<<endl;
cout<<"R:"<<endl;
for (i = 0; i < 3; i++)
{
   for (j = 0; j < 3; j++)
   {
    cout<<cvmGet(R,i,j)<<" ";
   }
   cout<<endl;
}
cout<<endl;
cout<<"T:"<<endl;
for (i = 0; i < 3; i++)
{
   for (j = 0; j < 1; j++)
   {
    cout<<cvmGet(T,i,j)<<" ";
   }
   cout<<endl;
}

cvReleaseMat(&P);
cvReleaseMat(&K);
cvReleaseMat(&R);
cvReleaseMat(&T);
}

#计算机视觉