cvmatrix64d.cpp.svn-base

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

/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "_cv.h"

static float
square_32f( float x )
{
    return x * x;
}

/*====================================================================================*/


IPCVAPI_IMPL( CvStatus, icvComplexMult_32f, (CvMatr32f srcMatr,
                                             CvMatr32f dstMatr, int width, int height) )
{
    CvVect32f tmpMatr;
    CvVect32f tmpMatr2;
    CvMatr32f srcTrans;

    float *pMatr;
    float sum;
    int i, j, k;
    float *ps;
    float *pt;
    float *pti;
    float *ptj;
    float *ptk;
    float d;
    float *srcBase;
    float *dstBase;
    float *ptkBase;
    float *begRow;
    float *dstBase_i;
    int colShift;
    float coef;
    int shift;
    int shift_i, shift_j;
    int di, dj;
    int delta;
    int matrSize;

    matrSize = width;

#ifdef TRACE_TO_FILE
    int t;
    FILE *file;

    file = fopen( "E:\\test_matrix\\comples_mult.txt", "w" );

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

    }
#endif
    tmpMatr = icvCreateVector_32f( (matrSize * (matrSize + 1)) / 2 );
    tmpMatr2 = icvCreateVector_32f( (matrSize * (matrSize + 1)) / 2 );
    srcTrans = icvCreateMatrix_32f( MAX( height, width ), width );

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

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

    icvTransposeMatrix_32f( srcMatr, width, height, srcTrans );

#ifdef TRACE_TO_FILE
    fprintf( file, "\nSource matrix\n" );
    for( t = 0; t < width * height; t++ )
    {
        fprintf( file, "srcMatr[%d] = %f\n", t, srcMatr[t] );

    }

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

    sum = 0;

    pMatr = tmpMatr;

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


#ifdef TRACE_TO_FILE
    fprintf( file, "\nMult matrix\n" );
    for( t = 0; t < (matrSize * (matrSize + 1)) / 2; t++ )
    {
        fprintf( file, "tmpMatr[%d] = %f\n", t, tmpMatr[t] );
    }
#endif
    /* 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 += square_32f( *pti++ );
        }
        d = (*ps++) - sum;
        *pti++ = (float) sqrt( (double)d );
    }

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

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


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

    dstBase = tmpMatr;
    shift = 2;

    for( i = 0; i < matrSize; i++ )
    {
        *dstBase = 1.0;
        dstBase += shift;
        shift++;
    }

#ifdef TRACE_TO_FILE
    fprintf( file, "\nIdentical matrix (tmpMatr)\n" );
    for( t = 0; t < (matrSize * (matrSize + 1)) / 2; t++ )
    {
        fprintf( file, "tmpMatr[%d] = %f\n", t, tmpMatr[t] );
    }
#endif

    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.0f / (*srcBase++);

        while( k-- )
        {
            *ptkBase++ *= coef;
        }
    }

#ifdef TRACE_TO_FILE
    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] );
    }
#endif

    /* 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++;
    }

#ifdef TRACE_TO_FILE
    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] );
    }
#endif

    /* 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++ = (float)icvDotProduct_32f( 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];
        }
    }

#ifdef TRACE_TO_FILE
    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" );
    }
#endif


    /* 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++ = (float)icvDotProduct_32f( pti, ptj, matrSize );
            ptj += width;
        }

        pti += matrSize;
    }

#ifdef TRACE_TO_FILE
    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" );
    }