12-2.cpp

基于OpenCV的计算机视觉技术实现.rar

//《计算机视觉》书后面的128个点，求解结果

///*

#pragma comment (lib,"highgui.lib")
#pragma comment (lib,"cxcore.lib")
#pragma comment (lib,"cv.lib")


#include <cv.h>
#include <highgui.h>
#include <stdio.h>
#include <iostream.h>

#include <iomanip.h>


#define num  128			//7幅不同旋转角度下拍摄的图，每幅图提取对应位置的36各点

void main()
{
	double *Wxyz;			//指向世界坐标系的指针数组

	double *Iuv;			//指向图像坐标系的指针数组
	

	char dt[]="d";			//指明存储数据类型
	
	int i=0;
	int j=0;


		//-------------测试代码运行时间变量-----------------
	double tick_frequency=0.0;			//每微秒的ticks数目

	int64 tick_num_begin=0;				//代码运行前的ticks数目

	int64 tick_num_end=0;				//代码运行后的ticks数目

	double cost_time=0.0;				//代码执行的时间（单位为秒）


	CvFileStorage* fs_read;		//读取数据文件存储器
	

	
	CvMat* mat_K = cvCreateMat( 2*num, 11, CV_64F );	//K矩阵 式（4.12）的左部分
	
	CvMat* mat_K_tran= cvCreateMat( 11, 2*num, CV_64F );	//K矩阵的转置
	
	CvMat* mat_KK = cvCreateMat( 11, 11, CV_64F );		//K的转置乘以K
	
	CvMat* mat_KK_inv = cvCreateMat( 11, 11, CV_64F );	//KK的逆矩阵
	
	CvMat* mat_m = cvCreateMat( 11, 1, CV_64F );		//m向量（11个元素）
	
	CvMat* mat_U = cvCreateMat( 2*num, 1, CV_64F );		//U向量（2*num个元素）
	
	CvMat* mat_K_U = cvCreateMat( 11, 1, CV_64F );		//K转置乘以U向量（11个元素）




		
	Wxyz=new double[num*3];			//世界坐标系的元素个数num*3

	Iuv=new double[num*2];			//图像坐标系的元素个数num*2


	
	//---------------------数据的读取------------------------

			//----读取世界坐标系-----
	fs_read = cvOpenFileStorage( "world_coordinate.xml", 0, CV_STORAGE_READ);

	
	cvReadRawData( fs_read, cvGetRootFileNode( fs_read ), Wxyz, dt );
	
	cvReleaseFileStorage (&fs_read);
	
			//----读取图像坐标系-----
	
	fs_read = cvOpenFileStorage( "image_coordinate.xml", 0, CV_STORAGE_READ);

	cvReadRawData( fs_read, cvGetRootFileNode( fs_read ), Iuv, dt );
	
	cvReleaseFileStorage (&fs_read);

	//---------- 显示读取的数据------------------
	for(i=0; i<3*num; )
	{
		for( j=0; j<3; j++)
		{
			//cout<<setw(8)<<Wxyz[i];
			i++;
		}
		
		//cout<<endl;
		
	}
	
	for(i=0; i<2*num; )
	{
		for( j=0; j<2; j++)
		{
			//cout<<setw(8)<<Iuv[i];
			i++;
		}
		
		//cout<<endl;
		
	}







	cvZero(mat_K);					//K矩阵所有元素先置零


	//----------------------设置K矩阵的元素------------

	for( i=0, j=0;  i< 2*num;  i=i+2, j=j+3 )
	{
		//cout<<CV_MAT_ELEM( *mat_K, double, i, 0 )<<endl;

		//对K矩阵的第i行的元素进行设置

		CV_MAT_ELEM( *mat_K, double, i, 0 )=Wxyz[j];
		CV_MAT_ELEM( *mat_K, double, i, 1 )=Wxyz[j+1];
		CV_MAT_ELEM( *mat_K, double, i, 2 )=Wxyz[j+2];
		CV_MAT_ELEM( *mat_K, double, i, 3 )=1;

		CV_MAT_ELEM( *mat_K, double, i, 8 )=-Iuv[i]*Wxyz[j];
		CV_MAT_ELEM( *mat_K, double, i, 9 )=-Iuv[i]*Wxyz[j+1];
		CV_MAT_ELEM( *mat_K, double, i, 10 )=-Iuv[i]*Wxyz[j+2];

		//对K矩阵的第i+1行的元素进行设置

		CV_MAT_ELEM( *mat_K, double, i+1, 4 )=Wxyz[j];
		CV_MAT_ELEM( *mat_K, double, i+1, 5 )=Wxyz[j+1];
		CV_MAT_ELEM( *mat_K, double, i+1, 6 )=Wxyz[j+2];
		CV_MAT_ELEM( *mat_K, double, i+1, 7 )=1;

		CV_MAT_ELEM( *mat_K, double, i+1, 8 )=-Iuv[i+1]*Wxyz[j];
		CV_MAT_ELEM( *mat_K, double, i+1, 9 )=-Iuv[i+1]*Wxyz[j+1];
		CV_MAT_ELEM( *mat_K, double, i+1, 10 )=-Iuv[i+1]*Wxyz[j+2];

	
	}


	//------------输出K矩阵的元素--------------------


	for( i=0 ;  i< 2*num;  i++ )
	{
		for( j=0; j<11; j++)
		{
			//cout<<setw(8)<<CV_MAT_ELEM( *mat_K, double, i, j );		
		}

		//cout<<endl;


	}


	//---------求K的转置矩阵---------
	cvTranspose( mat_K, mat_K_tran );


	
	//------K的转置与K矩阵的乘积-------

	cvMatMul( mat_K_tran, mat_K, mat_KK );



	//--------mat_KK的逆矩阵--------
	tick_num_begin=cvGetTickCount();
	//代码执行前的tick数目
	cvInvert( mat_KK, mat_KK_inv, CV_LU);		//CV_LU的速度比CV_SVD奇异值分解的方式

//	cvInvert( mat_KK, mat_KK_inv, CV_SVD);		//CV_LU的速度比CV_SVD奇异值分解的方式

	tick_num_end=cvGetTickCount();		//代码执行后的tick数目



	tick_frequency=cvGetTickFrequency();	//每微秒的ticks数目

	cost_time=( tick_num_end - tick_num_begin ) / tick_frequency ;

	cout<<" 矩阵求逆运行需要实际时间："<<cost_time<<endl;


	for( i=0 ;  i< 11;  i++ )
	{
		for( j=0; j<11; j++)
		{
		//	cout<<setw(15)<<CV_MAT_ELEM( *mat_KK_inv, double, i, j );		
		}
		
	//	cout<<endl;
		
		
	}
	
	


	//--------求向量m（11个元素）------------

	for( i=0; i< 2*num; i++ )
	{
		CV_MAT_ELEM(*mat_U, double,i,0)=Iuv[i];

	}


	cvMatMul( mat_K_tran, mat_U , mat_K_U);		//K^T*U

	cvMatMul(  mat_KK_inv, mat_K_U, mat_m );	//m=(K^T*K)^(-1)*K^T*U	


	
	//----------输出求得的m-----------

	cout<<"m: "<<endl;

	for( j=0; j<11; j++)
	{
		cout<<CV_MAT_ELEM( *mat_m, double, j, 0 )<<endl;		
	}
	




	CvMat* m1=cvCreateMat( 3, 1, CV_64F );		//列向量 m1=[m11; m12; m13]
	CvMat* m1_tran=cvCreateMat( 1, 3, CV_64F );	//m1_tran是m1的转置=[m11 m12 m13]

	CvMat* m2=cvCreateMat( 3, 1, CV_64F );		//列向量m2=[m21; m22; m23]
	CvMat* m2_tran=cvCreateMat( 1, 3, CV_64F );	//m2_tran是m2的转置=[m21 m22 m23]

	CvMat* m3=cvCreateMat( 3, 1, CV_64F );		//列向量m3=[m31; m32; m33]
	CvMat* m3_tran=cvCreateMat( 1, 3, CV_64F );	//m3_tran是m3的转置=[m31 m32 m33]


	CvMat* m1_cross_m3=cvCreateMat( 3, 1, CV_64F );	//列向量 m1×m3
	CvMat* m2_cross_m3=cvCreateMat( 3, 1, CV_64F );	//列向量 m2×m3



	for(i=0; i<3; i++)
	{
		CV_MAT_ELEM( *m1, double, i, 0 )=CV_MAT_ELEM( *mat_m, double,  i ,0);
		CV_MAT_ELEM( *m2, double, i, 0 )=CV_MAT_ELEM( *mat_m, double,  4+i,0 );
		CV_MAT_ELEM( *m3, double, i, 0 )=CV_MAT_ELEM( *mat_m, double,  8+i, 0 );
	
	}

	cvTranspose( m1, m1_tran );
	cvTranspose( m2, m2_tran );
	cvTranspose( m3, m3_tran );



	//for(i=0; i<3; i++)			//显示数据
	//{
		//cout<<CV_MAT_ELEM( *m2_tran, double, 0, i )<<endl;
	
	//}

	double m14=0.0;
	double m24=0.0;
	double m34=0.0;


	double u0=0.0;
	double v0=0.0;
	double alpha_x=0.0;
	double alpha_y=0.0;

	CvMat* r1=cvCreateMat( 3, 1, CV_64F );		//列向量 r1
	CvMat* r2=cvCreateMat( 3, 1, CV_64F );		//列向量 r2
	CvMat* r3=cvCreateMat( 3, 1, CV_64F );		//列向量 r3



	m34=1.0 / sqrt( cvDotProduct(m3,m3) ) ;		//cvDotProduct的运算对象要一样的哦

	cout<<endl<<endl;

	//cout<<"m34: "<<m34<<endl;


	//-----------求u0,v0 并输出-----------
	u0=m34*m34*cvDotProduct(m1,m3);
	v0=m34*m34*cvDotProduct(m2,m3);
	cout<<"u0: "<<u0<<endl;
	cout<<"v0: "<<v0<<endl;


	//------求alpha_x,alpha_y 并输出--------

	cvCrossProduct( m1, m3, m1_cross_m3 );
	cvCrossProduct( m2, m3, m2_cross_m3 );

	alpha_x=m34*m34*sqrt(cvDotProduct(m1_cross_m3,m1_cross_m3));
	alpha_y=m34*m34*sqrt(cvDotProduct(m2_cross_m3,m2_cross_m3));


	cout<<"alpha_x: "<<alpha_x<<endl;
	cout<<"alpha_y: "<<alpha_y<<endl;

	//-----------求r1,r2,r3并输出-----------

	cvScaleAdd( m3, cvRealScalar( -1.0*u0 ), m1, r1 );

	cvConvertScale( r1, r1, m34/alpha_x);


	cvScaleAdd( m3, cvRealScalar( -1.0*v0 ), m2, r2 );

	cvConvertScale( r2, r2, m34/alpha_y);

	

	cvConvertScale( m3, r3, m34);
	

	cout<<"r1: ";
	for(i=0; i<3; i++)			//显示r1数据
	{
		cout<<CV_MAT_ELEM( *r1, double, i, 0 )<<"  ";
		
	}cout<<endl;

	cout<<"r2: ";
	for(i=0; i<3; i++)			//显示r2数据
	{
		cout<<CV_MAT_ELEM( *r2, double, i, 0 )<<"  ";
		
	}cout<<endl;
	
	cout<<"r3: ";
	for(i=0; i<3; i++)			//显示r3数据
	{
		cout<<CV_MAT_ELEM( *r3, double, i, 0 )<<"  ";
		
	}cout<<endl;
	//------求tx ty  tz 并输出-----------

	double tx=0.0;
	double ty=0.0;
	double tz=0.0;


	tx = m34/alpha_x * ( CV_MAT_ELEM( *mat_m, double,  3 ,0)-u0 );
	ty = m34/alpha_y * ( CV_MAT_ELEM( *mat_m, double,  7 ,0)-v0 );
	tz = m34;

	cout<<"tx: "<<tx<<endl;
	cout<<"ty: "<<ty<<endl;
	cout<<"tz: "<<tz<<endl;

	

	delete []Wxyz;			//指向世界坐标系的指针数组
	delete []Iuv;			//指向图像坐标系的指针数组
	
	cvReleaseMat( &mat_K );		//释放矩阵
	cvReleaseMat( &mat_K_tran );	
	cvReleaseMat( &mat_KK );	
	cvReleaseMat( &mat_KK_inv  );
	cvReleaseMat( &mat_m );	
	cvReleaseMat( &mat_U );	
	cvReleaseMat( &mat_K_U);


	cvReleaseMat( &m1 );
	cvReleaseMat( &m1_tran );
	cvReleaseMat( &m2 );
	cvReleaseMat( &m2_tran );
	cvReleaseMat( &m3 );
	cvReleaseMat( &m3_tran );

	cvReleaseMat( &m1_cross_m3 );
	cvReleaseMat( &m2_cross_m3 );

	cvReleaseMat( &r1 );
	cvReleaseMat( &r2 );
	cvReleaseMat( &r3 );


}

//*/