matrix.cpp

opencv实现的人体运动跟踪源码

#include "stdafx.h"
#include <math.h>

#include "Matrix.h"


CMatrix::CMatrix()
{
}

CMatrix::~CMatrix()
{
}

BOOL CMatrix::Add(double *src1, double *src2, double *dest, int height, int width)
{
    if( src1 == NULL || src2 == NULL || dest == NULL || width <= 0 || height <= 0)
		return false;
    
	for(int  i = 0; i < width * height; i++ )
    {
        dest[i] = src1[i] + src2[i];
    }

    return true;
}

BOOL CMatrix::BiDiag(double *A, int n, int m, double *w, double *rv1, double *anorm)
{
    double f, g = 0.0, h, r, s = 0.0, scale = 0.0;
    int i, j, k, l;

    *anorm = 0.0;
    for( i = 0; 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 * n + i] );
            if( scale )
            {
                for( k = i; k < m; k++ )
                {
                    A[k * n + i] /= scale;
                    s += A[k * n + i] * A[k * n + i];
                }
                f = A[i * n + i];
                g = sqrt( s );
                if( f >= 0.0 )
                    g = -g;
                h = f * g - s;
                A[i * n + i] = f - g;
                if( i != (n - 1) )
                {
                    for( j = l; j < n; j++ )
                    {
                        for( s = 0.0, k = i; k < m; k++ )
                            s += A[k * n + i] * A[k * n + j];
                        f = s / h;
                        for( k = i; k < m; k++ )
                            A[k * n + j] += f * A[k * n + i];
                    }
                }
                for( k = i; k < m; k++ )
                    A[k * n + i] *= scale;
            }
        }
        w[i * n + i] = scale * g;
        g = s = scale = 0.0;
        if( i < m && i != (n - 1) )
        {
            for( k = l; k < n; k++ )
                scale += fabs( A[i * n + k] );
            if( scale )
            {
                for( k = l; k < n; k++ )
                {
                    A[i * n + k] /= scale;
                    s += A[i * n + k] * A[i * n + k];
                }
                f = A[i * n + l];
                g = sqrt( s );
                if( f >= 0.0 )
                    g = -g;
                h = 1.0f / (f * g - s);
                A[i * n + l] = f - g;
                for( k = l; k < n; k++ )
                    rv1[k] = A[i * n + k] * h;
                if( i != (m - 1) )
                {
                    for( j = l; j < m; j++ )
                    {
                        for( s = 0.0, k = l; k < n; k++ )
                            s += A[j * n + k] * A[i * n + k];
                        for( k = l; k < n; k++ )
                            A[j * n + k] += s * rv1[k];
                    }
                }
                for( k = l; k < n; k++ )
                    A[i * n + k] *= scale;
            }
        }
        r = (fabs( w[i * n + i] ) + fabs( rv1[i] ));
        if( r > *anorm )
            *anorm = r;
    }
    return true;
}

BOOL CMatrix::DotProduct(double *src1, double *src2, int length, double *value)
{
    double sum = 0.f;

    /* check bad arguments */
    if( src1 == NULL || src2 == NULL || length <= 0)
        return false;
    
    for(int i = 0; i < length; i++ )
    {
        sum += src1[i] * src2[i];
    }

    *value = sum;
    return true;
}

BOOL CMatrix::InitialWV(double *A, int n, int m, double *w, double *v, double *rv1)
{
    int i, j, k, l;
    double f, g = 0.0, s;

    for( i = (n - 1); i >= 0; i-- )
    {
        l = i + 1;
        if( i < (n - 1) )
        {
            if( g )
            {
                for( j = l; j < n; j++ )
                    v[j * n + i] = (A[i * n + j] / A[i * n + l]) / g;
                for( j = l; j < n; j++ )
                {
                    for( s = 0.0, k = l; k < n; k++ )
                        s += A[i * n + k] * v[k * n + j];
                    for( k = l; k < n; k++ )
                        v[k * n + j] += s * v[k * n + i];
                }
            }
            for( j = l; j < n; j++ )
                v[i * n + j] = v[j * n + i] = 0.0;
        }
        v[i * n + i] = 1.0;
        g = rv1[i];
    }
    for( i = (n - 1); i >= 0; i-- )
    {
        l = i + 1;
        g = w[i * n + i];
        if( i < (n - 1) )
        {
            for( j = l; j < n; j++ )
                A[i * n + j] = 0.0;
        }
        if( g )
        {
            g = 1.0f / g;
            if( i != (n - 1) )
            {
                for( j = l; j < n; j++ )
                {
                    for( s = 0.0, k = l; k < m; k++ )
                        s += A[k * n + i] * A[k * n + j];
                    f = (s / A[i * n + i]) * g;
                    for( k = i; k < m; k++ )
                        A[k * n + j] += f * A[k * n + i];
                }
            }
            for( j = i; j < m; j++ )
                A[j * n + i] *= g;
        }
        else
        {
            for( j = i; j < m; j++ )
                A[j * n + i] = 0.0;
        }
        A[i * n + i] += 1.0;
    }
    return true;
}

BOOL CMatrix::JacobiEigens(double *A, double *V, double *E, int n, double eps)
{
    int i, j, k, p, q, ind;
    double *A1 = A, *V1 = V, *A2 = A, *V2 = V;
    double Amax = 0.0, anorm = 0.0, ax, deps;

    if( A == NULL || V == NULL || E == NULL || n <= 0 )
        return false;
    if( eps < 1.0e-15 )
        eps = 1.0e-15;
    deps = eps / (double) n;

    /*-------- Prepare --------*/
    for( i = 0; i < n; i++, V1 += n, A1 += n )
    {
        for( j = 0; j < i; j++ )
        {
            double Am = A1[j];

            anorm += Am * Am;
        }
        for( j = 0; j < n; j++ )
            V1[j] = 0.0;
        V1[i] = 1.0;
    }

    anorm = sqrt( anorm + anorm );
    ax = anorm * eps / n;
    Amax = anorm;

    while( Amax > ax )
    {
        Amax /= n;
        do                      /* while (ind) */
        {
            ind = 0;
            A1 = A;
            V1 = V;
            for( p = 0; p < n - 1; p++, A1 += n, V1 += n )
            {
                A2 = A + n * (p + 1);
                V2 = V + n * (p + 1);
                for( q = p + 1; q < n; q++, A2 += n, V2 += n )
                {
                    double x, y, c, s, c2, s2, a;
                    double *A3, Apq, App, Aqq, App2, Aqq2, Aip, Aiq, Vpi, Vqi;

                    if( fabs( A1[q] ) < Amax )
                        continue;
                    Apq = A1[q];

                    ind = 1;

                    /*---- Calculation of rotation angle's sine & cosine ----*/
                    App = A1[p];
                    Aqq = A2[q];
                    y = 5.0e-1 * (App - Aqq);
                    x = -Apq / sqrt( Apq * Apq + y * y );
                    if( y < 0.0 )
                        x = -x;
                    s = x / sqrt( 2.0 * (1.0 + sqrt( 1.0 - x * x )));
                    s2 = s * s;
                    c = sqrt( 1.0 - s2 );
                    c2 = c * c;
                    a = 2.0 * Apq * c * s;

                    /*---- Apq annulation ----*/
                    A3 = A;
                    for( i = 0; i < p; i++, A3 += n )
                    {
                        Aip = A3[p];
                        Aiq = A3[q];
                        Vpi = V1[i];
                        Vqi = V2[i];
                        A3[p] = Aip * c - Aiq * s;
                        A3[q] = Aiq * c + Aip * s;
                        V1[i] = Vpi * c - Vqi * s;
                        V2[i] = Vqi * c + Vpi * s;
                    }
                    for( ; i < q; i++, A3 += n )
                    {
                        Aip = A1[i];
                        Aiq = A3[q];
                        Vpi = V1[i];
                        Vqi = V2[i];
                        A1[i] = Aip * c - Aiq * s;
                        A3[q] = Aiq * c + Aip * s;
                        V1[i] = Vpi * c - Vqi * s;
                        V2[i] = Vqi * c + Vpi * s;
                    }
                    for( ; i < n; i++ )
                    {
                        Aip = A1[i];
                        Aiq = A2[i];
                        Vpi = V1[i];
                        Vqi = V2[i];
                        A1[i] = Aip * c - Aiq * s;
                        A2[i] = Aiq * c + Aip * s;
                        V1[i] = Vpi * c - Vqi * s;
                        V2[i] = Vqi * c + Vpi * s;
                    }
                    App2 = App * c2 + Aqq * s2 - a;
                    Aqq2 = App * s2 + Aqq * c2 + a;
                    A1[p] = App2;
                    A2[q] = Aqq2;
                    A1[q] = A2[p] = 0.0;
                }               /*q */
            }                   /*p */
        }
        while( ind );
    }                           /* while ( Amax > ax ) */

    for( i = 0, k = 0; i < n; i++, k += n + 1 )
        E[i] = A[k];

    /* -------- ordering -------- */
    for( i = 0; i < n; i++ )
    {
        int m = i;
        double Em = fabs( E[i] );

        for( j = i + 1; j < n; j++ )
        {
            double Ej = fabs( E[j] );

            m = (Em < Ej) ? j : m;
            Em = (Em < Ej) ? Ej : Em;
        }
        if( m != i )
        {
            int l;
            double b = E[i];

            E[i] = E[m];
            E[m] = b;
            for( j = 0, k = i * n, l = m * n; j < n; j++, k++, l++ )
            {
                b = V[k];
                V[k] = V[l];
                V[l] = b;
            }
        }
    }

    return true;
}

BOOL CMatrix::Mul(double *src1, int height1, int width1, double *src2, int height2, int width2, double *dst)
{
    int i, j, k;
    double sum = 0;
    double *first = src1;
    double *second = src2;
    double *dest = dst;
    int Step1 = width1;
    int Step2 = width2;

    if( src1 == NULL || src2 == NULL || dest == NULL || height2 != width1 )
        return false;

    for( j = 0; j < height1; j++ )
    {
        for( i = 0; i < width2; i++ )
        {
            sum = 0;
            second = src2 + i;
            for( k = 0; k < width1; k++ )
            {
                sum += first[k] * (*second);
                second += Step2;
            }
            dest[i] = sum;
        }
        first += Step1;
        dest += Step2;
    }
    return true;
}

double CMatrix::Pythag(double a, double b)
{
    /* computes (a^2 + b^2)^1/2 without destructive underflow or overflow */
    double at, bt, ct;

    at = fabs( a );
    bt = fabs( b );
    if( at > bt )
    {
        ct = bt / at;
        return ( at * sqrt( 1.0 + ct * ct ));
    }
    else
    {
        if( bt )
        {
            ct = at / bt;
            return ( bt * sqrt( 1.0 + ct * ct ));
        }
        else
        {
            return 0.0;
        }
    }
}

BOOL CMatrix::Scale(double *src, double *dest, double value, int height, int width)
{
	if (src == NULL || dest == NULL || height <= 0 || width <= 0)
		return false;

	for (int i = 0 ; i < height * width; i++) {
		dest[i] = src[i] * value;
	}

	return true;
}

BOOL CMatrix::Sub(double *src1, double *src2, double *dest, int height, int width)
{
    if( src1 == NULL || src2 == NULL || dest == NULL || width <= 0 || height <= 0)
		return false;
    
	for(int  i = 0; i < width * height; i++ )
    {
        dest[i] = src1[i] - src2[i];
    }

    return true;
}

BOOL CMatrix::SVD(double *A, int height, int width, double *w, double *v)
{
    int m = height;
	int n = width;

	int flag, i, its, j, jj, k, l, nm = 0;
    double c, f, h, s, x, y, z;
    double anorm = 0.0, g = 0.0, r;
  //  CvVect64d rv1;
  //  CvMatr64d tmpA;
	double * rv1;
	double * tmpA;

    int min_num;
    double tmpf;

    //tmpA = icvCreateMatrix_64d( n, m );
    //rv1 = icvCreateVector_64d( n );
	tmpA = new double[m * n];
	rv1 = new double [n];

    //icvCopyMatrix_64d( A, n, m, tmpA );
    //icvSetZero_64d( rv1, 1, n );
	::memcpy(tmpA, A, m * n * sizeof(double));
	::memset(rv1, 0, n * sizeof(double));

    if( m < n )
        return false;  /* a error: rows < cols */

    //icvSetZero_64d( w, n, n );
    //icvSetZero_64d( v, n, n );
	::memset(w, 0, n * n * sizeof(double));
	::memset(v, 0, n * n * sizeof(double));

    BiDiag( A, n, m, w, rv1, &anorm ); //changed
    InitialWV( A, n, m, w, v, rv1 );