eigen.cpp

face recognition test source code

#include "stdafx.h"

#include "eigen.h"
#include "utilcpp.h"
#include "str.h"
#include "math.h"

#define SIGN(a, b) ((b) < 0 ? - fabs(a) : fabs(a))

inline float pythag(float a, float b)
	{
	return sqrt(a*a + b*b);
	}

/*****************************************************************************************************
* Householder reduction of the real symmetric matrix a which is a n*n CMatrix.
* On output, a is replaced by the orthogonal matrix Q effecting the transformation. d which is a vector
* of n columns returns the diagonal elements of the tridiagonal matrix, and e which is a vector
* of n columns the off-diagonal elements, with e[1] = 0.
* IF YOU WANT MORE INFO ABOUT THIS READ "Numerical Recipes, The Art of Scientific Computing" second ed.
* W.H. Press, S. A. Teukolsky, W. T. Vetterling, B. P. Flannery 
*****************************************************************************************************/
void tred2(CMatrix *a, long n, CVect *d, CVect *e)
	{
	long l, k, j, i;
	valtype scale, hh, h, g, f;
	
	for (i = n; i >= 2; i--)
		{
		l = i-1;
		h = scale = (valtype)0;
		if (l > 1)
			{
			for (k = 1; k <= l; k++)
				scale += fabs(a->GetAt(i, k));
			if (scale == (valtype)0)
				e->SetAt(i, a->GetAt(i, l));
			else
				{
				for (k = 1; k <= l; k++)
					{
					a->SetAt(i, k, a->GetAt(i, k) / scale);
					h += a->GetAt(i, k) * a->GetAt(i, k);
					}
				f = a->GetAt(i, l);
				g = (f > (valtype)0 ? -sqrt(h) : sqrt(h));
				e->SetAt(i, scale*g);
				h -= f*g;
				a->SetAt(i, l, f-g);
				f = (valtype)0;
				for (j = 1; j <= l; j++)
					{
					a->SetAt(j, i, a->GetAt(i, j) / h);
					g = (valtype)0;
					for (k = 1; k <= j; k++)
						g += a->GetAt(j, k) * a->GetAt(i, k);
					for (k = j+1; k <= l; k++)
						g += a->GetAt(k, j) * a->GetAt(i, k);
					e->SetAt(j, g/h);
					f += e->GetAt(j) * a->GetAt(i, j);
					}
				hh = f / (h+h);
				for (j = 1; j <= l; j++)
					{
					f = a->GetAt(i, j);
					g = e->GetAt(j) - hh * f;
					e->SetAt(j, g);
					for (k = 1; k <= j; k++)
						a->SetAt(j, k, a->GetAt(j, k) - (f * e->GetAt(k) + g * a->GetAt(i, k)));
					}
				}
			}
		else	
			{											/* l <= 1 */
			e->SetAt(i, a->GetAt(i, l));
			}
		d->SetAt(i, h);
		}
	
	d->SetAt(1, (valtype)0); 
	e->SetAt(1, (valtype)0); 
	
	for (i = 1; i <= n; i++)
		{
		l = i-1;
		if (d->GetAt(i))
			{
			for (j = 1; j <= l; j++)
				{
				g = (valtype)0;
				for (k = 1; k <= l; k++)
					g += a->GetAt(i, k) * a->GetAt(k, j);
				for (k = 1; k <= l; k++)
					a->SetAt(k, j, a->GetAt(k, j) - (g * a->GetAt(k, i)));
				}
			}
		d->SetAt(i, a->GetAt(i, i));
		a->SetAt(i, i, (valtype)1);
		for (j = 1; j <= l; j++)
			{
			a->SetAt(j, i, (valtype)0);
			a->SetAt(i, j, (valtype)0);
			}
		}
	}
	
/*****************************************************************************************************
* QL algorithm with implicit shifts, to determine the eigenvalues and eigenvectors of a real,
* symmetric, tridiagonal matrix, or of a real, symmetric matrix previously reduced by tred2.
*
* On input, d (dim. n) contains the diagonal elements of the tridiagonal matrix. On output, it returns 
* the eigenvalues. 
*
* The vector e (dim. n) inputs the subdiagonal elements of the tridiagonal matrix, with e[1] arbitrary. 
* On output e is destroyed.
*
* If the matrix is tridiagonal, the matrix z (n*n) is input as the identity matrix. If the matrix has 
* been reduced by tred2, then z is input as the matrix output by tred2. In either case, the kth column 
* of z returns the normalized eigenvector corresponding to d[k].
* 
* IF YOU WANT MORE INFO ABOUT THIS READ "Numerical Recipes, The Art of Scientific Computing" second ed.
* W.H. Press, S. A. Teukolsky, W. T. Vetterling, B. P. Flannery 
*****************************************************************************************************/
void tqli(CVect *d, CVect *e, long n, CMatrix *z)
	{
	long m, l, iter, i, k;
	valtype s, p, g, f, dd, c, b;
	float r;
	
	for (i = 2; i <= n; i++)
		e->SetAt(i-1, e->GetAt(i));
	e->SetAt(n, (valtype)0);
	
	for (l = 1; l <= n; l++)
		{
		iter = 0;
		do
			{
			for (m = l; m <= n-1; m++)
				{
				dd = fabs(d->GetAt(m)) + fabs(d->GetAt(m+1));
				if ((valtype)(fabs(e->GetAt(m))+dd) == dd)
					break;
				}
			if (m != l)
				{
				if (iter++ == 30)
					{
					Msg(0, str_TOOITER);
					break;
					}
					
				g = (d->GetAt(l+1) - d->GetAt(l)) / ((valtype)2 * e->GetAt(l));
				r = pythag(g, 1.0);
				g = d->GetAt(m) - d->GetAt(l) + e->GetAt(l) / (g + SIGN(r, g));
				s = c = (valtype)1;
				p = (valtype)0;
				for (i = m-1; i >= l; i--)
					{
					f = s * e->GetAt(i);
					b = c * e->GetAt(i);
					r = pythag(f, g);
					e->SetAt(i+1, r);
					if (r == 0.0)
						{
						d->SetAt(i+1, d->GetAt(i+1) - p);
						e->SetAt(m, (valtype)0);
						break;
						}
					s = f/r;
					c = g/r;
					g = d->GetAt(i+1) - p;
					r = (d->GetAt(i) - g) * s + (valtype)2 * c * b;
					p = s*r;
					d->SetAt(i+1, g + p);
					g = c * r - b;
					for (k = 1; k <= n; k++)
						{										/* Form eigenvectors */
						f = z->GetAt(k, i+1);
						z->SetAt(k, i+1, s*z->GetAt(k, i) + c*f);
						z->SetAt(k, i, c*z->GetAt(k, i) - s*f);
						}
					}
					if (r == (valtype)0 && i)
						continue;
					d->SetAt(l, d->GetAt(l) - p);
					e->SetAt(l, g);
					e->SetAt(m, (valtype)0);
				}
			}
		while (m != l);
		}
	}