matrix.c

基于主成份分析（PCA）的人脸特征识别核心源程序。

/***
 **     libface - Library of face recognition and supporting algorithms
        Copyright (c) 2003 Stefan Farthofer

	This file is part of libface, which is

        free software; you can redistribute it and/or modify
        it under the terms of the GNU General Public License as published by
        the Free Software Foundation; either version 2 of the License, or
        (at your option) any later version.

        This program is distributed in the hope that it will be useful,
        but WITHOUT ANY WARRANTY; without even the implied warranty of
        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
        GNU General Public License for more details.

        You should have received a copy of the GNU General Public License
        along with this program; if not, write to the Free Software
        Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

	For further information seek us at http://sourceforge.net/projects/openbio/
**	or write an email to [e-mail] or [e-mail].
***/

/* we need error codes so include frbase.h */
#include <stdlib.h>
#include <malloc.h>
#include <math.h>
#include "frbase.h"
#include "matrix.h"

void _matTransposeD(float* dst, float* src, unsigned int rowsSrc, unsigned int colsSrc) {
	unsigned int i,j;

	for (i=0; i < rowsSrc; i++)
		for (j=0; j < colsSrc; j++)
			dst[j*rowsSrc+i] = src[i*colsSrc+j];
}

void _matTransposeQ(float* mat, unsigned int n) {
	unsigned int i,j;
	float tmp;

	for(i=0; i < n; i++) { 
		for(j=i+1; j < n; j++) {
			tmp = mat[i*n+j];
			mat[i*n+j] = mat[j*n+i];
			mat[j*n+i] = tmp;
		}
	}
}

/* computes m x n * n x p matrix multiply */
void _matMultiplyD(float* dst, float* a, float* b, unsigned int m, unsigned int n, unsigned int p) {
	unsigned int i, j, k;

	for (i=0; i < m; i++) {
		for (j=0; j < p; j++) {
			dst[i*p+j] = 0;
			for (k=0; k < n; k++) dst[i*p+j] += a[i*n+k] * b[k*p+j];
		}
	}
}

/* subtract same-sized matrizes */ 
void _matSubtractD(float* dst, float* minuend, float* subtrahend, unsigned int m, unsigned int n) {
	for(n *= m, m = 0; m < n; m++)
		dst[m] = minuend[m] - subtrahend[m];
}

/* add same-sized matrizes */ 
void _matAdd(float* dst, float* b, unsigned int m, unsigned int n) {
	for(n *= m, m = 0; m < n; m++)
		dst[m] = dst[m] + b[m];
}


/* computes eigenvalues and vectors of a symmetric matrix 
 * columns of matVectors contain the eigenvectors
 *
 * a - matrix
 * n - dimension of matrix
 * d - array for eigenvvalues
 * v - array for eigenvectors (each vector occupies a column)
 */
int _matEigenSD(float* a, int n, float* d, float* v) {
  int j,iq,ip,ip_times_n,i ;
  float tresh,theta,tau,t,sm,s,h,g,c,*b,*z;
  int nrotint, *nrot = &nrotint;
  
  b = (float *)malloc(n*sizeof(float));
  if (b == NULL) return FR_ERR_NOMEM;
  z = (float *)malloc(n*sizeof(float));
  if (z == NULL) { free(b); return FR_ERR_NOMEM; }

  
  for(ip_times_n=0, ip=0; ip<n; ++ip, ip_times_n+=n)  
      {
      
      /* Initialize the identity matrix */
      for(iq=0; iq<n; ++iq)v[ip_times_n + iq] = 0.0 ;
      v[ip_times_n + ip] = 1.0 ;
      
      /* Initailize b and d to be diagonal of a */
      b[ip] = d[ip] = a[ip_times_n + ip];
      z[ip] = 0.0 ;
      }
      
  *nrot = 0 ;
  for(i=0;i<50;++i)
      {
      /* Sum off-diagonal elements */
      sm=0.0 ;

      for(ip_times_n=0,ip=0;ip<n-1;ip++,ip_times_n+=n)
	for(iq=ip+1;iq<n;iq++)
	  sm += (float)fabs(a[ip_times_n + iq]);
	  
      /*  If we have converged,  free the working vectors and return. */
      if(sm == 0.0)
	  {
	  free(b);
	  free(z);
	  return FR_OK;
	  }
      
      /* tresh is different on first three iterations...*/
      tresh=(i<3) ? 0.2f*sm/(n*n) : 0.0f ;
      
      for(ip_times_n=0,ip=0;ip<n-1;ip++,ip_times_n+=n)
	  {
	  for(iq=ip+1;iq<n;++iq)
	      {
	      g=100.0f*(float)fabs(a[ip_times_n + iq]);

	      /* After four sweeps, skip the rotation if the off-diagonal element is small */
	      /* This test is taken directly from the text and looks a little suspect to me... */

	      if(i > 3 && g < EPS)
		a[ip_times_n + iq] = 0.0 ;

	      else if(fabs(a[ip_times_n+iq]) > tresh) 
		  {
		  h=d[iq]-d[ip];
		  if(g < EPS)
		    t = (fabs(a[ip_times_n+iq]) > EPS) ? (a[ip_times_n+iq])/h : 0.0f ; 
		  else
		      { 
		      theta=(fabs(h) < EPS) ? 0.0f : 0.5f*h/(a[ip_times_n+iq]);
		      t=1.0f/((float)fabs(theta)+(float)sqrt(1.0f+theta*theta));
		      if(theta < 0.0f)
			t = -t ; 
		      } 
		  c=1.0f/(float)sqrt(1.0f+t*t);
		  s=t*c;
		  tau=s/(1.0f+c);
		  
		  h=t*a[ip_times_n+iq];
		  z[ip] -= h;
		  z[iq] += h;
		  d[ip] -= h;
		  d[iq] += h;
		  a[ip_times_n+iq]=0.0;
		  
		  for(j=0;j<ip;j++)
		      {
		      ROTATE(a,j,ip,j,iq,n);
		      }
		  for(j=ip+1;j<iq;j++)
		      {
		      ROTATE(a,ip,j,j,iq,n);
		      }
		  for(j=iq+1;j<n;j++)
		      {
		      ROTATE(a,ip,j,iq,j,n);
		      }
		  for(j=0;j<n;j++)
		      {
		      ROTATE(v,j,ip,j,iq,n);
		      }
		  ++(*nrot);
		  }
	      }
	  }
      for(ip=0;ip<n;++ip)
	  {
	  b[ip] += z[ip];
	  d[ip]=b[ip];
	  z[ip]=0.0;
	  }
      }

  /* Failed to converge!! What to do ??? */
  /* Well, let's at least free up memory and return without a murmur */

  free(b);
  free(z);
  return FR_ERR_OTHER;
}