cvmatmul.cpp.svn-base

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

    if( shift )
    {
        if( !CV_IS_MAT( shift ))
        {
            int coi = 0;
            CV_CALL( shift = cvGetMat( shift, &sstub, &coi ));

            if( coi != 0 )
                CV_ERROR( CV_BadCOI, "" );
        }

        if( shift->height == 1 && (CV_MAT_CN( shift->type ) != 1 ||
            CV_MAT_CN( shift->type ) * shift->width == cn) )
        {
            if( CV_MAT_DEPTH( shift->type ) == CV_64F )
            {
                for( i = 0; i < cn; i++ )
                    buffer[i*cn + cn-1] += shift->data.db[i];
            }
            else if( CV_MAT_DEPTH( shift->type ) == CV_32F )
            {
                for( i = 0; i < cn; i++ )
                    buffer[i*cn + cn-1] += shift->data.fl[i];
            }
            else
            {
                CV_ERROR( CV_StsUnsupportedFormat,
                    "Shift vector must be 32f or 64f" );
            }
        }
        else if( CV_MAT_CN( shift->type ) == 1 && shift->height == cn )
        {
            if( CV_MAT_DEPTH( shift->type ) == CV_64F )
            {
                for( i = 0; i < cn; i++ )
                    buffer[i*cn + cn-1] += *(double*)(shift->data.ptr + shift->step*i);
            }
            if( CV_MAT_DEPTH( shift->type ) == CV_32F )
            {
                for( i = 0; i < cn; i++ )
                    buffer[i*cn + cn-1] += *(float*)(shift->data.ptr + shift->step*i);
            }
            else
            {
                CV_ERROR( CV_StsUnsupportedFormat,
                    "Shift vector must be 32f or 64f" );
            }
        }
        else
        {
            CV_ERROR( CV_StsUnmatchedSizes,
                "Shift (if present) must have a total length equal to number "
                "of channels in the processed arrays" );
        }
    }

    if( cn > 1 )
    {
        CvFunc2D_2A1P func = (CvFunc2D_2A1P)(mmuladds_tab.fn_2d[type]);
        CvSize size;

        if( !func )
            CV_ERROR( CV_StsUnsupportedFormat, "" );

        if( !seq )
        {
            size = icvGetMatSize( src );

            if( CV_IS_MAT_CONT( src->type & dst->type ))
            {
                size.width *= size.height;
                size.height = 1;
            }

            IPPI_CALL( func( src->data.ptr, src->step, dst->data.ptr,
                             dst->step, size, buffer));
        }
        else
        {
            CvSeqBlock* block = seq->first;

            if( block )
            {
                do
                {
                    IPPI_CALL( func( block->data, CV_STUB_STEP, block->data,
                               CV_STUB_STEP, cvSize( block->count, 1 ), buffer ));
                    block = block->next;
                }
                while( block != seq->first );
            }
        }
    }
    else // the simplest variant
    {
        CV_CALL( cvCvtScale( src, dst, buffer[0], buffer[1] ));
    }

    __END__;
}


/****************************************************************************************\
*                                       cvMulAddS                                        *
\****************************************************************************************/

#define  ICV_DEF_MULADDC_CASE_C1( arrtype, temptype, src1, src2, dst, len )     \
{                                                                               \
    int i;                                                                      \
                                                                                \
    for( i = 0; i <= (len) - 4; i += 4 )                                        \
    {                                                                           \
        temptype t0 = (src1)[i]*s0 + (src2)[i];                                 \
        temptype t1 = (src1)[i+1]*s0 + (src2)[i+1];                             \
                                                                                \
        (dst)[i] = (arrtype)t0;                                                 \
        (dst)[i+1] = (arrtype)t1;                                               \
                                                                                \
        t0 = (src1)[i+2]*s0 + (src2)[i+2];                                      \
        t1 = (src1)[i+3]*s0 + (src2)[i+3];                                      \
                                                                                \
        (dst)[i+2] = (arrtype)t0;                                               \
        (dst)[i+3] = (arrtype)t1;                                               \
    }                                                                           \
                                                                                \
    for( ; i < (len); i++ )                                                     \
    {                                                                           \
        temptype t0 = (src1)[i]*s0 + (src2)[i];                                 \
        (dst)[i] = (arrtype)t0;                                                 \
    }                                                                           \
}


#define  ICV_DEF_MULADDC_CASE_C2( arrtype, temptype, src1, src2, dst, len )     \
{                                                                               \
    int i;                                                                      \
                                                                                \
    for( i = 0; i <= (len) - 4; i += 4 )                                        \
    {                                                                           \
        temptype t0 = (src1)[i]*s0 - (src1)[i+1]*s1 + (src2)[i];                \
        temptype t1 = (src1)[i]*s1 + (src1)[i+1]*s0 + (src2)[i+1];              \
                                                                                \
        (dst)[i] = (arrtype)t0;                                                 \
        (dst)[i+1] = (arrtype)t1;                                               \
                                                                                \
        t0 = (src1)[i+2]*s0 - (src1)[i+3]*s1 + (src2)[i+2];                     \
        t1 = (src1)[i+2]*s1 + (src1)[i+3]*s0 + (src2)[i+3];                     \
                                                                                \
        (dst)[i+2] = (arrtype)t0;                                               \
        (dst)[i+3] = (arrtype)t1;                                               \
    }                                                                           \
                                                                                \
    for( ; i < (len); i += 2 )                                                  \
    {                                                                           \
        temptype t0 = (src1)[i]*s0 - (src1)[i+1]*s1 + (src2)[i];                \
        temptype t1 = (src1)[i]*s1 + (src1)[i+1]*s0 + (src2)[i+1];              \
                                                                                \
        (dst)[i] = (arrtype)t0;                                                 \
        (dst)[i+1] = (arrtype)t1;                                               \
    }                                                                           \
}


#define  ICV_DEF_MULADDS_FUNC( flavor, arrtype, scalartype, entry, cn )     \
IPCVAPI_IMPL( CvStatus,                                                     \
icvMulAddC_##flavor,( const arrtype* src1, int srcstep1,                    \
                      const arrtype* src2, int srcstep2,                    \
                      arrtype* dst, int dststep, CvSize size,               \
                      const scalartype* scalar ))                           \
{                                                                           \
    entry(scalartype);                                                      \
    size.width *= (cn);                                                     \
                                                                            \
    for( ; size.height--; (char*&)src1 += srcstep1,                         \
                          (char*&)src2 += srcstep2,                         \
                          (char*&)dst += dststep )                          \
    {                                                                       \
        ICV_DEF_MULADDC_CASE_C##cn( arrtype, scalartype, src1, src2,        \
                                    dst, size.width )                       \
    }                                                                       \
                                                                            \
    return CV_OK;                                                           \
}


ICV_DEF_MULADDS_FUNC( 32f_C1R, float, double, ICV_UN_ENTRY_C1, 1 )
ICV_DEF_MULADDS_FUNC( 32f_C2R, float, double, ICV_UN_ENTRY_C2, 2 )
ICV_DEF_MULADDS_FUNC( 64f_C1R, double, double, ICV_UN_ENTRY_C1, 1 )
ICV_DEF_MULADDS_FUNC( 64f_C2R, double, double, ICV_UN_ENTRY_C2, 2 )


static void
icvInitMulAddCTable( CvBigFuncTable* tab )
{
    tab->fn_2d[CV_32FC1] = (void*)icvMulAddC_32f_C1R;
    tab->fn_2d[CV_32FC2] = (void*)icvMulAddC_32f_C2R;
    tab->fn_2d[CV_64FC1] = (void*)icvMulAddC_64f_C1R;
    tab->fn_2d[CV_64FC2] = (void*)icvMulAddC_64f_C2R;
}


CV_IMPL void
cvScaleAdd( const CvArr* srcarr1, CvScalar scale,
            const CvArr* srcarr2, CvArr* dstarr )
{
    static CvBigFuncTable muladds_tab;
    static int inittab = 0;
    
    CV_FUNCNAME( "cvScaleAdd" );

    __BEGIN__;

    CvMat stub1, *src1 = (CvMat*)srcarr1;
    CvMat stub2, *src2 = (CvMat*)srcarr2;
    CvMat stub, *dst = (CvMat*)dstarr;
    CvSize size;
    int type;

    if( !CV_IS_MAT( src1 ) || !CV_IS_MAT(src2) || !CV_IS_MAT(dst))
    {
        int coi1 = 0, coi2 = 0, coi3 = 0;
        CV_CALL( src1 = cvGetMat( src1, &stub1, &coi1 ));
        CV_CALL( src2 = cvGetMat( src2, &stub2, &coi2 ));
        CV_CALL( dst = cvGetMat( dst, &stub, &coi3 ));

        if( coi1 + coi2 + coi3 != 0 )
            CV_ERROR( CV_BadCOI, "" );
    }

    if( !CV_ARE_TYPES_EQ( src1, dst ) || !CV_ARE_TYPES_EQ( src2, dst ))
        CV_ERROR( CV_StsUnmatchedFormats, "" );

    if( !CV_ARE_SIZES_EQ( src1, dst ) || !CV_ARE_SIZES_EQ( src2, dst ))
        CV_ERROR( CV_StsUnmatchedSizes, "" );

    type = CV_MAT_TYPE( src1->type );
    size = icvGetMatSize( src1 );

    if( CV_IS_MAT_CONT( src1->type & src2->type & dst->type ))
    {
        size.width *= size.height;

        if( size.width <= CV_MAX_INLINE_MAT_OP_SIZE )
        {
            if( type == CV_32FC1 )
            {
                float* mA = src1->data.fl;
                float* mB = src2->data.fl;
                float* mC = dst->data.fl;

                do
                {
                    mC[size.width - 1] = (float)(mA[size.width - 1]*scale.val[0] +
                                         mB[size.width - 1]);
                }
                while( --size.width );

                EXIT;
            }

            if( type == CV_64FC1 )
            {
                double* mA = src1->data.db;
                double* mB = src2->data.db;
                double* mC = dst->data.db;

                do
                {
                    mC[size.width - 1] = mA[size.width - 1]*scale.val[0] +
                                         mB[size.width - 1];
                }
                while( --size.width );

                EXIT;
            }
        }

        size.height = 1;
    }

    if( !inittab )
    {
        icvInitMulAddCTable( &muladds_tab );
        inittab = 1;
    }

    {
        CvFunc2D_3A1P func = (CvFunc2D_3A1P)(muladds_tab.fn_2d[type]);

        if( !func )
            CV_ERROR( CV_StsUnsupportedFormat, "" );

        IPPI_CALL( func( src1->data.ptr, src1->step, src2->data.ptr, src2->step,
                         dst->data.ptr, dst->step, size, scale.val ));
    }

    CV_CHECK_NANS( dst );

    __END__;
}


/****************************************************************************************\
*                                        cvMahalanobis                                   *
\****************************************************************************************/

CV_IMPL double
cvMahalanobis( const CvArr* srcAarr, const CvArr* srcB, CvArr* mat )
{
    uchar* buffer = 0;
    int local_alloc = 0;
    double dist = 0;

    CV_FUNCNAME( "cvMahalanobis" );

    __BEGIN__;