invertmatrix.c

矩阵求逆的算法

// invertmatrix.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"

#include <stdlib.h> 
#include <math.h> 
#include <stdio.h> 
#include "highgui.h"
#include "cxcore.h"
#include "cv.h"

int CR_InvertMatrix(double a[], int n) 
{	
	int *is,*js,i,j,k,l,u,v; 
	double d,p; 
	is=(int *)malloc(n*sizeof(int)); 
	js=(int *)malloc(n*sizeof(int)); 
	for (k=0; k<=n-1; k++) 
	{ 
		d=0.0; 
		for (i=k; i<=n-1; i++) 
			for (j=k; j<=n-1; j++) 
			{ 
				l=i*n+j; 
				p=fabs(a[l]); 
				if (p>d) 
				{ 
					d=p;
					is[k]=i; 
					js[k]=j;
				} 
			} 
		if (d+1.0==1.0) 
		{ 
			free(is); 
			free(js);
			printf("err**not inv\n"); 
			return(0); 
		} 
		if (is[k]!=k) 
			for (j=0; j<=n-1; j++) 
			{ 
				u=k*n+j; 
				v=is[k]*n+j; 
				p=a[u]; 
				a[u]=a[v]; 
				a[v]=p; 
			} 
		if (js[k]!=k) 
			for (i=0; i<=n-1; i++) 
			{ 
				u=i*n+k; 
				v=i*n+js[k]; 
				p=a[u]; 
				a[u]=a[v]; 
				a[v]=p; 
			} 
		l=k*n+k; 
		a[l]=1.0/a[l]; 
		for (j=0; j<=n-1; j++) 
			if (j!=k) 
			{ 
				u=k*n+j; 
				a[u]=a[u]*a[l];
			} 
		for (i=0; i<=n-1; i++) 
			if (i!=k) 
				for (j=0; j<=n-1; j++) 
					if (j!=k) 
					{ 
						u=i*n+j; 
						a[u]=a[u]-a[i*n+k]*a[k*n+j]; 
					} 
		for (i=0; i<=n-1; i++) 
			if (i!=k) 
			{
				u=i*n+k; 
				a[u]=-a[u]*a[l];
			} 
	} 
	for (k=n-1; k>=0; k--) 
	{ 
		if (js[k]!=k) 
			for (j=0; j<=n-1; j++) 
			{
				u=k*n+j; 
				v=js[k]*n+j; 
				p=a[u];
				a[u]=a[v]; 
				a[v]=p; 
			} 
		if (is[k]!=k) 
			for (i=0; i<=n-1; i++) 
			{
				u=i*n+k;
				v=i*n+is[k]; 
				p=a[u]; 
				a[u]=a[v]; 
				a[v]=p; 
			} 
	} 
	free(is); 
	free(js); 
	return(1); 
} 

void brmul(double a[], double b[],int m,int n,int k,double c[]) 
{ int i,j,l,u; 
	for (i=0; i<=m-1; i++) 
		for (j=0; j<=k-1; j++) 
		{ 
			u=i*k+j; c[u]=0.0; 
			for (l=0; l<=n-1; l++) 
			c[u]=c[u]+a[i*n+l]*b[l*k+j]; 
		} 
	return; 
} 
void daodMutiMatx(double R[3][3],double pw[3],double ret[3] )
{
	int i,j;
	double sum = 0;
	for ( i = 0; i < 3; i++)
	{
		for ( j = 0; j < 3; j++)
		{
			sum += R[i][j] * pw[j];
		}
		ret[i] = sum;
		sum = 0.0;
	}
}
__inline daodMat creatdaodMat( int rows, int cols, int type, void* data )
{
	daodMat m;

	m.cols = cols;
	m.rows = rows;
	//m.step = rows > 1 ? m.cols*CV_ELEM_SIZE(type) : 0;
	m.data.ptr = (uchar*)data;
	return m;
}
void daodGetRoadEquation( )
{
 
	//lpAbc->a = 0.045075;
	//lpAbc->b = 0.381996;
	//lpAbc->c = 0.632894;

	//double Alph = lpCameraparam->alph;
	//double Beta = lpCameraparam->beta;
	//double Gama = lpCameraparam->gama;
	//
	double Alph = 0;
	double Beta = 0;
	double Gama = 0;
	double gameraHeight = 0.5f; //lpCameraparam->cameraHeight;
	double f = 1.0f;//lpCameraparam->f;

	int i,j;
	double sum = 0;
	double coe[3];
	double R[3][3];
	double pc[3][3];
	double retPc[3][3];
	double sumd = 0;
	double coed[3];

	double pw1[3] = {0,gameraHeight,0};
	double pw2[3] = {1,gameraHeight,0};
	double pw3[3] = {0,gameraHeight,1};
	double a[9];
	int k = 0;
	CvMat Ma;
	CvMat Ma1;
	//
	daodMat daMa;
	daodMat daMa1;

	R[0][0] = cos(Gama) * cos(Beta);
	R[0][1] = cos(Gama)*sin(Beta)*sin(Alph)-sin(Gama)*cos(Alph);
	R[0][2] = cos(Gama)*sin(Beta)*cos(Alph)+sin(Gama)*sin(Alph);
	R[1][0] = sin(Gama)*cos(Beta);
	R[1][1] = cos(Alph)*cos(Gama)+sin(Gama)*sin(Beta)*sin(Alph);
	R[1][2] = sin(Gama)*sin(Beta)*cos(Alph)-cos(Gama)*sin(Alph);
	R[2][0] = -sin(Beta);
	R[2][1] = cos(Beta)*sin(Alph);
	R[2][2] = cos(Beta)*cos(Alph);

	daodMutiMatx(R,pw1,pc[0]);
	daodMutiMatx(R,pw2,pc[1]);
	daodMutiMatx(R,pw3,pc[2]);

	for ( i = 0; i < 3; i++)
	{
		for ( j = 0; j < 3; j++ )
		{
			retPc[j][i] = pc[i][j];
		}
	}

	for ( i = 0; i < 3; i++)
	{
		for ( j = 0; j < 3; j++ )
		{
			a[k++] = retPc[i][j];
		}
	}

	Ma= cvMat(3, 3, CV_64FC1, a);
	Ma1 = cvMat(3, 3, CV_64FC1, a);
	//
	daMa = creatdaodMat(3, 3, CV_64FC1, a);
	daMa1 = creatdaodMat(3, 3, CV_64FC1, a);

	cvInvert(&Ma,&Ma1,0);
	//
	CR_InvertMatrix(daMa.data.db,daMa.cols);

	for (  i = 0; i < 3; i++ )
	{
		for ( j = 0; j < 3; j++ )
		{
			sum += cvmGet(&Ma1,j,i);
			//
			sumd += *(daMa.data.db + j * (daMa.cols) + i);
		}
		coe[i] = sum;
		sum = 0;

		coed[i] = sumd;
		sumd = 0;
	}


	coe[0] /f;
	coe[1] /f;
	coe[2];

	coed[0] /f;
	coed[1] /f;
	coed[2];
	//	
	0.0;
	0.00040330711837;
	0.0;

}
int _tmain(int argc, _TCHAR* argv[])
{
	
	int i,j; 
	IplImage * pimage = cvLoadImage("medtest.png",CV_LOAD_IMAGE_ANYDEPTH | CV_LOAD_IMAGE_ANYCOLOR);
	cvNamedWindow("show",CV_WINDOW_AUTOSIZE);
	cvShowImage("show",pimage);
	


	cvDestroyWindow("result");
	cvReleaseImage(&pimage);	
	system("pause");
	return 0;
}