/***************************************************************
将投影矩阵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);
}
