实例介绍
【实例简介】基于Opencv库的投影矩阵P分解成K R T三分量，即内参K，外参R T。
【实例截图】用Matlab仿真的真实图像实验的投影矩阵
【实例截图】用Matlab仿真的真实图像实验的投影矩阵
【核心代码】
void HOUSEHOLDER_MULT(CvMat* A, CvMat* V);
void HOUSEHOLDER(CvMat* A, CvMat* V);
void Q_HOUSE(CvMat* A, CvMat* Q);
void KRT_From_P_QR(CvMat* P, CvMat* K, CvMat* R, CvMat* T);
void qrHouse(CvMat* A, CvMat* Q, CvMat* R);
void QR_HOUSE(CvMat* A);
int main()
{
//用Matlab仿真的真实图像实验的投影矩阵
double P0[12]={
1009.984583,305.274308,-1695.377402,-4847.771233,
-47.769902,-1901.303711,-473.239933,-498.560473,
-0.698910,0.016966,-0.715008,2.118015
};
CvMat* P = cvCreateMat(3,4,CV_32FC1);
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);
return 0;
}
////////////////////
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 qrHouse(CvMat* A, CvMat* Q, CvMat* R)
{
CvMat* B = cvCreateMat(A->rows, A->cols, CV_64FC1);
cvCopy(A, B);
QR_HOUSE(B);
CvMat* QT = cvCreateMat(Q->cols,Q->rows,CV_64FC1);
Q_HOUSE(B, QT);
cvMatMul(QT, A, R);
cvTranspose(QT, Q);
cvReleaseMat(&B);
cvReleaseMat(&QT);
}
void QR_HOUSE(CvMat* A)
{
int n = A->rows;
int m = A->cols;
int i, j;
int h, g;
for (int k = 0; k < min(n-1, m); k )
{
CvMat* A0 = cvCreateMat(n-k,1,CV_64FC1);
CvMat* V = cvCreateMat(n-k,1,CV_64FC1);
j = 0;
for (i = k; i < n; i )
{
cvmSet(A0,j,0,cvmGet(A,i,k));
j ;
}
HOUSEHOLDER(A0, V);
cvReleaseMat(&A0);
CvMat* A1 = cvCreateMat(n-k,m-k,CV_64FC1);
for (i = 0, h = k; i < n-k, h < n; i , h )
{
for (j = 0, g = k; j < m-k, g < m; j , g )
{
cvmSet(A1,i,j,cvmGet(A,h,g));
}
}
HOUSEHOLDER_MULT(A1,V);
for (i = 0, h = k; i < n-k, h < n; i , h )
{
for (j = 0, g = k; j < m-k, g < m; j , g )
{
cvmSet(A,h,g,cvmGet(A1,i,j));
}
}
cvReleaseMat(&A1);
for (i = k 1, j = 1; i < n, j < n-k; i , j )
{
cvmSet(A,i,k,cvmGet(V,j,0));
}
cvReleaseMat(&V);
}
}
void Q_HOUSE(CvMat* A, CvMat* Q)
{
int i, j;
int g, h;
int n = A->rows;
cvSetIdentity(Q);
for (int k = 0; k < n-1; k )
{
CvMat* V = cvCreateMat(n-k,1,CV_64FC1);
cvSet(V, cvScalarAll(1), NULL);
for (i = 1, j = k 1; i < n-k, j < n; i , j )
{
cvmSet(V,i,0,cvmGet(A,j,k));
}
CvMat* Qk = cvCreateMat(n,n,CV_64FC1);
cvSetIdentity(Qk);
CvMat* II = cvCreateMat(n-k,n-k,CV_64FC1);
cvSetIdentity(II);
CvMat* VT = cvCreateMat(V->cols,V->rows,CV_64FC1);
CvMat* de = cvCreateMat(1,1,CV_64FC1);
cvTranspose(V,VT);
cvMatMul(VT,V,de);
CvMat* QQ = cvCreateMat(n-k,n-k,CV_64FC1);
cvGEMM(V,VT,-2/cvmGet(de,0,0),II,1,QQ);
cvReleaseMat(&V);
cvReleaseMat(&VT);
cvReleaseMat(&de);
cvReleaseMat(&II);
for (i = k, g = 0; i < n, g < QQ->rows; i , g )
{
for (j = k, h = 0; j < n, h < QQ->cols; j , h )
{
cvmSet(Qk,i,j,cvmGet(QQ,g,h));
}
}
cvReleaseMat(&QQ);
CvMat* Q0 = cvCreateMat(n,n,CV_64FC1);
cvMatMul(Qk,Q,Q0);
cvCopy(Q0, Q);
cvReleaseMat(&Qk);
cvReleaseMat(&Q0);
}
}
void HOUSEHOLDER(CvMat* A, CvMat* V)
{
int n = A->rows;
cvCopy(A, V);
double beta;
if (cvmGet(A,0,0) >= 0)
{
beta = cvmGet(A,0,0) cvNorm(A);
}
else
{
beta = cvmGet(A,0,0) - cvNorm(A);
}
cvmSet(V,0,0,1);
for (int i = 1; i < n; i )
{
cvmSet(V, i, 0, 1/beta*cvmGet(V, i, 0));
}
}
void HOUSEHOLDER_MULT(CvMat* A, CvMat* V)
{
int n = V->rows;
CvMat* beta0 = cvCreateMat(1,1,CV_64FC1);
cvMulTransposed(V, beta0, 1);
double beta = -2/cvmGet(beta0,0,0);
cvReleaseMat(&beta0);
CvMat* w = cvCreateMat(1, A->cols, CV_64FC1);
CvMat* VT = cvCreateMat(V->cols, V->rows, CV_64FC1);
cvTranspose(V, VT);
cvMatMul(VT,A,w);
cvReleaseMat(&VT);
cvGEMM(V,w,beta,A,1,A,0);
cvReleaseMat(&w);
}
标签：
好例子网口号：伸出你的我的手 — 分享！
相关软件
小贴士
感谢您为本站写下的评论，您的评论对其它用户来说具有重要的参考价值，所以请认真填写。
- 类似“顶”、“沙发”之类没有营养的文字，对勤劳贡献的楼主来说是令人沮丧的反馈信息。
- 相信您也不想看到一排文字/表情墙，所以请不要反馈意义不大的重复字符，也请尽量不要纯表情的回复。
- 提问之前请再仔细看一遍楼主的说明，或许是您遗漏了。
- 请勿到处挖坑绊人、招贴广告。既占空间让人厌烦，又没人会搭理，于人于己都无利。
关于好例子网
本站旨在为广大IT学习爱好者提供一个非营利性互相学习交流分享平台。本站所有资源都可以被免费获取学习研究。本站资源来自网友分享，对搜索内容的合法性不具有预见性、识别性、控制性，仅供学习研究，请务必在下载后24小时内给予删除，不得用于其他任何用途，否则后果自负。基于互联网的特殊性，平台无法对用户传输的作品、信息、内容的权属或合法性、安全性、合规性、真实性、科学性、完整权、有效性等进行实质审查；无论平台是否已进行审查，用户均应自行承担因其传输的作品、信息、内容而可能或已经产生的侵权或权属纠纷等法律责任。本站所有资源不代表本站的观点或立场，基于网友分享，根据中国法律《信息网络传播权保护条例》第二十二与二十三条之规定，若资源存在侵权或相关问题请联系本站客服人员，点此联系我们。关于更多版权及免责申明参见 版权及免责申明
网友评论
我要评论