matrix.cpp

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

    for( k = (n - 1); k >= 0; k-- )
    {
        for( its = 1; its <= 30; its++ )
        {
            flag = 1;
            for( l = k; l >= 0; l-- )
            {
                nm = l - 1;
                if( (fabs( rv1[l] ) + anorm) == anorm )
                {
                    flag = 0;
                    break;
                }
                if( (fabs( w[nm * n + nm] ) + anorm) == anorm )
                    break;
            }
            if( flag )
            {
                c = 0.0;
                s = 1.0;
                for( i = l; i <= k; i++ )
                {
                    f = s * rv1[i];
                    if( (fabs( f ) + anorm) != anorm )
                    {
                        g = w[i * n + i];
                        h = Pythag( f, g ); //changed
                        w[i * n + i] = h;
                        h = 1.0f / h;
                        c = g * h;
                        s = (-f * h);
                        for( j = 0; j < m; j++ )
                        {
                            y = A[j * n + nm];
                            z = A[j * n + i];
                            A[j * n + nm] = y * c + z * s;
                            A[j * n + i] = z * c - y * s;
                        }
                    }
                }
            }
            z = w[k * n + k];
            if( l == k )
            {
                if( z < 0.0 )
                {
                    w[k * n + k] = -z;
                    for( j = 0; j < n; j++ )
                        v[j * n + k] = (-v[j * n + k]);
                }
                break;
            }
            if( its == 30 )
            {
                return false;  /* error: no convergence */
            }
            x = w[l * n + l];
            nm = k - 1;
            y = w[nm * n + nm];
            g = rv1[nm];
            h = rv1[k];
            f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0f * h * y);
            g = Pythag( f, 1.0 ); //changed
            r = (f >= 0.0 ? g : -g);
            f = ((x - z) * (x + z) + h * ((y / (f + r)) - h)) / x;
            c = s = 1.0;
            for( j = l; j <= nm; j++ )
            {
                i = j + 1;
                g = rv1[i];
                y = w[i * n + i];
                h = s * g;
                g = c * g;
                z = Pythag( f, h );  //changed
                rv1[j] = z;
                c = f / z;
                s = h / z;
                f = x * c + g * s;
                g = g * c - x * s;
                h = y * s;
                y = y * c;
                for( jj = 0; jj < n; jj++ )
                {
                    x = v[jj * n + j];
                    z = v[jj * n + i];
                    v[jj * n + j] = x * c + z * s;
                    v[jj * n + i] = z * c - x * s;
                }
                z = Pythag( f, h );  //changed
                w[j * n + j] = z;
                if( z )
                {
                    z = 1.0f / z;
                    c = f * z;
                    s = h * z;
                }
                f = (c * g) + (s * y);
                x = (c * y) - (s * g);
                for( jj = 0; jj < m; jj++ )
                {
                    y = A[jj * n + j];
                    z = A[jj * n + i];
                    A[jj * n + j] = y * c + z * s;
                    A[jj * n + i] = z * c - y * s;
                }
            }
            rv1[l] = 0.0;
            rv1[k] = f;
            w[k * n + k] = x;
        }
    }


    /* Sort values */
    for( i = n - 1; i >= 0; i-- )
    {
        min_num = i;
        for( j = 0; j < i; j++ )
        {                       /* find minimal mean */
            if( w[j * n + j] < w[i * n + i] )
            {
                min_num = j;
            }
        }

        if( i != min_num )
        {
            tmpf = w[min_num * n + min_num];
            w[min_num * n + min_num] = w[i * n + i];
            w[i * n + i] = tmpf;

            for( k = 0; k < n; k++ )
            {
                tmpf = A[k * n + i];
                A[k * n + i] = A[k * n + min_num];
                A[k * n + min_num] = tmpf;
            }

            for( k = 0; k < n; k++ )
            {
                tmpf = v[k * n + i];
                v[k * n + i] = v[k * n + min_num];
                v[k * n + min_num] = tmpf;
            }

        }
    }

    {
/*      CvMatr64d tmp_matrV;
        CvMatr64d tmp_matrS;
        CvVect64d tmp_f1;
        CvVect64d tmp_f2;
*/
        double * tmp_matrV;
		double * tmp_matrS;
		double * tmp_f1;
		double * tmp_f2;

		int i, j, t;
        double dot;

/*      tmp_matrV = icvCreateMatrix_64d( n, n );
        tmp_matrS = icvCreateMatrix_64d( n, n );
        tmp_f1 = icvCreateVector_64d( n );
        tmp_f2 = icvCreateVector_64d( n );
*/
		tmp_matrV = new double[n * n];
		tmp_matrS = new double[n * n];
		tmp_f1 = new double[n];
		tmp_f2 = new double[n];
        /* Find eigen vectors */


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

        JacobiEigens( tmpA, tmp_matrV, tmp_matrS,       /* this matrix uses as a vector(3) */
                              n, 0.0f );    //changed

        /* Change signs if need */
        for( i = 0; i < n; i++ )
        {                       /* for each column */

            for( t = 0; t < n; t++ )
            {
                tmp_f1[t] = tmp_matrV[t * n + i];
                tmp_f2[t] = A[t * n + i];
            }

            DotProduct( tmp_f1, tmp_f2, n, &dot );  //changed

            if( dot < 0 )
            {
                for( j = 0; j < n; j++ )
                {               /* for each row */
                    A[j * n + i] = -A[j * n + i];
                    v[j * n + i] = -v[j * n + i];
                }
            }
        }

/*       icvDeleteMatrix( tmp_matrV );
        icvDeleteMatrix( tmp_matrS );
        icvDeleteVector( tmp_f1 );
        icvDeleteVector( tmp_f2 );
*/
		delete [] tmp_matrV;
		delete [] tmp_matrS;
		delete [] tmp_f1;
		delete [] tmp_f2;
    }

/*    icvDeleteMatrix( tmpA );
    icvDeleteVector( rv1 );
 */
	delete [] tmpA;
	delete [] rv1;

	return true;
}

BOOL CMatrix::Transpose(double *src, int height, int width, double *dest)
{
    int i, j;

    int Decrem = width * height - 1;

    if( src == NULL || dest == NULL || width <= 0 || height <= 0 )
        return false;
	
	for( j = 0; j < height; j++ )
    {
        for( i = 0; i < width; i++ )
        {
            *dest = src[i];
            dest += height;
        }
        dest -= Decrem;
        src += width;
    }
    return true;
}

BOOL CMatrix::Invert(double *srcMatr, int matrSize, double *dstMatr)
{
    double *Temp;
    double *tmpVect;
    double *tmpLineVect;
    double *tmpDstLineVect;
    int i, j;
    double Mult;

/*    int         ti,tj; */
    int change, st;
    int change_sign;

/*    int         t; */


#ifdef TRACE_TO_FILE
    FILE *file;

    file = fopen( "e:\\test_matrix\\Inv_matr.txt", "w" );
#endif

    change_sign = 0;

/*  Temp = (double *) icvAlloc( matrSize * matrSize * sizeof( double ));
    tmpVect = (double *) icvAlloc( matrSize * sizeof( double ));
    tmpLineVect = (double *) icvAlloc( matrSize * sizeof( double ));
    tmpDstLineVect = (double *) icvAlloc( matrSize * sizeof( double ));
*/
	int size = matrSize * sizeof(double);
	Temp = new double[matrSize * size];
	tmpVect = new double[size];
	tmpLineVect = new double[size];
	tmpDstLineVect = new double[size];

	memset( Temp, 0, matrSize * matrSize * sizeof( double ));
    memset( tmpVect, 0, matrSize * sizeof( double ));
    memset( tmpLineVect, 0, matrSize * sizeof( double ));
    memset( tmpDstLineVect, 0, matrSize * sizeof( double ));

    //icvCopyMatrix_64d( srcMatr, matrSize, matrSize, Temp );
	memcpy(Temp, srcMatr, size);

    //icvSetIdentity_64d( dstMatr, matrSize, matrSize );
	SetIdentity(dstMatr, matrSize, matrSize);

    for( i = 0; i < matrSize; i++ )
    {

        /* Sort line for non zero element */

        change = i;


        /* !!! In change when test !=0 if all values == 0 then DET=0 and INV not exist */

        for( st = i; st < matrSize; st++ )
        {

            if( Temp[st * matrSize + i] != 0 )
            {

                change = st;
                break;
            }
        }

        if( change != i )
        {
 /*         icvCopyVector_64d( &Temp[(i) * matrSize], matrSize, tmpVect );

            icvCopyVector_64d( &Temp[st * matrSize], matrSize, &Temp[(i) * matrSize] );

            icvCopyVector_64d( tmpVect, matrSize, &Temp[st * matrSize] );

            icvCopyVector_64d( &dstMatr[(i) * matrSize], matrSize, tmpVect );

            icvCopyVector_64d( &dstMatr[st * matrSize], matrSize, &dstMatr[(i) * matrSize] );

            icvCopyVector_64d( tmpVect, matrSize, &dstMatr[st * matrSize] );
  */
			memcpy(tmpVect, &Temp[(i) * matrSize], matrSize * sizeof(double));
			memcpy(&Temp[(i) * matrSize], &Temp[st * matrSize], matrSize * sizeof(double));
			memcpy(&Temp[st * matrSize], tmpVect, matrSize * sizeof(double));
			memcpy(tmpVect, &dstMatr[(i) * matrSize], matrSize * sizeof(double));
			memcpy(&dstMatr[(i) * matrSize], &dstMatr[st * matrSize], matrSize * sizeof(double));
			memcpy(&dstMatr[st * matrSize], tmpVect, matrSize * sizeof(double));

			change_sign++;

        }
        else
        {
            if( Temp[i * matrSize + i] == 0 )
            {
                /* Inv matrix does not exist det == 0 */
                //return CV_BADCOEF_ERR;
				return false;
            }
        }

        Mult = 1.0 / Temp[i * matrSize + i];

#ifdef TRACE_TO_FILE

        if( Temp[i * matrSize + i] < 0.001 )
        {
            fprintf( file, "\nElement too small: %f\n", Temp[i * matrSize + i] );
        }

        if( Temp[i * matrSize + i] > 1000 )
        {
            fprintf( file, "\nElement too big: %f\n", Temp[i * matrSize + i] );
        }
#endif
        Temp[i * matrSize + i] = 1.0;

        /* Test !!! */

        //icvScaleVector_64d( &Temp[i * matrSize + i + 1],
        //                     &Temp[i * matrSize + i + 1], matrSize - i - 1, Mult );

        //icvScaleVector_64d( &dstMatr[i * matrSize], &dstMatr[i * matrSize], matrSize, Mult );
		ScaleVector(&Temp[i * matrSize + i + 1], &Temp[i * matrSize + i + 1], matrSize - i - 1, Mult);
		ScaleVector(&dstMatr[i * matrSize], &dstMatr[i * matrSize], matrSize, Mult );

        /* Subtructing */

        //icvCopyVector_64d( &Temp[i * matrSize + i + 1], matrSize - i - 1, tmpLineVect );

        //icvCopyVector_64d( &dstMatr[i * matrSize], matrSize, tmpDstLineVect );
		memcpy(tmpLineVect, &Temp[i * matrSize + i + 1], (matrSize - i - 1) * sizeof(double));
		memcpy(tmpDstLineVect, &dstMatr[i * matrSize], matrSize * sizeof(double));

        for( j = i + 1; j < matrSize; j++ )
        {

            Mult = Temp[j * matrSize + i];

            Temp[j * matrSize + i] = 0.0;

            //icvScaleVector_64d( tmpLineVect, tmpVect, matrSize - i - 1, Mult );
			ScaleVector(tmpLineVect, tmpVect, matrSize - i - 1, Mult);

            //icvSubVector_64d( &Temp[j * matrSize + i + 1],
            //                   tmpVect, &Temp[j * matrSize + i + 1], matrSize - i - 1 );
			SubVector(&Temp[j * matrSize + i + 1], tmpVect, &Temp[j * matrSize + i + 1], matrSize - i - 1 );


            //icvScaleVector_64d( tmpDstLineVect, tmpVect, matrSize, Mult );
			ScaleVector(tmpDstLineVect, tmpVect, matrSize, Mult );

            //icvSubVector_64d( &dstMatr[j * matrSize],
            //                   tmpVect, &dstMatr[j * matrSize], matrSize );
			SubVector(&dstMatr[j * matrSize], tmpVect, &dstMatr[j * matrSize], matrSize );

        }
    }

    /* Down to top direct */

    for( i = matrSize - 1; i > 0; i-- )
    {

        /* Subtructing */

        //icvCopyVector_64d( &Temp[i * matrSize + i + 1], matrSize - i - 1, tmpLineVect );
		memcpy(tmpLineVect, &Temp[i * matrSize + i + 1], matrSize - i - 1);

        //icvCopyVector_64d( &dstMatr[i * matrSize], matrSize, tmpDstLineVect );
		memcpy(tmpDstLineVect, &dstMatr[i * matrSize], matrSize);

        for( j = i - 1; j >= 0; j-- )
        {

            Mult = Temp[j * matrSize + i];

            Temp[j * matrSize + i] = 0.0;

            //icvScaleVector_64d( tmpLineVect, tmpVect, matrSize - i - 1, Mult );
			ScaleVector( tmpLineVect, tmpVect, matrSize - i - 1, Mult );

            //icvSubVector_64d( &Temp[j * matrSize + i + 1],
            //                   tmpVect, &Temp[j * matrSize + i + 1], matrSize - i - 1 );
			SubVector( &Temp[j * matrSize + i + 1], tmpVect, &Temp[j * matrSize + i + 1], matrSize - i - 1 );

            //icvScaleVector_64d( tmpDstLineVect, tmpVect, matrSize, Mult );
			ScaleVector( tmpDstLineVect, tmpVect, matrSize, Mult );

            //icvSubVector_64d( &dstMatr[j * matrSize],
            //                   tmpVect, &dstMatr[j * matrSize], matrSize );
			SubVector( &dstMatr[j * matrSize], tmpVect, &dstMatr[j * matrSize], matrSize );

        }
    }

    if( change_sign & 1 == 1 )
    {
        //icvScaleMatrix_64d( dstMatr, dstMatr, matrSize, matrSize, -1.0 );
		ScaleVector( dstMatr, dstMatr, matrSize * matrSize, -1.0 );
    }

    /* free allocated memory */

/*  icvFree( &Temp );
    icvFree( &tmpVect );
    icvFree( &tmpLineVect );
    icvFree( &tmpDstLineVect );
*/
	delete [] Temp;
	delete [] tmpVect;
	delete [] tmpLineVect;
	delete [] tmpDstLineVect;

#ifdef TRACE_TO_FILE