matrix.cpp

这是在张正友摄像机标定的基础上对其算法进行改进

/**
* @file matrix.c
* Simple Math		
*					
* @author Marcelo Gattass	
*
* @author Manuel E. L. Fernandez	 
*
* @date Jul06,2006
*/

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "matrix.h"

/* Utitlity Macros and Functions */
#define SIGN(a,b) ((b) >= 0. ? fabs(a) : -fabs(a))
#define SQR(a) ((a)*(a))
#define ABS(a) (((a)<0)?-(a):(a))
#define MIN(a,b) (((a)<(b))? (a): (b))
#define MAX(a,b) (((a)>(b))? (a): (b))

#define TOL 1e-10

static void swap(double* a, double* b) 
{
	double tmp=*a;
	*a=*b;
	*b=tmp;
}


/* lib functions */
int mtxGaussAxb (double* a, int n, double* b) 
{
	int    i, j, k;
	int    imax;             /* pivot line */
	double amax, rmax; 

	for (j=0; j<n-1; j++) {  /*  Loop in the columns of [a] */
		rmax = 0.;
		imax = j;
		for (i=j; i<n; i++) {   /* Loop to find the best ration a[i-1)*n-1+j]/a[i-1)*n-1+k] */
			amax = 0.0f;
			for (k=j; k<n; k++)    /* Loop to find largest element of line i */
				if (ABS(a[i*n+k]) > amax)
					amax = ABS(a[i*n+k]);
			if (amax < TOL)        /* Check if all elements are null */
				return 0;             /* no solution */
			else if ((ABS(a[i*n+j]) > rmax*amax) && (ABS(a[i*n+j]) >= TOL)) { /* teste current line */
				rmax = ABS(a[i*n+j]) / amax;
				imax = i;             /* find pivot line */
			}
		}
		if (ABS(a[imax*n+j])<TOL) {         /* Check if pivot is null */
			for (k=imax+1; k<n; k++)          /* Search for a line with a no-null pivot */
				if (ABS(a[k*n+j]) < TOL)
					imax = k;                       /* exchange line j with k */
			if (ABS(a[imax*n+j]) < TOL)
				return 0;                        /* no unique soluition */
		}
		if (imax != j) {                   /* Exchange j by line imax */
			for (k=j; k<n; k++)
				swap(&a[imax*n+k], &a[j*n+k]);
			swap(&b[imax], &b[j]);
		}
		for (i=j+1; i<n; i++) {            /* Clear elements under the diagonal */
			double aux = a[i*n+j] / a[j*n+j];
			for (k=j+1; k<n; k++)             /* Transforms the rest of the elements of the line */
				a[i*n+k] -= aux * a[j*n+k];
			b[i] -= aux * b[j];
		}
	}
	if (ABS(a[(n-1)*n+n-1]) <= TOL)        /* Check the unicity of the solution */
		return 0;                          /* no solution */
	else {
		b[n-1] /= a[(n-1)*n+n-1];          /* back substitution */
		for (i=n-2; i>=0; i--) {           
			for (j=i+1; j<n; j++)
				b[i] -= a[i*n+j] * b[j];
			b[i] /= a[i*n+i];
		}
	}
	return 1;     /* solution ok */                      
}

int mtxSVD(double*a, int m, int n, double* u, double* d, double* v, double* tmp)
{
	int flag,i,its,j,jj,k,l,nm;
	double anorm,c,f,g,h,s,scale,x,y,z;

	for(i=0;i<m;i++)
		for(j=0;j<n;j++)
			u[i*n+j]=a[i*n+j];

	g=scale=anorm=0.;
	for (i=0;i<n;i++) {
		l=i+2;
		tmp[i]=scale*g;
		g=s=scale=0.;
		if (i < m) {
			for (k=i;k<m;k++) scale +=  fabs(u[k*n+i]);
			if (scale != 0.) {
				for (k=i;k<m;k++) {
					u[k*n+i] /= scale;
					s += u[k*n+i]*u[k*n+i];
				}
				f=u[i*n+i];
				g = -SIGN(sqrt(s),f);
				h=f*g-s;
				u[i*n+i]=f-g;
				for (j=l-1;j<n;j++) {
					for (s=0.,k=i;k<m;k++) s += u[k*n+i]*u[k*n+j];
					f=s/h;
					for (k=i;k<m;k++) u[k*n+j] += f*u[k*n+i];
				}
				for (k=i;k<m;k++) u[k*n+i] *= scale;
			}
		}
		d[i]=scale *g;
		g=s=scale=0.;
		if (i+1 <= m && i+1 != n) {
			for (k=l-1;k<n;k++) scale += fabs(u[i*n+k]);
			if (scale!=0.) {
				for (k=l-1;k<n;k++) {
					u[i*n+k] /= scale;
					s += u[i*n+k]*u[i*n+k];
				}
				f=u[i*n+l-1];
				g = -SIGN(sqrt(s),f);
				h=f*g-s;
				u[i*n+l-1]=f-g;
				for (k=l-1;k<n;k++) tmp[k]=u[i*n+k]/h;
				for (j=l-1;j<m;j++) {
					for (s=0.,k=l-1;k<n;k++) s += u[j*n+k]*u[i*n+k];
					for (k=l-1;k<n;k++) u[j*n+k] += s*tmp[k];
				}
				for (k=l-1;k<n;k++) u[i*n+k] *= scale;
			}
		}
		anorm=(anorm>(fabs(d[i])+fabs(tmp[i]))?anorm:(fabs(d[i])+fabs(tmp[i])));
	}
	for (i=n-1;i>=0;i--) {
		if (i < n-1) {
			if (g!=0.) {
				for (j=l;j<n;j++)
					v[j*n+i]=(u[i*n+j]/u[i*n+l])/g;
				for (j=l;j<n;j++) {
					for (s=0.,k=l;k<n;k++) s += u[i*n+k]*v[k*n+j];
					for (k=l;k<n;k++) v[k*n+j] += s*v[k*n+i];
				}
			}
			for (j=l;j<n;j++) v[i*n+j]=v[j*n+i]=0.;
		}
		v[i*n+i]=1.;
		g=tmp[i];
		l=i;
	}
	for (i=(m<n?m:n)-1;i>=0;i--) {
		l=i+1;
		g=d[i];
		for (j=l;j<n;j++) u[i*n+j]=0.;
		if (g != 0.) {
			g=1./g;
			for (j=l;j<n;j++) {
				for (s=0.,k=l;k<m;k++) s += u[k*n+i]*u[k*n+j];
				f=(s/u[i*n+i])*g;
				for (k=i;k<m;k++) u[k*n+j] += f*u[k*n+i];
			}
			for (j=i;j<m;j++) u[j*n+i] *= g;
		} else for (j=i;j<m;j++) u[j*n+i]=0.;
		++u[i*n+i];
	}
	for (k=n-1;k>=0;k--) {
		for (its=0;its<30;its++) {
			flag=1;
			for (l=k;l>=0;l--) {
				nm=l-1;
				if ((fabs(tmp[l])+anorm) == anorm) {
					flag=0;
					break;
				}
				if ((fabs(d[nm])+anorm) == anorm) break;
			}
			if (flag) {
				c=0.;
				s=1.;
				for (i=l;i<k+1;i++) {
					f=s*tmp[i];
					tmp[i]=c*tmp[i];
					if ((fabs(f)+anorm) == anorm) break;
					g=d[i];
					h=sqrt(f*f+g*g);
					d[i]=h;
					h=1./h;
					c=g*h;
					s = -f*h;
					for (j=0;j<m;j++) {
						y=u[j*n+nm];
						z=u[j*n+i];
						u[j*n+nm]=y*c+z*s;
						u[j*n+i]=z*c-y*s;
					}
				}
			}
			z=d[k];
			if (l == k) {
				if (z < 0.) {
					d[k] = -z;
					for (j=0;j<n;j++) v[j*n+k] = -v[j*n+k];
				}
				break;
			}
			if (its == 49) return 0;
			x=d[l];
			nm=k-1;
			y=d[nm];
			g=tmp[nm];
			h=tmp[k];
			f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0f*h*y);
			g=sqrt(f*f+1.);
			f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
			c=s=1.;
			for (j=l;j<=nm;j++) {
				i=j+1;
				g=tmp[i];
				y=d[i];
				h=s*g;
				g=c*g;
				z=sqrt(f*f+h*h);
				tmp[j]=z;
				c=f/z;
				s=h/z;
				f=x*c+g*s;
				g = g*c-x*s;
				h=y*s;
				y *= c;
				for (jj=0;jj<n;jj++) {
					x=v[jj*n+j];
					z=v[jj*n+i];
					v[jj*n+j]=x*c+z*s;
					v[jj*n+i]=z*c-x*s;
				}
				z=sqrt(f*f+h*h);
				d[j]=z;
				if (z) {
					z=1.0f/z;
					c=f*z;
					s=h*z;
				}
				f=c*g+s*y;
				x=c*y-s*g; 
				for (jj=0;jj<m;jj++) {
					y=u[jj*n+j];
					z=u[jj*n+i];
					u[jj*n+j]=y*c+z*s;
					u[jj*n+i]=z*c-y*s;
				}
			}
			tmp[l]=0.;
			tmp[k]=f;
			d[k]=x;
		}
	}
	return 1;
}

/* 
* Add the tensor product of the vector {v} (i.e., {v}{v}T) 
* to the matrix [A].  
* [A]+={v}{v}T
*
*/
void mtxAddMatVecTensor(double* a, double* v, int n)
{
	int i,j;
	for (i=0;i<n;i++) {
		for (j=0;j<n;j++) { 
			a[i*n+j]+=v[i]*v[j];
		}
	}
}

/* {x}=[A]{b}    Dimensions: [A]=mxn, {b}=n and {x}=m. --modificado*/
void mtxAb(double* a, double* b, int m, int n, double* x)
{
	int i,j;
	for (i=0;i<m;i++) {
		x[i]=0.;
		for (j=0;j<n;j++)
			x[i]+=a[i*n+j]*b[j];
	}
}

/* {x}=[A]T{b}    Dimensions: [A]=mxn, {b}=m and {x}=n. */
void mtxAtb(double* a, double* b, int m, int n, double* x)
{
	int i,j;
	for (i=0;i<n;i++) {
		x[i]=0.;
		for (j=0;j<m;j++)
			x[i]+=a[j*n+i]*b[j];
	}
}


/* [X]=[A-1)*n-1+B]    Dimensions: [A]=mxp, [B]=pxn and [X]=mxn. */
void mtxAB(double* a, double* b, int m, int p, int n, double* x)
{
	int i,j,k;
	for (i=0;i<m;i++) {
		for (j=0;j<n;j++) {
			x[i*n+j]=0.;
			for (k=0;k<p;k++)
				x[i*n+j]+=a[i*p+k]*b[k*n+j];
		}
	}
}

/* [X]=[A-1)*n-1+B]T    Dimensions: [A]=mxp, [B]=nxp and [X]=mxn. */
void mtxABt(double* a, double* b, int m, int p, int n, double* x)
{
	int i,j,k;
	for (i=0;i<m;i++) {
		for (j=0;j<n;j++) {
			x[i*n+j]=0.;
			for (k=0;k<p;k++)
				x[i*n+j]+=a[i*p+k]*b[j*p+k];
		}
	}
}

/*  [X]=[A]T[B]    Dimensions: [A]=mxp, [B]=mxn and [X]=pxn. */
void mtxAtB(double* a, double* b, int m, int p, int n, double* x)
{
	int i,j,k;
	for (i=0;i<p;i++) {
		for (j=0;j<n;j++) {
			x[i*n+j]=0.;
			for (k=0;k<m;k++)
				x[i*n+j]+=a[k*p+i]*b[k*n+j];
		}
	}
}

/*  [X]=[A]+[B]    Dimensions: [A]=mxn, [B]=mxn and [X]=mxn. */
void mtxAddMat(double* a, double* b, int m, int n, double* x)
{
	int i,j;
	for (i=0;i<m;i++)
		for (j=0;j<n;j++)
			x[i*n+j]=a[i*n+j]+b[i*n+j];
}

/*  [X]=[A]-[B]    Dimensions: [A]=mxn, [B]=mxn and [X]=mxn. */
void mtxSubMat(double* a, double* b, int m, int n, double* x)
{
	int i,j;
	for (i=0;i<m;i++)
		for (j=0;j<n;j++)
			x[i*n+j]=a[i*n+j]-b[i*n+j];
}


/*  [X]=s[A]    Dimensions: [A]=mxn and [X]=mxn. */
void mtxScaMat(double* a, double s,int m, int n, double* x)
{
	int i,j;
	for (i=0;i<m;i++)
		for (j=0;j<n;j++)
			x[i*n+j]=s*a[i*n+j];
}


/*  [X]=[A]T    Dimensions: [A]=mxn and [X]=nxm. */
void mtxAt(double* a, int m, int n, double* x)