解超定方程求透视矩阵

float* fInit1DPointer(int num)
{
	register int i;

	float* p = new float[num];

	for(i=0; i<num; i++)
	{
		p[i]=0.0f;
	}
	return(p);
}

float** fInit2DPointer(int row,int col)
{
	register int i,j;

	float* arr = new float[row*col];
	float** p = new float*[row];
	for(i=0; i<row; i++)
	{
		p[i] = arr + i*col;

		for(j=0; j<col; j++)
		{
			p[i][j]=(float)0.0;
		}
	}
	return(p);
}

float *cal_axb(float **a, float **b, int m, int n)
{
	float **at;//a的转置
	float **ata;//at与a的乘积
	float **atb;//at与b的乘积
	float **u;//[ata,atb]
	at=fInit2DPointer(n,m);
	ata=fInit2DPointer(n,n);
	atb=fInit2DPointer(n,1);
	u=fInit2DPointer(n,n+1);

	float *x;
	x=fInit1DPointer(8);	

	int i,j,k,r;
	//转置
	for(i=0;i<m;i++)
	{
		for(j=0;j<n;j++)
		{
			at[j][i]=a[i][j];           
		}
	}
	//ata
	for(i=0;i<n;i++)
	{
		for(j=0;j<n;j++)
		{
			for(k=0;k<m;k++)
			{
				ata[i][j]+=at[i][k]*a[k][j];
			}	
		}
	}
	//atb
	for(i=0;i<n;i++)
	{
		for(j=0;j<1;j++)
		{
			for(k=0;k<m;k++)
				atb[i][j]+=at[i][k]*b[k][j];
		}
	}
	//[ata,atb]
	for(i=0;i<n;i++)	
	{
		for(j=0;j<n;j++)
		{	
			u[i][j]=ata[i][j];
		}
		u[i][n]=atb[i][0];
	}

	for(r=0;r<n;r++)
	{
		for(i=r;i<=n;i++)
			for(k=0;k<r;k++)
				u[r][i]-=u[r][k]*u[k][i];
		for(i=r+1;i<n;i++)
		{
			for(k=0;k<r;k++)
				u[i][r]-=u[i][k]*u[k][r];
			u[i][r]/=u[r][r];
		}		
	}
	for(i=n-1;i>=0;i--)
	{
		for(r=n-1;r>=i+1;r--)
			u[i][n]-=u[i][r]*x[r];
		x[i]=u[i][n]/u[i][i];
	}
	return x;
}
		
void get_H_matrix(matchingslist matchings,CvMat* H_Mat1,CvMat* H_Mat2)
{
	int numPoints=matchings.size();
	int i,j;
	CvPoint2D32f *points1=new CvPoint2D32f[numPoints];
	CvPoint2D32f *points2=new CvPoint2D32f[numPoints];
	matchingslist::iterator ptr = matchings.begin();
	for(i=0;i<numPoints;ptr++,i++)
	{
		points1[i].x=cvRound(ptr->first.x);
		points1[i].y=cvRound(ptr->first.y);
		points2[i].x=cvRound(ptr->second.x);
		points2[i].y=cvRound(ptr->second.y);
	}
	float **a;
	float **b;
	a=fInit2DPointer(2*numPoints,8);
	b=fInit2DPointer(2*numPoints,1);

	for(i = 0; i <numPoints; ++i )
	{
		a[i][0] = a[i+numPoints][3] =points1[i].x;
		a[i][1] = a[i+numPoints][4] =points1[i].y;
		a[i][2] = a[i+numPoints][5] = 1;
		a[i][3] = a[i][4] = a[i][5] =0;
		a[i+numPoints][0] = a[i+numPoints][1] = a[i+numPoints][2] = 0;

		a[i][6] = -points1[i].x*points2[i].x;
		a[i][7] = -points1[i].y*points2[i].x;
		a[i+numPoints][6] = -points1[i].x*points2[i].y;
		a[i+numPoints][7] = -points1[i].y*points2[i].y;

		b[i][0]=points2[i].x;
		b[i+numPoints][0] =points2[i].y;
	}

	float *x;
	x=fInit1DPointer(8);	
	x=cal_axb(a, b,2*numPoints,8);

	for(i=0;i<8;i++)
		cvmSet(H_Mat1,i/3,i%3,x[i]);
	cvmSet(H_Mat1,2,2,1.0);
	
	delete [] *a;
	delete [] a;
	delete [] *b;
	delete [] b;
	delete [] x;
}