cvmatrix64d.cpp.svn-base

来自「非结构化路识别」· SVN-BASE 代码 · 共 766 行 · 第 1/2 页

#endif


    icvDeleteMatrix( tmpMatr );
    icvDeleteMatrix( tmpMatr2 );
    icvDeleteMatrix( srcTrans );



#ifdef TRACE_TO_FILE
    fclose( file );
#endif

    return CV_NO_ERR;
}


#if 0
/*======================================================================================*/
IPCVAPI_IMPL( CvStatus, icvComplexMult_64d, (CvMatr64d srcMatr,
                                             CvMatr64d dstMatr, int width, int height) )
{
    double *tmpMatr;
    double *tmpMatr2;
    double *srcTrans;

    double *pMatr;
    double sum;
    int i, j, k;
    double *ps;
    double *pt;
    double *pti;
    double *ptj;
    double *ptk;
    double d;
    double *srcBase;
    double *dstBase;
    double *ptkBase;
    double *begRow;
    double *dstBase_i;
    int colShift;
    double coef;
    int shift;

/*    int         t; */
    int shift_i, shift_j;
    int di, dj;
    int delta;
    int matrSize;

#ifdef TRACE_TO_FILE
    FILE *file;
#endif
    matrSize = width;
/*
    file = fopen("E:\\test_matrix\\comples_mult.txt","w");

    fprintf(file,"\nSource matrix\n");
    for (t = 0; t < (matrSize * (matrSize )) ; t++) {
        fprintf(file,"srcMatr[%d] = %f\n",t,srcMatr[t]);
        
    }
*/

    tmpMatr = (double *) icvAlloc( (matrSize * (matrSize + 1)) / 2 * sizeof( double ));
    tmpMatr2 = (double *) icvAlloc( (matrSize * (matrSize + 1)) / 2 * sizeof( double ));
    srcTrans = (double *) icvAlloc( MAX( height, width ) * width * sizeof( double ));
    memset( tmpMatr, 0, (matrSize * (matrSize + 1)) / 2 * sizeof( double ));
    memset( tmpMatr2, 0, (matrSize * (matrSize + 1)) / 2 * sizeof( double ));
    memset( srcTrans, 0, MAX( height, width ) * width * sizeof( double ));

    /* Compute (AtA)-1 * At */

    /* Compute AtA and store in low triangular matrix */

    icvTransposeMatrix_64d( srcMatr, width, height, srcTrans );
/*
    fprintf(file,"\ntranspose source matrix\n");
    for (t = 0; t < width * height  ; t++) {
        fprintf(file,"srcTrans[%d] = %f\n",t,srcTrans[t]);
    }
*/
    sum = 0;

    pMatr = tmpMatr;

    for( i = 0; i < width; i++ )
    {
        for( j = 0; j < i + 1; j++ )
        {
            *pMatr++ = icvDotProduct_64d( srcTrans + i * height,
                                   srcTrans + j * height, height );
        }
    }

/*
    fprintf(file,"\nMult matrix\n");
    for (t = 0; t < (matrSize * (matrSize + 1)) / 2; t++) {
        fprintf(file,"tmpMatr[%d] = %f\n",t,tmpMatr[t]);
    }
*/
    /* Apply cholesky decomposition */


    ps = tmpMatr;
    pt = tmpMatr2;
    pti = pt;

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

        ptj = pt;               /* Get begin of the matrix */

        for( j = 0; j < i; j++ )
        {                       /*  By all elements in line of source */
            ptk = pti;          /* ti - current element of the matrix */
            sum = 0.0;
            k = j;

            while( k-- )
            {
                sum += (*ptj++) * (*ptk++);
            }
            *ptk = ((*ps++) - sum) / (*ptj++);
        }

        sum = 0.0;
        k = i;
        while( k-- )
        {
            sum += ((*pti) * (*pti));
            pti++;
        }
        d = (*ps++) - sum;
/*
        if (d <= 0.0) {
            return CV_BADCOEF_ERR;
        }
*/
        *pti++ = sqrt( (double)d );
    }

/*  
    fprintf(file,"\nCholesky matrix (tmpMatr2)\n");
    for (t = 0; t < (matrSize * (matrSize + 1)) / 2; t++) {
        fprintf(file,"tmpMatr2[%d] = %f\n",t,tmpMatr2[t]);
    }
*/

    /*  Find inverse matrix for triangular tmpMatr2 and strore in tmpMatr */


    icvSetZero_64d( tmpMatr, 1, (matrSize * (matrSize + 1)) / 2 );

    dstBase = tmpMatr;
    shift = 2;

    for( i = 0; i < matrSize; i++ )
    {
        *dstBase = 1.0;
        dstBase += shift;
        shift++;
    }
/*
    fprintf(file,"\nIdentical matrix (tmpMatr)\n");
    for (t = 0; t < (matrSize * (matrSize + 1)) / 2; t++) {
        fprintf(file,"tmpMatr[%d] = %f\n",t,tmpMatr[t]);
    }
*/
    srcBase = tmpMatr2;
    dstBase = tmpMatr;
    ptk = tmpMatr;
    ptkBase = tmpMatr;

    for( i = 0; i < matrSize; i++ )
    {                           /* For each row */

        ptj = tmpMatr;

        for( j = 0; j < i; j++ )
        {                       /* for each element in row */
            coef = *srcBase++;  /* Get current coefficient */
            k = j + 1;

            ptk = ptkBase;
            while( k-- )
            {                   /* Multiplacate elements and subtruct */
                *(ptk++) -= coef * (*ptj++);
            }
        }

        k = i + 1;
        coef = 1.0 / (*srcBase++);

        while( k-- )
        {
            *ptkBase++ *= coef;
        }
    }
/*
    fprintf(file,"\nInv Cholesky matrix (tmpMatr)\n");
    for (t = 0; t < (matrSize * (matrSize + 1)) / 2; t++) {
        fprintf(file,"tmpMatr[%d] = %f\n",t,tmpMatr[t]);
    }
*/

    /* Transpose this matrix for suitable computation tmpMatr -> tmpMatr2 */


    dstBase = tmpMatr2;
    srcBase = tmpMatr;
    begRow = tmpMatr;           /* begin of row for each cycle */

    shift = 2;

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

        colShift = i + 1;


        ptj = begRow;

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

            *dstBase++ = *ptj;

            ptj += colShift;
            colShift++;
        }

        begRow += shift;
        shift++;
    }
/*
    fprintf(file,"\nTranspose Inv Cholesky matrix (tmpMatr2)\n");
    for (t = 0; t < (matrSize * (matrSize + 1)) / 2; t++) {
        fprintf(file,"tmpMatr2[%d] = %f\n",t,tmpMatr2[t]);
    }
*/
    /* multiplacate to itself (quadr) matrix tmpMatr2  */
    /* and store result in the srcTrans (quadr) */

    dstBase_i = srcTrans;

    shift_i = matrSize;

    pti = tmpMatr2;

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

        dstBase = dstBase_i;
        dstBase_i += matrSize;

        ptj = tmpMatr2;

        shift_j = matrSize;

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

            if( j < i )
            {
                dj = i - j;
                di = 0;
                delta = i;
            }
            else
            {
                dj = 0;
                di = j - i;
                delta = j;
            }

            *dstBase++ = icvDotProduct_64d( pti + di, ptj + dj, matrSize - delta );
            ptj += shift_j;
            shift_j--;
        }

        pti += shift_i;
        shift_i--;

    }

    /* copy elements to up the diagonal */

    for( i = 0; i < matrSize - 1; i++ )
    {
        for( j = i + 1; j < matrSize; j++ )
        {
            srcTrans[i * matrSize + j] = srcTrans[j * matrSize + i];
        }
    }

/*
    fprintf(file,"\nMultiplacate matix not diagonal(srcTrans)\n");
    for(i = 0; i < matrSize; i++) {
        for (j = 0; j < matrSize; j++) {
            fprintf(file,"srcTrans[%d,%d] = %f\n",i,j,srcTrans[i * matrSize + j]);
        }
        fprintf(file,"\n");
    }
*/


    /* Multiplicate this triangular matrix and transpose source */
    /* for suitable use not transpose matrix srcMatr */

    dstBase = dstMatr;

    pti = srcTrans;

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

        ptj = srcMatr;

        for( j = 0; j < height; j++ )
        {
            *dstBase++ = icvDotProduct_64d( pti, ptj, matrSize );
            ptj += width;
        }

        pti += matrSize;
    }

/*
    fprintf(file,"\nResult matrix (dstMatr)\n");
    for(i = 0; i < width; i++) {
        for (j = 0; j < height; j++) {
            fprintf(file,"dstMatr[%d,%d] = %f\n",i,j,dstMatr[i * height + j]);
        }
        fprintf(file,"\n");
    }
*/

    icvFree( &tmpMatr );
    icvFree( &tmpMatr2 );
    icvFree( &srcTrans );

/*    fclose(file); */
    return CV_NO_ERR;
}
#else
IPCVAPI_IMPL( CvStatus, icvComplexMult_64d, (CvMatr64d srcMatr,
                                             CvMatr64d dstMatr, int width, int height) )
{
    CvMat src = cvMat( height, width, CV_64FC1, srcMatr );
    CvMat dst = cvMat( width, height, CV_64FC1, dstMatr );

    CvMat* at = cvCreateMat( width, height, CV_64FC1 );
    CvMat* ata = cvCreateMat( width, width, CV_64FC1 );
    CvMat* inv_ata = cvCreateMat( width, width, CV_64FC1 );

    cvMulTransposed( &src, ata, 1 );
    cvInvert( ata, inv_ata, CV_SVD );

#if 0
    {
        CvMat* t = cvCreateMat( width, width, CV_64FC1 );
        CvMat* eye = cvCreateMat( width, width, CV_64FC1 );
        cvSetIdentity( eye );
        cvMatMul( ata, inv_ata, t );
        double norm = cvNorm( eye, t, CV_L1 );
        cvReleaseMat( &t );
        cvReleaseMat( &eye );
    }
#endif

    cvT( &src, at );
    cvMatMul( inv_ata, at, &dst );

    cvReleaseMat( &at );
    cvReleaseMat( &ata );
    cvReleaseMat( &inv_ata );

    return CV_OK;
}
#endif