依赖Opencv的投影矩阵分解得到K R T分量

实例介绍

【实例简介】基于Opencv库的投影矩阵P分解成K R T三分量，即内参K，外参R T。
【实例截图】用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小时内给予删除，不得用于其他任何用途，否则后果自负。基于互联网的特殊性，平台无法对用户传输的作品、信息、内容的权属或合法性、安全性、合规性、真实性、科学性、完整权、有效性等进行实质审查；无论平台是否已进行审查，用户均应自行承担因其传输的作品、信息、内容而可能或已经产生的侵权或权属纠纷等法律责任。本站所有资源不代表本站的观点或立场，基于网友分享，根据中国法律《信息网络传播权保护条例》第二十二与二十三条之规定，若资源存在侵权或相关问题请联系本站客服人员，点此联系我们。关于更多版权及免责申明参见版权及免责申明

依赖Opencv的投影矩阵分解得到K R T分量

同类人气实例

实例介绍

实例下载地址