source: proiecte/PPPP/eigenface2/eigenface_p.c @ 139

#include <stdio.h>
#include <string.h>
#include <math.h>
#include "cv.h"
#include "cvaux.h"
#include "highgui.h"
#include <omp.h>

#define ROTATE(a,i,j,k,l) g=a[i*n + j];h=a[k*n + l];a[i*n + j]=g-s*(h+g*tau);\
a[k*n + l]=h+s*(g-h*tau);

/*
Computes all eigenvalues and eigenvectors of a real symmetric matrix a[1..n][1..n]. On
output, elements of a above the diagonal are destroyed. d[1..n] returns the eigenvalues of a.
v[1..n][1..n] is a matrix whose columns contain, on output, the normalized eigenvectors of
a. nrot returns the number of Jacobi rotations that were required.
*/
void jacobi(float *a, int n, float d[], float *v, int *nrot)
{
        int j,iq,ip,i;
        float tresh,theta,tau,t,sm,s,h,g,c,*b,*z;
        int tid, nthreads;
        
        b = (float *) malloc(n * sizeof(float));
        z = (float *) malloc(n * sizeof(float));
        
        //~ #pragma omp parallel private(nthreads, tid)
        //~ {

        //~ /* Obtain thread number */
        //~ tid = omp_get_thread_num();
        //~ printf("Hello World from thread = %d\n", tid);

        //~ /* Only master thread does this */
        //~ if (tid == 0) 
    //~ {
        //~ nthreads = omp_get_num_threads();
        //~ printf("Number of threads = %d\n", nthreads);
    //~ }}
        
        //#pragma omp parallel for
        for (ip=0;ip<n;ip++) {                                                                  // Initialize to the identity matrix.
                for (iq=0;iq<n;iq++) v[ip*n + iq]=0.0;
                        v[ip*n + ip]=1.0;
        }
        //#pragma omp parallel for
        for (ip=0;ip<n;ip++) {                                                          // Initialize b and d to the diagonal of a.
                b[ip]=d[ip]=a[ip*n + ip];                                                                                                               
                z[ip]=0.0;                                                                              // This vector will accumulate terms of the form t*a[pq] as in equation (11.1.14).
        }
                
        *nrot=0;
        for (i=1;i<=50;i++) {
                sm=0.0;
        //#pragma omp parallel for reduction(+:sm)
                for (ip=0;ip<n-1;ip++) {                                                        // Sum off-diagonal elements.
                        for (iq=ip+1;iq<n;iq++)
                        sm += fabs(a[ip*n + iq]);
                }
                if (sm == 0.0) {                                                                        // The normal return, which relies on quadratic convergence to machine underflow.
                        free(z);
                        free(b);
                        return;
                }
                if (i < 4)
                        tresh=0.2*sm/(n*n);                                                                                                             // ...on the first three sweeps.
                else
                        tresh=0.0;                                                                                                                      // ...thereafter.
                for (ip=0;ip<n-1;ip++) {
                        for (iq=ip+1;iq<n;iq++) {
                                g=100.0*fabs(a[ip*n + iq]);
                                if (i > 4 && (float)(fabs(d[ip])+g) == (float)fabs(d[ip])                                               // After four sweeps, skip the rotation if the off-diagonal element is small.
                                        && (float)(fabs(d[iq])+g) == (float)fabs(d[iq]))
                                        a[ip*n + iq]=0.0;
                                else if (fabs(a[ip*n + iq]) > tresh) {
                                        h=d[iq]-d[ip];
                                        if ((float)(fabs(h)+g) == (float)fabs(h))
                                                t=(a[ip*n + iq])/h;                                                                                     // t = 1/(2*theta)
                                        else {
                                                theta=0.5*h/(a[ip*n + iq]);                                                                     // Equation (11.1.10).
                                                t=1.0/(fabs(theta)+sqrt(1.0+theta*theta));
                                                if (theta < 0.0) t = -t;
                                        }
                                        
                                        c=1.0/sqrt(1+t*t);
                                        s=t*c;
                                        tau=s/(1.0+c);
                                        h=t*a[ip*n + iq];
                                        z[ip] -= h;
                                        z[iq] += h;
                                        d[ip] -= h;
                                        d[iq] += h;
                                        a[ip*n + iq]=0.0;
                                        
                                        #pragma omp parallel shared(a,ip,iq, s, tau) private(j)
                                        {
                                                  #pragma omp sections nowait
                                                {
                                                        
                                                        #pragma omp section
                                                        //#pragma omp parallel for
                                                        for (j=0;j<=ip-1;j++) {                                                                                         // Case of rotations 1 <= j < p.
                                                                ROTATE(a,j,ip,j,iq)
                                                        }
                                                        
                                                        #pragma omp section
                                                        for (j=ip+1;j<=iq-1;j++) {                                                                              // Case of rotations p < j < q.
                                                                ROTATE(a,ip,j,j,iq)
                                                        }
                                                        #pragma omp section
                                                        for (j=iq+1;j<n;j++) {                                                                                  // Case of rotations q < j <= n.
                                                                ROTATE(a,ip,j,iq,j)
                                                        }
                                                }
                                        }
                                        
                                        for (j=0;j<n;j++) {
                                                ROTATE(v,j,ip,j,iq)
                                        }
                                        ++(*nrot);
                                }
                        }
                }
                
                //#pragma omp parallel for
                for (ip=0;ip<n;ip++) {
                        b[ip] += z[ip];
                        d[ip]=b[ip];                                                                                                                    // Updte d with the sum of t*a[pq],
                        z[ip]=0.0;                                                                                                                      // and reinitialize z.
                }
        }
}
/* 
Given the eigenvalues d[1..n] and eigenvectors v[1..n][1..n] as output from jacobi
(x11.1) or tqli (x11.3), this routine sorts the eigenvalues into descending order, and rearranges
the columns of v correspondingly. The method is straight insertion. 
*/
void eigsrt(float d[], float *v, int n) 
{
        int k,j,i;
        float p;
        for (i=0;i<n-1;i++) {
                p=d[k=i];
                for (j=i+1;j<n;j++)
                        if (d[j] >= p) p=d[k=j];
                if (k != i) {
                        d[k]=d[i];
                        d[i]=p;
                        for (j=0;j<n;j++) {
                                p=v[j*n + i];
                                v[j*n + i]=v[j*n + k];
                                v[j*n + k]=p;
                        }
                }
        }
}

void calcMeanImage(int nFaces, uchar** faceArr, int faceStep, 
                                        CvSize size, float* avg, int avgStep) 
{
        int i,j,k;
        float m = 1.0f / (float) nFaces;
        float* bf = avg;
        
        for( i = 0; i < size.height; i++, bf += avgStep)
                //#pragma omp parallel for
                for( j = 0; j < size.width; j++ )
                        bf[j] = 0.f;
        
        //#pragma omp parallel for
        for( i = 0; i < nFaces; i++ )
        {
                uchar* bu = faceArr[i];
                bf  = avg;
                for( k = 0; k < size.height; k++, bf += avgStep, bu += faceStep )
                        for( j = 0; j < size.width; j++ )
                                bf[j] += bu[j];
        }
        
        bf = avg;
        for( i = 0; i < size.height; i++, bf += avgStep)
                //#pragma omp parallel for
                for( j = 0; j < size.width; j++ ) {
                        bf[j] *= m;
                }
}

void calcCovarMatrix(int nFaces, uchar** faceArr, int faceStep, CvSize size, 
                                float* avg, int avgStep, float *covarMatrix) 
{
        int i, j;
        
        for( i = 0; i < nFaces; i++ )
        {
                uchar *bu = faceArr[i];

                for( j = i; j < nFaces; j++ )
                {
                        int k, l;
                        float w = 0.f;
                        float *a = avg;
                        uchar *bu1 = bu;
                        uchar *bu2 = faceArr[j];

                        for( k = 0; k < size.height;
                             k++, bu1 += faceStep, bu2 += faceStep, a += avgStep )
                        {
                                //#pragma omp parallel for
                                for( l = 0; l < size.width - 3; l += 4 )
                                {
                                        float f = a[l];
                                        uchar u1 = bu1[l];
                                        uchar u2 = bu2[l];

                                        w += (u1 - f) * (u2 - f);
                                        f = a[l + 1];
                                        u1 = bu1[l + 1];
                                        u2 = bu2[l + 1];
                                        w += (u1 - f) * (u2 - f);
                                        f = a[l + 2];
                                        u1 = bu1[l + 2];
                                        u2 = bu2[l + 2];
                                        w += (u1 - f) * (u2 - f);
                                        f = a[l + 3];
                                        u1 = bu1[l + 3];
                                        u2 = bu2[l + 3];
                                        w += (u1 - f) * (u2 - f);
                                }
                                
                                for( ; l < size.width; l++ )
                                {
                                        float f = a[l];
                                        uchar u1 = bu1[l];
                                        uchar u2 = bu2[l];

                                        w += (u1 - f) * (u2 - f);
                                }
                        }

                        covarMatrix[i * nFaces + j] = covarMatrix[j * nFaces + i] = w;
                }
        }
}

void calcEigenFaces(int nFaces, IplImage** facesArr, IplImage** eigArr,  int iter,
                                IplImage *avg,  float *eigVals) 
{
        int i,j,k,l, p;
        float *covarMat, *ev;
        
        float *avg_data;
        int avg_step = 0, eig_step = 0;
        CvSize size;
        
        cvGetImageRawData( avg, (uchar **) & avg_data, &avg_step, &size );
        
        avg_step = avg_step/4;
        uchar **faces = (uchar **) cvAlloc( sizeof( uchar * ) * nFaces );
        float **eigs = (float **) cvAlloc( sizeof( float * ) * iter );
        int face_step = 0;
        
        for( i = 0; i < nFaces; i++ )
        {
                IplImage *face = facesArr[i];
                uchar *face_data;

                cvGetImageRawData( face, (uchar **) &face_data, &face_step, NULL);
                faces[i] = face_data;
        }
        
        
        for( i = 0; i < iter; i++ )
        {
                IplImage *eig = eigArr[i];
                float *eig_data;

                cvGetImageRawData( eig, (uchar **) & eig_data, NULL, NULL);
                eigs[i] = eig_data;
        }
        
        calcMeanImage( nFaces, 
                                faces, 
                                face_step, 
                                size, 
                                avg_data, 
                                avg_step );
        
        covarMat = (float *) cvAlloc( sizeof( float ) * nFaces * nFaces );
        
        //~ calcCovarMatrix( nFaces, 
                                //~ faces, 
                                //~ avg_step, 
                                //~ size, 
                                //~ avg_data, 
                                //~ avg_step,
                                //~ covarMat );
        
        //~ for ( i = 0; i < nFaces; i++ ) 
        //~ {
                //~ for ( j = 0; j < nFaces; j++ ) 
                //~ {
                        //~ printf("%f ", covarMat[i*nFaces+j]);
                //~ }
                //~ printf("\n");
        //~ }
        
        cvCalcCovarMatrixEx( nFaces,  facesArr, 0, 0, NULL, NULL, avg, covarMat );
        
        //~ printf("\n");
        //~ for ( i = 0; i < nFaces; i++ ) 
        //~ {
                //~ for ( j = 0; j < nFaces; j++ ) 
                //~ {
                        //~ printf("%f ", covarMat[i*nFaces+j]);
                //~ }
                //~ printf("\n");
        //~ }
        //~ printf("\n");
        
        ev = (float *) cvAlloc( sizeof( float ) * nFaces * nFaces );
        
        int nrot=0;
        jacobi(covarMat, nFaces, eigVals, ev, &nrot);
        eigsrt(eigVals, ev, nFaces);
        //JacobiEigens_32f(covarMat, ev, eigVals, nFaces, 0);
                
        //~ for ( j = 0; j < nFaces; j++ ) 
        //~ {
                //~ printf("%f ", eigVals[j]);
        //~ }
        //~ printf("\n\n");
        
        //~ for ( i = 0; i < nFaces; i++ ) 
        //~ {
                //~ for ( j = 0; j < nFaces; j++ ) 
                //~ {
                        //~ printf("%f ", ev[i*nFaces+j]);
                //~ }
                //~ printf("\n");
        //~ }
        
        //#pragma omp parallel for
        for( i = 0; i < iter; i++ )
                eigVals[i] = (float) (1.0 / sqrt( (double)eigVals[i] ));
        
        for( i = 0; i < iter; i++ ) 
        {
                float *be = eigs[i];

                //#pragma omp parallel for
                for( k = 0; k < size.height; k++, be += avg_step ) {
                        for( l = 0; l < size.width; l++ )
                                be[l] = 0.0f;
                }
        }

    for( k = 0; k < nFaces; k++ )
    {
                uchar *bv = faces[k];
                
                for( i = 0; i < iter; i++ )
                {
                        float v = eigVals[i] * ev[k * nFaces + i];
                        // float v = ev[i * nFaces + k];
                        float *be = eigs[i];
                        uchar *bu = bv;

                        float *bf = avg_data;

                        for( p = 0; p < size.height; p++, bu += face_step,
                                bf += avg_step, be += avg_step )
                        {
                                //#pragma omp parallel for
                                for( l = 0; l < size.width - 3; l += 4 )
                                {
                                        float f = bf[l];
                                        uchar u = bu[l];

                                        be[l] += v * (u - f);
                                        f = bf[l + 1];
                                        u = bu[l + 1];
                                        be[l + 1] += v * (u - f);
                                        f = bf[l + 2];
                                        u = bu[l + 2];
                                        be[l + 2] += v * (u - f);
                                        f = bf[l + 3];
                                        u = bu[l + 3];
                                        be[l + 3] += v * (u - f);
                                }
                                for( ; l < size.width; l++ )
                                        be[l] += v * (bu[l] - bf[l]);
                        }
                } 
        }
        
        //#pragma omp parallel for
        for( i = 0; i < iter; i++ )
                eigVals[i] = 1.f / (eigVals[i] * eigVals[i]);
}

void calcDecomp( IplImage* face, int nEigens, IplImage** eigArr, IplImage *avg,  float*  coeffs )
{
        int i, j, k;
        float w = 0.0f;
        
        float *avg_data;
        uchar *face_data;
        int avg_step = 0, face_step = 0;
        CvSize size;
        
        cvGetImageRawData( avg, (uchar **) & avg_data, &avg_step, &size );
        cvGetImageRawData( face, &face_data, &face_step, NULL);
        avg_step = avg_step/4;
        
        float **eigs = (float **) cvAlloc( sizeof( float * ) * nEigens );
        for( i = 0; i < nEigens; i++ )
        {
                IplImage *eig = eigArr[i];
                float *eig_data;

                cvGetImageRawData( eig, (uchar **) & eig_data, NULL, NULL );
                eigs[i] = eig_data;
        }
        
        for( k = 0; k < nEigens; k++ )
        {
                float *be = eigs[k];
                uchar *bu = face_data;
                float *bf = avg_data;
                
                for( i = 0; i < size.height; i++,  bu+= face_step, 
                        be += avg_step, bf += avg_step )
                {
                        for( j = 0; j < size.width - 4; j += 4 )
                        {
                                float o = (float) bu[j];
                                float e = be[j];
                                float a = bf[j];

                                w += e * (o - a);
                                o = (float) bu[j + 1];
                                e = be[j + 1];
                                a = bf[j + 1];
                                w += e * (o - a);
                                o = (float) bu[j + 2];
                                e = be[j + 2];
                                a = bf[j + 2];
                                w += e * (o - a);
                                o = (float) bu[j + 3];
                                e = be[j + 3];
                                a = bf[j + 3];
                                w += e * (o - a);
                        }
                        for( ; j < size.width; j++ )
                                w += be[j] * ((float) bu[j] - bf[j]);
                }
                
                //~ if( w < -1.0e29f )
                        //~ return CV_NOTDEFINED_ERR;
                coeffs[i] = w;
        }
}