matrix.c

这是著名的Tsai摄像机内外参数标定的源码

}

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;
		
   /* 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);
}


void mtxLUdcmp(double **a,int n,int *indx,double *d)
{
	int i,imax,j,k;
	double big,dum,sum,temp;
	double *vv;
	vv = (double*) malloc(sizeof(double)*(n+1)); 
	
	*d=1.0;
	for (i=1;i<=n;i++) {
		big=0.0;
		for (j=1;j<=n;j++)
			if ((temp=(double)fabs(a[i][j])) > big) big=temp;
		if (big == 0.0) printf("\nSingular matrix in routine mtxLUDCMP");
		vv[i]=(double)1.0/big;
	}
	for (j=1;j<=n;j++) {
		for (i=1;i<j;i++) {
			sum=a[i][j];
			for (k=1;k<i;k++) sum -= a[i][k]*a[k][j];
			a[i][j]=sum;
		}
		big=0.0;
		for (i=j;i<=n;i++) {
			sum=a[i][j];
			for (k=1;k<j;k++)
				sum -= a[i][k]*a[k][j];
			a[i][j]=sum;
			if ( (dum=(double)(vv[i]*fabs(sum))) >= big) {
				big=dum;
				imax=i;
			}
		}
		if (j != imax) {
			for (k=1;k<=n;k++) {
				dum=a[imax][k];
				a[imax][k]=a[j][k];
				a[j][k]=dum;
			}
			*d = -(*d);
			vv[imax]=vv[j];
		}
		indx[j]=imax;
		if (a[j][j] == 0.0f) a[j][j]=TINY;
		if (j != n) {
			dum=(double)1.0/(a[j][j]);
			for (i=j+1;i<=n;i++) a[i][j] *= dum;
		}
	}
	free(vv);
}

void mtxLUbksb(double **a,int n,int *indx,double *b)
{
	int i,ii=0,ip,j;
	double sum;

	for (i=1;i<=n;i++) {
		ip=indx[i];
		sum=b[ip];
		b[ip]=b[i];
		if (ii)
			for (j=ii;j<=i-1;j++) sum -= a[i][j]*b[j];
		else if (sum) ii=i;
		b[i]=sum;
	}
	for (i=n;i>=1;i--) {
		sum=b[i];
		for (j=i+1;j<=n;j++) sum -= a[i][j]*b[j];
		b[i]=sum/a[i][i];
	}
}

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

	lu = (double**) malloc(sizeof(double*)*(n+1));
	y = (double**) malloc(sizeof(double*)*(n+1));
	col = (double*) malloc(sizeof(double)*(n+1));
	indx = (int*) malloc(sizeof(int)*(n+1));

	for(i=1;i<=n;i++){
		lu[i] = (double*) malloc(sizeof(double)*(n+1));
		 y[i] = (double*) malloc(sizeof(double)*(n+1));
	}

	/* Copy the original matrix inside lu */
	for (i=1;i<=n;i++){
		for (j=1;j<=n;j++) {
			lu[i][j]=a[(i-1)*n + (j-1)];
		}
	}

	/* LU decomposition */
	mtxLUdcmp(lu,n,indx,&d);

	for(j=1;j<=n;j++) { //Find inverse by columns.
		for(i=1;i<=n;i++) 
			col[i]=0.0;
		col[j]=1.0;
		mtxLUbksb(lu,n,indx,col);
		for(i=1;i<=n;i++) 
			y[i][j]=col[i];
	}

	for (i=1;i<=n;i++)
		for (j=1;j<=n;j++) 
			x[(i-1)*n + (j-1)]= y[i][j];

	
	for(i=1;i<=n;i++){
		free(lu[i]); free(y[i]); 
	}
	free(lu);	free(indx);	free(y);	free(col);
	
	return;
}

/*
* Computes the mxn matrix [X] such that [A][X]=[I]
* , then [X] is the inverse of [A].  
*/
void mtxAxI_mxn(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);
	mtxAXI_nxn(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 mtxAx_b(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);
	mtxAXI_nxn(AtA,n,invAtA);
	mtxAB(invAtA,At,n,n,m,invAtA_At);
	mtxAb(invAtA_At,b,n,m,x);

	return;
}