matrixnew.cpp

在人脸检测的基础之上,对嘴部的运动表情进行分析,进行语音模拟.

	Matrix  temp;
	
	temp=*this;
	n=row;
	index= (int *) new int[n];
	Vector col(n);
	temp.ludcmp(index,&d);
	for( j=0;j<n;j++)
	{ 
        for( i=0;i<n;i++) col[i]=0.0;
        col[j]=1.0;
        temp.lubksb(index,col);
        for( i=0;i<n;i++) matrix[i][j]=col[i];
	}
	delete[] index;
	
	return *this;  
}

void Matrix::linearSysSol(Vector& b)
{
	// matrix must not be empty
	ASSERT( matrix);
	
	// matrix must be square
	ASSERT( column == row);
	
	// vector must be compatible with the matrix
	ASSERT( b.getN() == row);
	
	int *index,n;
	double d;
	n=row;
	index= (int *) new int[n];
	ludcmp(index,&d);
	lubksb(index,b);
	delete[] index; 
	
}  // b is not same anymore

Matrix& Matrix::transpose()
{
	// if matrix is empty, so is its transpose
	if( matrix) 
	{ 
		Matrix MT( column, row);
		for( int i = 0; i < row; i++)
		{
			for( int j = 0; j < column; j++)
			{
				MT.matrix[j][i] = matrix[i][j];
			}
		}
		(*this) = MT;
	}
	
	return *this;
}

void Matrix::leastSquares(Vector& b)   // b is nx1, matrix is nxm
{
    // matrix must not be empty
	ASSERT( matrix);
	
	// vector must be compatible with the matrix
	ASSERT( b.getN() == row);
	
    Matrix mt;
    Vector bt(column);
    
    mt=(*this);
    mt.transpose();         // mt is mxn
    mt*=(*this) ;            // mt is mxm
    for (int i=0; i<column; i++)
    {
		bt[i]=0.0;
		for( int j=0; j<row; j++)
		{
			bt[i]+=matrix[j][i]*b[j];  //multiply with the transpose;
		} 
	}
    b=bt;
    mt.linearSysSol(b);
	
}   // b is not same any more.

//=======================================================================
// SVD from Numerical Recipes
//
// This routine computes the singular value decomposition 
// A =  U W V~ of the caller matrix A. The diagonal matrix W
// of singular values is output as vector w, 
// and the square matrix V (not the transpose V~) is output as matrix v.
//=======================================================================
Matrix& Matrix::svdcmp( Vector& diagonal, Matrix& square)
{
	diagonal.setN( column);
	square.setSize( column, column);

	// initialization of the parameters
	// used by the original KRC routine
	// Note that KRC uses arrays with index
	// starting from 1 instead of 0.
	int m = row; // number of rows
	int n = column; // number of columns

	// allocate memory for 1-indexed arrays
	double** a = new double*[ m + 1];
	for( int M = 0; M <= m; M++)
	{
		a[M] = new double[ n + 1];
	}

	double** v = new double*[ n + 1];
	for( int N = 0; N <= n; N++)
	{
		v[N] = new double[ n + 1];
	}

	double* w = new double[ n + 1];

	// copy the entries of the input matrix
	// into the 1-indexed array
	for( M = 1; M <= m; M++)
	{
		for( N = 1; N <= n; N++)
		{
			a[M][N] = matrix[M-1][N-1];
		}
	}
	
	// ORIGINAL ROUTINE STARTS HERE
	int flag,i,its,j,jj,k,l,nm;
	double anorm,c,f,g,h,s,scale,x,y,z,*rv1;

	//rv1=dvector(1,n);
	rv1 = new double[ n + 1];
	g=scale=anorm=0.0;
	for (i=1;i<=n;i++) {
		l=i+1;
		rv1[i]=scale*g;
		g=s=scale=0.0;
		if (i <= m) {
			for (k=i;k<=m;k++) scale += fabs(a[k][i]);
			if (scale) {
				for (k=i;k<=m;k++) {
					a[k][i] /= scale;
					s += a[k][i]*a[k][i];
				}
				f=a[i][i];
				g = -fMath.Sign(sqrt(s),f);
				h=f*g-s;
				a[i][i]=f-g;
				for (j=l;j<=n;j++) {
					for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
					f=s/h;
					for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
				}
				for (k=i;k<=m;k++) a[k][i] *= scale;
			}
		}
		w[i]=scale *g;
		g=s=scale=0.0;
		if (i <= m && i != n) {
			for (k=l;k<=n;k++) scale += fabs(a[i][k]);
			if (scale) {
				for (k=l;k<=n;k++) {
					a[i][k] /= scale;
					s += a[i][k]*a[i][k];
				}
				f=a[i][l];
				g = -fMath.Sign(sqrt(s),f);
				h=f*g-s;
				a[i][l]=f-g;
				for (k=l;k<=n;k++) rv1[k]=a[i][k]/h;
				for (j=l;j<=m;j++) {
					for (s=0.0,k=l;k<=n;k++) s += a[j][k]*a[i][k];
					for (k=l;k<=n;k++) a[j][k] += s*rv1[k];
				}
				for (k=l;k<=n;k++) a[i][k] *= scale;
			}
		}
		anorm=fMath.Max(anorm,(fabs(w[i])+fabs(rv1[i])));
	}
	for (i=n;i>=1;i--) {
		if (i < n) {
			if (g) {
				for (j=l;j<=n;j++) v[j][i]=(a[i][j]/a[i][l])/g;
				for (j=l;j<=n;j++) {
					for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j];
					for (k=l;k<=n;k++) v[k][j] += s*v[k][i];
				}
			}
			for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;
		}
		v[i][i]=1.0;
		g=rv1[i];
		l=i;
	}
	for (i=fMath.Min(m,n);i>=1;i--) {
		l=i+1;
		g=w[i];
		for (j=l;j<=n;j++) a[i][j]=0.0;
		if (g) {
			g=1.0/g;
			for (j=l;j<=n;j++) {
				for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j];
				f=(s/a[i][i])*g;
				for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
			}
			for (j=i;j<=m;j++) a[j][i] *= g;
		} else for (j=i;j<=m;j++) a[j][i]=0.0;
		++a[i][i];
	}
	for (k=n;k>=1;k--) {
		for (its=1;its<=30;its++) {
			flag=1;
			for (l=k;l>=1;l--) {
				nm=l-1;
				if ((double)(fabs(rv1[l])+anorm) == anorm) {
					flag=0;
					break;
				}
				if ((double)(fabs(w[nm])+anorm) == anorm) break;
			}
			if (flag) {
				c=0.0;
				s=1.0;
				for (i=l;i<=k;i++) {
					f=s*rv1[i];
					rv1[i]=c*rv1[i];
					if ((double)(fabs(f)+anorm) == anorm) break;
					g=w[i];
					h=fMath.dpythag(f,g);
					w[i]=h;
					h=1.0/h;
					c=g*h;
					s = -f*h;
					for (j=1;j<=m;j++) {
						y=a[j][nm];
						z=a[j][i];
						a[j][nm]=y*c+z*s;
						a[j][i]=z*c-y*s;
					}
				}
			}
			z=w[k];
			if (l == k) {
				if (z < 0.0) {
					w[k] = -z;
					for (j=1;j<=n;j++) v[j][k] = -v[j][k];
				}
				break;
			}
			//nrerror("no convergence in 30 dsvdcmp iterations");
			if(its == 30)
			{
				*this = 0.0;
				square = 0.0;
				diagonal.empty(); // to signal error
				return *this;
			}
			x=w[l];
			nm=k-1;
			y=w[nm];
			g=rv1[nm];
			h=rv1[k];
			f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
			g=fMath.dpythag(f,1.0);
			f=((x-z)*(x+z)+h*((y/(f+fMath.Sign(g,f)))-h))/x;
			c=s=1.0;
			for (j=l;j<=nm;j++) {
				i=j+1;
				g=rv1[i];
				y=w[i];
				h=s*g;
				g=c*g;
				z=fMath.dpythag(f,h);
				rv1[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=1;jj<=n;jj++) {
					x=v[jj][j];
					z=v[jj][i];
					v[jj][j]=x*c+z*s;
					v[jj][i]=z*c-x*s;
				}
				z=fMath.dpythag(f,h);
				w[j]=z;
				if (z) {
					z=1.0/z;
					c=f*z;
					s=h*z;
				}
				f=c*g+s*y;
				x=c*y-s*g;
				for (jj=1;jj<=m;jj++) {
					y=a[jj][j];
					z=a[jj][i];
					a[jj][j]=y*c+z*s;
					a[jj][i]=z*c-y*s;
				}
			}
			rv1[l]=0.0;
			rv1[k]=f;
			w[k]=x;
		}
	}
	//free_dvector(rv1,1,n);
	delete[] rv1;

	//ORIGINAL KRC ROUTINE ENDS HERE

	// copy back from the 1-indexed arrays
	// and deallocate memory
	for( M = 1; M <= m; M++)
	{
		for( N = 1; N <= n; N++)
		{
			matrix[M-1][N-1] = a[M][N];
		}
		delete[] a[M];
	}
	delete[] a[0];
	delete[] a;

	for( int NN = 1; NN <= n; NN++)
	{
		diagonal.column[NN-1] = w[NN];

		for( N = 1; N <= n; N++)
		{
			square.matrix[NN-1][N-1] = v[NN][N];
		}
		delete[] v[NN];
	}
	delete[] v[0];
	delete[] v;

	delete[] w;

	return *this;
}

// multiply matA with matB
void Matrix::setAxB( Matrix& matA, Matrix& matB)
{
	// Matrices must be compatible for multiplication
	ASSERT( matA.column == matB.row);

	// sets row & column
	setSize( matA.row, matB.column);
	
	for( int i = 0; i < row; i++)
	{
		for( int j = 0; j < column; j++)
		{
			matrix[i][j] = 0.0;

			for( int k = 0; k < matA.column; k++)
			{
				matrix[i][j] += matA.matrix[i][k] * matB.matrix[k][j];
			}
		}
	}
}

// multiply matA with the transpose of matB
void Matrix::setAxBtrans( Matrix& matA, Matrix& matB)
{
	// Matrices must be compatible for multiplication
	ASSERT( matA.column == matB.column);

	// sets row & column
	setSize( matA.row, matB.row);
	
	for( int i = 0; i < row; i++)
	{
		for( int j = 0; j < column; j++)
		{
			matrix[i][j] = 0.0;

			for( int k = 0; k < matA.column; k++)
			{
				matrix[i][j] += matA.matrix[i][k] * matB.matrix[j][k];
			}
		}
	}
}

// multiply transpose of matA with matB
void Matrix::setAtransxB( Matrix& matA, Matrix& matB)
{
	// Matrices must be compatible for multiplication
	ASSERT( matA.row == matB.row);

	// sets row & column
	setSize( matA.column, matB.column);
	
	for( int i = 0; i < row; i++)
	{
		for( int j = 0; j < column; j++)
		{
			matrix[i][j] = 0.0;

			for( int k = 0; k < matA.row; k++)
			{
				matrix[i][j] += matA.matrix[k][i] * matB.matrix[k][j];
			}
		}
	}
}

// finds the LS solution via SVD
// solve the equation "Ax = b" for x in the least squares sense
Vector Matrix::leastSquaresSVD( Vector& vecB)
{
	// matrix and vector must be compatible
	ASSERT( row == vecB.getN());

	// make a copy of the caller matrix
	Matrix matA( *this);

	// find the SVD-inverse of matrix A
	matA.svdInverse();

	// make a copy of the input vector
	Vector vecX( vecB);

	// find the LS solution
	vecX.rightMult( matA);

	return vecX;
}

// finds the LS solution via SVD
// solve the equation "AX = B" for X in the least squares sense
Matrix Matrix::leastSquaresSVD( Matrix& matB)
{
	// matrix and vector must be compatible
	ASSERT( row == matB.getRow());

	// make a copy of the caller matrix
	Matrix matA( *this);

	// find the SVD-inverse of matrix A
	matA.svdInverse();

	// find the LS solution
	Matrix matX;
	matX.setAxB( matA, matB);

	return matX;
}

// finds the SVD-inverse of matrix
void Matrix::svdInverse()
{
	ASSERT( row >= column);
	ASSERT( column > 0);

	// make a copy of the caller matrix
	Matrix matA( *this);

	// do an SVD on matrix A
	// A = U D V(trans)
	// A(inv) = V D(inv) U(trans)
	Vector vecD;
	Matrix matV;
	matA.svdcmp( vecD, matV);

	vecD.diagInverse();

	matV.rightMultDiag( vecD);

	setAxBtrans( matV, matA);
}

///////////////////////////////////////////////////////////////////
CString Matrix::toText()
{
	CString textPart;
	CString text = "";
	for( int i = 0; i < getRow(); i++)
	{
		for( int j = 0; j < getColumn(); j++)
		{
			textPart.Format("%14.4f", matrix[ i][ j]);
			text += textPart;
		}
		text += "\n";
	}
	return text;
}