matrix.cpp

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

{
	int i,j;
	for (i=0;i<m;i++)
		for (j=0;j<n;j++)
			x[j*m+i]=a[i*n+j];
}

/*  [X]=[A]    Dimensions: [A]=mxn and [X]=mxn. */
void mtxMatCopy(double* a, 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];
}

/*  {x}={v}+{u}  Dimensions: {v}=m, {u}=m and {x}=m. */
void mtxAddVec(double* u, double* v, int m, double* x)
{
	int i;
	for (i=0;i<m;i++) x[i]=u[i]+v[i];
}

/*  {x}={v}-{u}  Dimensions: {v}=m, {u}=m and {x}=m. */
void mtxSubVec(double* u, double* v, int m, double* x)
{
	int i;
	for (i=0;i<m;i++) x[i]=u[i]-v[i];
}

/*  {x}=s{u}  Dimensions: {u}=m and {x}=m. */
void mtxScaVec(double* u, int m,  double s, double* x)
{
	int i;
	for (i=0;i<m;i++) x[i]=s*u[i];
}

/*  s={v}.{u}  Dimensions: {v}=m and  {u}=m. */
double mtxDotProd(double* u, double* v, int m)
{
	double tmp=0;
	int i;
	for (i=0;i<m;i++) 
		tmp+=u[i]*v[i];
	return tmp;
}

/*  {x}={u}x{v}  Dimensions: {v}=m and  {u}=m. */
void mtxProdVec3(double* u, double* v, double*x)
{
	x[0] = u[1]*v[2] - u[2]*v[1] ;
	x[1] = u[2]*v[0] - u[0]*v[2] ;
	x[2] = u[0]*v[1] - u[1]*v[0] ;
}

double mtxNormalizeVector(int n, double* v)
{
	double norm=mtxDotProd(v,v,n);
	int i;
	if (norm>TOL) {
		norm = sqrt(norm);
		for (i=0;i<n;i++) v[i]/=norm;
	}
	return norm;
}

void mtxCol(double* A,int col,  int m, int n, double* x)
{
	int i;
	for (i=0;i<m;i++){
		x[i]=A[i*n+col];
	}
}

/* {x}={v} Dimensions: {v}={x}=m. */
void mtxVecCopy(double* v, int m, double* x)
{
	int i;
	for(i=0;i<m;i++) x[i]=v[i];
}



/* Print matrix */
void mtxShowMat(char*  title, double* a, int m, int n)
{
	int i,j;
	printf("\n%s",title);
	for (i=0;i<m;i++){
		for (j=0;j<n;j++){
			if ((j%6)==0) printf("\n");
			printf(" %lf ", a[i*n+j]);
		}
	}
	printf("\n");
}

/* Print vector */
void mtxShowVec(char*  title, double* v, int n)
{
	int j;
	printf("\n%s",title);
	for (j=0;j<n;j++){
		if ((j%6)==0) printf("\n");
		printf("\t%lf ", v[j]);
	}
	printf("\n");
}



int mtxDecompLU ( double* a, int n, int *rp, double* d, double* scale )
{
	int i, imax, j, k;
	double max, tmp, sum;

	rp--;
	scale--;

	*d=1.;                   /* No row interchanges (yet). */
	for (i=0;i<n;i++) {     /* Loop over rows to get the implicit scaling */
		max=0.;          
		for (j=0;j<n;j++)
			max = (max>fabs(a[i*n+j]))?max:fabs(a[i*n+j]);
		if (max == 0.) {
			printf( "\nLUdmcp: Singular Matrix");
			return 0;
		}
		/* Largest nonzero largest element. */
		scale[i+1]=1./max;    /* Save the scaling. */
	}
	for (j=1;j<=n;j++) {  /* This is the loop over columns of Crout's method */
		for (i=1;i<j;i++) {  /* Sum form of a triangular matrix except for i=j */
			sum=a[(i-1)*n-1+j];     
			for (k=1;k<i;k++) sum -= a[(i-1)*n-1+k]*a[(k-1)*n-1+j];
			a[(i-1)*n-1+j]=sum;
		}
		max=0.;  /* Initialize for the search for largest pivot element. */
		imax = 0;        /* Set default value for imax */
		for (i=j;i<=n;i++) {  /* This is i=j of previous sum and i=j+1...N */
			sum=a[(i-1)*n-1+j];      /* of the rest of the sum */
			for (k=1;k<j;k++)
				sum -= a[(i-1)*n-1+k]*a[(k-1)*n-1+j];
			a[(i-1)*n-1+j]=sum;
			if ( (tmp=scale[i]*fabs(sum)) >= max) {
				/* Is the figure of merit for the pivot better than the best so far? */
				max=tmp;
				imax=i;
			}
		}
		if (j != imax) {          /* Do we need to interchange rows? */
			for (k=1;k<=n;k++) {  /* Yes, do so... */
				tmp=a[(imax-1)*n-1+k];
				a[(imax-1)*n-1+k]=a[(j-1)*n-1+k];
				a[(j-1)*n-1+k]=tmp;
			}
			*d = -(*d);           /* ...and change the parity of d */
			scale[imax]=scale[j];       /* Also interchange the scale factor */
		}
		rp[j]=imax;
		if (a[(j-1)*n-1+j] == 0.) a[(j-1)*n-1+j]=TOL;
		/* If the pivot element is zero the matrix is singular (at least */
		/* to the precision of the algorithm). For some applications on */
		/* singular matrices, it is desiderable to substitute TOL for zero */
		if (j != n) {           /* Now, finally divide by pivot element */
			tmp=1./(a[(j-1)*n-1+j]);
			for ( i=j+1 ; i<=n ; i++ ) a[(i-1)*n-1+j] *= tmp;
		}
	}   /* Go back for the next column in the reduction. */

	return 1;
}

void mtxBackSubLU ( double* a, int n, int *rp, double* b )
{
	int i, ii=0, ip, j;
	double sum;
	b--;
	rp--;

	for (i=1;i<=n;i++) {  
		ip=rp[i];     
		sum=b[ip];  
		b[ip]=b[i]; 
		if (ii)  
			for ( j=ii ; j<=i-1 ; j++ ) sum -= a[(i-1)*n-1+j]*b[j];
		else if (sum)      
			ii = i; 
		b[i]=sum;
	}
	for (i=n-1;i>=0;i--) {   /* Backsubstitution */
		sum=b[i+1];
		for (j=i+1;j<n;j++) sum -= a[i*n+j]*b[j+1];
		b[i+1]=sum/a[i*n+i];  
		if (fabs(b[i])<1.e-8) b[i] = 0.;   
	}
}

double mtxDetLU( double* a, double d, int n)
{
	double det=d;
	int i;
	for (i=0;i<n;i++)
		det*=a[i*n+i];
	return det;
}

/*
* Finds a non trivial solution for the system Ax=0
* A mxn, m>n and rank(A)< n (better if rank(A)= n-1).
*
* The return value indicates the error. He is the ratio from the smallest  
* to the largest singular value of A. If rank(A) < n
* this value should be zero (~=TOL for the implementation).
*/ 
double mtxSVDAx0(double* a, int m, int n, double* x, double* u, double* d, double* v, double* tmp)
{
	double wmax,wmin,wmin2;
	int i,j,jmin;
    jmin=0;
	/* perform decomposition */
	mtxSVD(a,m,n,u,d,v,tmp);

	/* test if A has a non-trivial solution */
	wmax=d[0]; wmin = d[0];
	for (j=1;j<n;j++){
		if (d[j] < wmin) { wmin=d[j]; jmin =j; }
		if (d[j] > wmax) wmax=d[j];
	}

	/* test for the second smallest singular value */
	wmin2=wmax;
	for (j=0;j<n;j++){
		if (j==jmin) continue;
		if (d[j] < wmin2) wmin2=d[j];
	}

	/* copy the column of V correspondent to the smallest singular value of A */
	for (i=0;i<n;i++)
		x[i] = v[i*n+jmin];

	return (wmin/wmax);
}



#define TINY 1.0e-20

void mtxSVDbacksub(double* u, double* d, double* v, int m, int n, double* b, double* x, double* tmp)
{
	int j,i;
	double s;

	for (j=0;j<n;j++) {
		s=0.0;
		if (d[j]<TINY) {  
			for (i=0;i<m;i++) s += u[i*n+j]*b[i]; /* computes [U]T{b} */
			s /= d[j];   /* multiply by [d]-1 */
		}
		tmp[j]=s;
	}
	for (i=0;i<n;i++) {
		s=0.0;
		for (j=0;j<n;j++) s += v[i*n+j]*tmp[j];  /* computes [V]{tmp} */
		x[i]=s;
	}
}

/*
* Finds a solution for the system Ax=b
* A mxn, m>n.
* 
*/ 
void mtxSVDAxb(double* a, int m, int n, double* x, double* b, double* u, double* d, double* v, double* tmp)
{
	double wmax,wmin;
	int j;

	/* perform decomposition */
	mtxSVD(a,m,n,u,d,v,tmp);

	/* find the larger single value */
	wmax=d[0];
	for (j=1;j<n;j++)
		if (d[j] > wmax) wmax = d[j];

	/* cutoff small values */
	wmin=1.e-6*wmax;
	for (j=0;j<n;j++)
		if (d[j]<wmin) d[j]=0;

	/* backsubstitution */
	mtxSVDbacksub(u,d,v,m,n,b,x,tmp);
}


/*
* Computes the square matrix nxn [X] such that [A][X]=[I],
* then [X] is the inverse of [A]nxn.  
*(uses LU decomposition).
*/ 
void mtxAxInxn(double *a, int n, double *x)
{
	int i,j;
	int *rp = 0;
	double d, *lu = 0, *scale = 0,*col = 0;

	lu = (double*) malloc(sizeof(double)*(n*n));
	rp = (int*) malloc(sizeof(int)*n);
	scale = (double*) malloc(sizeof(double)*n);
	col = (double*) malloc(sizeof(double)*n);

	/* Copy the original matrix inside lu */
	for (i=0;i<n*n;i++)	lu[i]=a[i];

	/* LU decomposition */	
	mtxDecompLU ( lu, n, rp, &d, scale );

	for(j=0;j<n;j++) { //Find inverse by columns.
		for(i=0;i<n;i++) 
			col[i]=0.0;
		col[j]=1.0;
		mtxBackSubLU ( lu, n, rp, col );
		for(i=0;i<n;i++) 
			x[i*n + j]=col[i];
	}

	free(lu);	free(rp);	free(scale); free(col);	
	return;
}

/*
* Computes the nxm matrix [X] such that [X][A]=[I],
* then [X] is the inverse of [A]mxn.
* (uses LU decomposition).  
*/
void mtxAxImxn(double *A, int m, int n, double *X)
{
	double *At, *AtA, *invAtA, *invAtA_At;

	At = (double*) malloc(sizeof(double)*n*m);
	AtA = (double*) malloc(sizeof(double)*n*n);
	invAtA = (double*) malloc(sizeof(double)*n*n);
	invAtA_At = (double*) malloc(sizeof(double)*n*m);

	mtxAt(A, m, n, At);
	mtxAB(At,A,n,m,n,AtA);
	mtxAxInxn(AtA,n,invAtA);
	mtxAB(invAtA,At,n,n,m,X);

	return;
}
/*
* Get a solution x for the system Ax=b
*										[A]x =				b
*								[At][A]x =	[At]b
*	 inv([At][A])	[At][A]x =	inv([At][A])[At]b	
*										[I]x =  inv([At][A])[At]b	
*/ 
void mtxAxb(double *A, int m, int n, double *b, double *x)
{
	double *At, *AtA, *invAtA, *invAtA_At;

	At = (double*) malloc(sizeof(double)*n*m);
	AtA = (double*) malloc(sizeof(double)*m*m);
	invAtA = (double*) malloc(sizeof(double)*n*n);
	invAtA_At = (double*) malloc(sizeof(double)*n*m);

	mtxAt(A, m, n, At);
	mtxAB(At,A,n,m,n,AtA);
	mtxAxInxn(AtA,n, invAtA);
	mtxAB(invAtA,At,n,n,m,invAtA_At);
	mtxAb(invAtA_At,b,n,m,x);

	return;
}