cvcalccovarmatrix.cpp.svn-base

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

                        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 * nObjects + j] = covarMatrix[j * nObjects + i] = w;
            }
        }
    }                           /* else */

    return CV_NO_ERR;
}

/*======================== end of icvCalcCovarMatrixEx_8u32fR ===========================*/


#define ICV_DOT_PRODUCT_CASE( flavor, srctype, avgtype, load_macro )                    \
static CvStatus CV_STDCALL                                                              \
icvDotProductShifted_##flavor##_C1R( const srctype* vec1, int vecstep1,                 \
                                     const srctype* vec2, int vecstep2,                 \
                                     const avgtype* avg, int avgstep,                   \
                                     CvSize size, double* _result )                     \
{                                                                                       \
    double result = 0;                                                                  \
                                                                                        \
    for( ; size.height--; (char*&)vec1 += vecstep1,                                     \
                          (char*&)vec2 += vecstep2,                                     \
                          (char*&)avg += avgstep )                                      \
    {                                                                                   \
        int x;                                                                          \
        for( x = 0; x <= size.width - 4; x += 4 )                                       \
            result += (load_macro(vec1[x]) - avg[x])*(load_macro(vec2[x]) - avg[x]) +   \
                (load_macro(vec1[x+1]) - avg[x+1])*(load_macro(vec2[x+1]) - avg[x+1]) + \
                (load_macro(vec1[x+2]) - avg[x+2])*(load_macro(vec2[x+2]) - avg[x+2]) + \
                (load_macro(vec1[x+3]) - avg[x+3])*(load_macro(vec2[x+3]) - avg[x+3]);  \
        for( ; x < size.width; x++ )                                                    \
            result += (load_macro(vec1[x]) - avg[x])*(load_macro(vec2[x]) - avg[x]);    \
    }                                                                                   \
                                                                                        \
    *_result = result;                                                                  \
    return CV_OK;                                                                       \
}


ICV_DOT_PRODUCT_CASE( 8u32f, uchar, float, CV_8TO32F )
ICV_DOT_PRODUCT_CASE( 8u64f, uchar, double, CV_8TO32F )
ICV_DOT_PRODUCT_CASE( 8s32f, char, float, CV_8TO32F )
ICV_DOT_PRODUCT_CASE( 8s64f, char, double, CV_8TO32F )
ICV_DOT_PRODUCT_CASE( 16s32f, short, float, CV_NOP )
ICV_DOT_PRODUCT_CASE( 16s64f, short, double, CV_NOP )
ICV_DOT_PRODUCT_CASE( 32f, float, float, CV_NOP )
ICV_DOT_PRODUCT_CASE( 32f64f, float, double, CV_NOP )
ICV_DOT_PRODUCT_CASE( 64f, double, double, CV_NOP )


static void  icvInitDotProductShiftedTable( CvFuncTable* tabfl, CvFuncTable* tabdb )
{
    tabfl->fn_2d[CV_8U] = (void*)icvDotProductShifted_8u32f_C1R;
    tabfl->fn_2d[CV_8S] = (void*)icvDotProductShifted_8s32f_C1R;
    tabfl->fn_2d[CV_16S] = (void*)icvDotProductShifted_16s32f_C1R;
    tabfl->fn_2d[CV_32F] = (void*)icvDotProductShifted_32f_C1R;

    tabdb->fn_2d[CV_8U] = (void*)icvDotProductShifted_8u64f_C1R;
    tabdb->fn_2d[CV_8S] = (void*)icvDotProductShifted_8s64f_C1R;
    tabdb->fn_2d[CV_16S] = (void*)icvDotProductShifted_16s64f_C1R;
    tabdb->fn_2d[CV_32F] = (void*)icvDotProductShifted_32f64f_C1R;
    tabdb->fn_2d[CV_64F] = (void*)icvDotProductShifted_64f_C1R;
}


typedef struct vec_data
{
    void* ptr;
    int step;
}
vec_data;


CV_IMPL void
cvCalcCovarMatrix( const CvArr** vecarr, CvArr* covarr, CvArr* avgarr )
{
    static CvFuncTable acc_tab[2], dot_tab[2];
    static int inittab = 0;
    vec_data* vecdata = 0;
    
    CV_FUNCNAME( "cvCalcCovarMatrix" );

    __BEGIN__;

    CvMat covstub, *cov = (CvMat*)covarr;
    CvMat avgstub, *avg = (CvMat*)avgarr;
    CvSize srcsize, contsize;
    int srctype = 0, dsttype;
    int i, j, count;
    CvFunc2D_2A acc_func = 0;
    CvFunc2D_3A1P dot_func = 0;
    int cont_flag = 1;

    if( !inittab )
    {
        icvInitAccTable( acc_tab + 0, acc_tab + 1, 0 );
        icvInitDotProductShiftedTable( dot_tab + 0, dot_tab + 1 );
        inittab = 1;
    }

    if( !vecarr )
        CV_ERROR( CV_StsNullPtr, "NULL vec pointer" );

    CV_CALL( cov = cvGetMat( cov, &covstub ));
    CV_CALL( avg = cvGetMat( avg, &avgstub ));

    if( !CV_ARE_TYPES_EQ( cov, avg ))
        CV_ERROR( CV_StsUnmatchedFormats,
        "Covariation matrix and average vector should have the same types" );

    dsttype = CV_MAT_TYPE( cov->type );
    if( dsttype != CV_32FC1 && dsttype != CV_64FC1 )
        CV_ERROR( CV_StsUnsupportedFormat, "Covariation matrix must be 32fC1 or 64fC1" );

    if( cov->rows != cov->cols )
        CV_ERROR( CV_StsBadSize, "Covariation matrix must be square" );

    count = cov->rows;
    CV_CALL( vecdata = (vec_data*)cvAlloc( count*sizeof(vecdata[0])));

    cvZero( avg );
    srcsize = icvGetMatSize( avg );

    contsize.width = srcsize.width * srcsize.height;
    contsize.height = 1;

    for( i = 0; i < count; i++ )
    {
        CvMat vecstub, *vec = (CvMat*)vecarr[i];
        if( !CV_IS_MAT(vec) )
        {
            CV_CALL( vec = cvGetMat( vec, &vecstub ));
        }

        if( !CV_ARE_SIZES_EQ( vec, avg ))
            CV_ERROR( CV_StsUnmatchedSizes,
            "All input vectors and average vector must have the same size" );

        vecdata[i].ptr = vec->data.ptr;
        vecdata[i].step = vec->step;

        if( i == 0 )
        {
            srctype = CV_MAT_TYPE( vec->type );
            if( CV_MAT_CN( srctype ) != 1 )
                CV_ERROR( CV_BadNumChannels, "All vectors must have a single channel" );
            acc_func = (CvFunc2D_2A)
                acc_tab[dsttype == CV_64FC1].fn_2d[CV_MAT_DEPTH(srctype)];
            dot_func = (CvFunc2D_3A1P)
                dot_tab[dsttype == CV_64FC1].fn_2d[CV_MAT_DEPTH(srctype)];
            if( !acc_func || !dot_func )
                CV_ERROR( CV_StsUnsupportedFormat,
                "The input vectors' type is unsupported" );
        }
        else if( CV_MAT_TYPE(vec->type) != srctype )
            CV_ERROR( CV_StsUnmatchedFormats,
            "All input vectors must have the same type" );

        if( CV_IS_MAT_CONT( avg->type & vec->type ))
        {
            acc_func( vec->data.ptr, CV_STUB_STEP,
                      avg->data.ptr, CV_STUB_STEP, contsize );
        }
        else
        {
            acc_func( vec->data.ptr, vec->step,
                      avg->data.ptr, avg->step, srcsize );
            cont_flag = 0;
        }
    }

    cvScale( avg, avg, 1./count );

    for( i = 0; i < count; i++ )
    {
        int a, b, delta;
        if( !(i & 1) )
            a = 0, b = i+1, delta = 1;
        else
            a = i, b = -1, delta = -1;

        for( j = a; j != b; j += delta )
        {
            double result = 0;
            if( cont_flag )
            {
                dot_func( vecdata[i].ptr, CV_STUB_STEP, vecdata[j].ptr, CV_STUB_STEP,
                          avg->data.ptr, CV_STUB_STEP, contsize, &result );
            }
            else
            {
                dot_func( vecdata[i].ptr, vecdata[i].step,
                          vecdata[j].ptr, vecdata[j].step,
                          avg->data.ptr, avg->step, srcsize, &result );
            }
            if( dsttype == CV_64FC1 )
            {
                ((double*)(cov->data.ptr + i*cov->step))[j] =
                ((double*)(cov->data.ptr + j*cov->step))[i] = result;
            }
            else
            {
                ((float*)(cov->data.ptr + i*cov->step))[j] =
                ((float*)(cov->data.ptr + j*cov->step))[i] = (float)result;
            }
        }
    }

    __END__;

    cvFree( (void**)&vecdata );
}

/* End of file. */