cvmatmul.cpp.svn-base

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

            src1->height != dst->height ||
            src2->width != dst->width )
            CV_ERROR( CV_StsUnmatchedSizes, "" );
        break;
    case 1:
        if( src1->height != src2->height ||
            src1->width != dst->height ||
            src2->width != dst->width )
            CV_ERROR( CV_StsUnmatchedSizes, "" );
        break;
    case 2:
        if( src1->width != src2->width ||
            src1->height != dst->height ||
            src2->height != dst->width )
            CV_ERROR( CV_StsUnmatchedSizes, "" );
        break;
    case 3:
        if( src1->height != src2->width ||
            src1->width != dst->height ||
            src2->height != dst->width )
            CV_ERROR( CV_StsUnmatchedSizes, "" );
        break;
    }
    
    type = CV_MAT_TYPE( src1->type );

    // general case
    {
        CvGEMMFunc func;
        CvSize size = icvGetMatSize( src1 );
        CvSize dstsize = icvGetMatSize( dst );
        int buf_size = dstsize.width*dstsize.height*icvPixSize[type];
        CvMat tmat, *tdst = dst;
        
        if( !inittab )
        {
            icvInitGEMMTable( &gemm_tab );
            inittab = 1;
        }

        if( dst->data.ptr == src1->data.ptr || dst->data.ptr == src2->data.ptr ||
            (tABC & 4) != 0 && src3 != 0 && dst->data.ptr == src3->data.ptr )
        {
            if( size.width <= CV_MAX_LOCAL_MAT_SIZE )
            {
                buffer = (uchar*)alloca( buf_size + 8 );
                buffer = (uchar*)icvAlignPtr( buffer, 8 );
                local_alloc = 1;
            }
            else
            {
                CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
            }

            CV_CALL( cvInitMatHeader( &tmat, dstsize.height,
                                      dstsize.width, type, buffer ));
            tdst = &tmat;
        }

        func = (CvGEMMFunc)(gemm_tab.fn_2d[CV_MAT_DEPTH(type)]);
        if( !func )
            CV_ERROR( CV_StsUnsupportedFormat, "" );

        IPPI_CALL( func( src1->data.ptr, src1->step, src2->data.ptr, src2->step, alpha,
                         src3->data.ptr, src3->step, beta, tdst->data.ptr, tdst->step,
                         size, dstsize, tABC ));

        if( tdst != dst )
        {
            CV_CALL( cvCopy( tdst, dst ));
        }
    }

    CV_CHECK_NANS( dst );

    __END__;

    if( buffer && !local_alloc )
        cvFree( (void**)&buffer );
}


/****************************************************************************************\
*                                        cvMatMulAddS                                    *
\****************************************************************************************/


#define  ICV_DEF_MMULADDC_CASE_C2( arrtype, temptype, src, dst, mat,        \
                                  _cast_macro1_, _cast_macro2_ )            \
{                                                                           \
    temptype t0, t1;                                                        \
                                                                            \
    t0 = _cast_macro1_((mat)[0]*(src)[0] + (mat)[1]*(src)[1] + (mat)[2]);   \
    t1 = _cast_macro1_((mat)[3]*(src)[0] + (mat)[4]*(src)[1] + (mat)[5]);   \
                                                                            \
    (dst)[0] = _cast_macro2_(t0);                                           \
    (dst)[1] = _cast_macro2_(t1);                                           \
}


#define  ICV_DEF_MMULADDC_CASE_C3( arrtype, temptype, src, dst, mat,        \
                                  _cast_macro1_, _cast_macro2_ )            \
{                                                                           \
    temptype t0, t1, t2;                                                    \
                                                                            \
    t0 = _cast_macro1_((mat)[0]*(src)[0] + (mat)[1]*(src)[1] +              \
                       (mat)[2]*(src)[2] + (mat)[3]);                       \
    t1 = _cast_macro1_((mat)[4]*(src)[0] + (mat)[5]*(src)[1] +              \
                       (mat)[6]*(src)[2] + (mat)[7]);                       \
    t2 = _cast_macro1_((mat)[8]*(src)[0] + (mat)[9]*(src)[1] +              \
                       (mat)[10]*(src)[2] + (mat)[11]);                     \
                                                                            \
    (dst)[0] = _cast_macro2_(t0);                                           \
    (dst)[1] = _cast_macro2_(t1);                                           \
    (dst)[2] = _cast_macro2_(t2);                                           \
}


#define  ICV_DEF_MMULADDC_CASE_C4( arrtype, temptype, src, dst, mat,        \
                                  _cast_macro1_, _cast_macro2_ )            \
{                                                                           \
    temptype t0, t1, t2, t3;                                                \
                                                                            \
    t0 = _cast_macro1_((mat)[0]*(src)[0] + (mat)[1]*(src)[1] +              \
                       (mat)[2]*(src)[2] + (mat)[3]*(src)[3] + (mat)[4]);   \
    t1 = _cast_macro1_((mat)[5]*(src)[0] + (mat)[6]*(src)[1] +              \
                       (mat)[7]*(src)[2] + (mat)[8]*(src)[3] + (mat)[9]);   \
    t2 = _cast_macro1_((mat)[10]*(src)[0] + (mat)[11]*(src)[1] +            \
                       (mat)[12]*(src)[2] + (mat)[13]*(src)[3] + (mat)[14]);\
    t3 = _cast_macro1_((mat)[15]*(src)[0] + (mat)[16]*(src)[1] +            \
                       (mat)[17]*(src)[2] + (mat)[18]*(src)[3] + (mat)[19]);\
                                                                            \
    (dst)[0] = _cast_macro2_(t0);                                           \
    (dst)[1] = _cast_macro2_(t1);                                           \
    (dst)[2] = _cast_macro2_(t2);                                           \
    (dst)[3] = _cast_macro2_(t3);                                           \
}



#define  ICV_DEF_MATMULADDS_FUNC( flavor, arrtype, scalartype, temptype,    \
                                  _cast_macro1_, _cast_macro2_, cn  )       \
IPCVAPI_IMPL( CvStatus,                                                     \
icvMatMulAddC_##flavor,( const arrtype* src, int srcstep,                   \
                         arrtype* dst, int dststep, CvSize size,            \
                         const scalartype* mat ))                           \
{                                                                           \
    size.width *= (cn);                                                     \
                                                                            \
    for( ; size.height--; (char*&)src += srcstep, (char*&)dst += dststep )  \
    {                                                                       \
        int i;                                                              \
        for( i = 0; i < size.width; i += (cn) )                             \
        {                                                                   \
            ICV_DEF_MMULADDC_CASE_C##cn( arrtype, temptype, src + i,        \
                         dst + i, mat, _cast_macro1_, _cast_macro2_ )       \
        }                                                                   \
    }                                                                       \
                                                                            \
    return CV_OK;                                                           \
}


ICV_DEF_MATMULADDS_FUNC( 8u_C2R, uchar, double, int, cvRound, CV_CAST_8U, 2 )
ICV_DEF_MATMULADDS_FUNC( 8u_C3R, uchar, double, int, cvRound, CV_CAST_8U, 3 )
ICV_DEF_MATMULADDS_FUNC( 8u_C4R, uchar, double, int, cvRound, CV_CAST_8U, 4 )

ICV_DEF_MATMULADDS_FUNC( 32s_C2R, int, double, int, cvRound, CV_NOP, 2 )
ICV_DEF_MATMULADDS_FUNC( 32s_C3R, int, double, int, cvRound, CV_NOP, 3 )
ICV_DEF_MATMULADDS_FUNC( 32s_C4R, int, double, int, cvRound, CV_NOP, 4 )

ICV_DEF_MATMULADDS_FUNC( 32f_C2R, float, double, double, CV_NOP, CV_CAST_32F, 2 )
ICV_DEF_MATMULADDS_FUNC( 32f_C3R, float, double, double, CV_NOP, CV_CAST_32F, 3 )
ICV_DEF_MATMULADDS_FUNC( 32f_C4R, float, double, double, CV_NOP, CV_CAST_32F, 4 )

ICV_DEF_MATMULADDS_FUNC( 64f_C2R, double, double, double, CV_NOP, CV_CAST_64F, 2 )
ICV_DEF_MATMULADDS_FUNC( 64f_C3R, double, double, double, CV_NOP, CV_CAST_64F, 3 )
ICV_DEF_MATMULADDS_FUNC( 64f_C4R, double, double, double, CV_NOP, CV_CAST_64F, 4 )


static void
icvInitMatMulAddCTable( CvBigFuncTable* tab )
{
    tab->fn_2d[CV_8UC2] = (void*)icvMatMulAddC_8u_C2R;
    tab->fn_2d[CV_8UC3] = (void*)icvMatMulAddC_8u_C3R;
    tab->fn_2d[CV_8UC4] = (void*)icvMatMulAddC_8u_C4R;

    tab->fn_2d[CV_32SC2] = (void*)icvMatMulAddC_32s_C2R;
    tab->fn_2d[CV_32SC3] = (void*)icvMatMulAddC_32s_C3R;
    tab->fn_2d[CV_32SC4] = (void*)icvMatMulAddC_32s_C4R;

    tab->fn_2d[CV_32FC2] = (void*)icvMatMulAddC_32f_C2R;
    tab->fn_2d[CV_32FC3] = (void*)icvMatMulAddC_32f_C3R;
    tab->fn_2d[CV_32FC4] = (void*)icvMatMulAddC_32f_C4R;

    tab->fn_2d[CV_64FC2] = (void*)icvMatMulAddC_64f_C2R;
    tab->fn_2d[CV_64FC3] = (void*)icvMatMulAddC_64f_C3R;
    tab->fn_2d[CV_64FC4] = (void*)icvMatMulAddC_64f_C4R;
}


CV_IMPL void
cvMatMulAddS( const CvArr* srcarr, CvArr* dstarr,
              const CvMat* transform, const CvMat* shiftvec )
{
    static CvBigFuncTable mmuladds_tab;
    static int inittab = 0;
    
    double buffer[20];
    
    CV_FUNCNAME( "cvMatMulAddS" );

    __BEGIN__;

    CvMat stub1, *src = (CvMat*)srcarr;
    CvMat stub, *dst = (CvMat*)dstarr;
    CvMat rstub, *rot = (CvMat*)transform;
    CvMat sstub, *shift = (CvMat*)shiftvec;
    CvSeq* seq = 0;
    int i, j, type, cn;

    if( !inittab )
    {
        icvInitMatMulAddCTable( &mmuladds_tab );
        inittab = 1;
    }

    if( !CV_IS_MAT( src ))
    {
        if( CV_IS_SEQ( src ))
        {
            seq = (CvSeq*)src;
            if( icvPixSize[CV_SEQ_ELTYPE(seq)] != seq->elem_size )
                CV_ERROR( CV_StsUnsupportedFormat,
                "Unsupported type of sequence elements" );
            if( dst != src && dst != 0 )
                CV_ERROR( CV_StsBadArg,
                "For sequences only inplace mode is supported" );
        }
        else
        {
            int coi = 0;
            CV_CALL( src = cvGetMat( src, &stub1, &coi ));

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

    if( dst == 0 )
        dst = src;

    if( dst != src )
    {
        if( !CV_IS_MAT( dst ))
        {
            int coi = 0;
            CV_CALL( dst = cvGetMat( dst, &stub, &coi ));

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

        if( !CV_ARE_TYPES_EQ( src, dst ))
            CV_ERROR( CV_StsUnmatchedFormats, "" );

        if( !CV_ARE_SIZES_EQ( src, dst ))
            CV_ERROR( CV_StsUnmatchedSizes, "" );
    }

    type = CV_MAT_TYPE( src->type );
    cn = CV_MAT_CN( type );

    if( !CV_IS_MAT( rot ))
    {
        int coi = 0;
        CV_CALL( rot = cvGetMat( rot, &rstub, &coi ));

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

    if( rot->height != cn )
        CV_ERROR( CV_StsBadSize,
        "The height of transform matrix must be equal to number of channels" );

    if( rot->width == cn + 1 || rot->width == cn )
    {
        if( CV_MAT_TYPE( rot->type ) == CV_64FC1 )
        {
            for( i = 0; i < cn; i++ )
            {
                buffer[i*cn + cn-1] = 0;

                for( j = 0; j < rot->width; j++ )
                    buffer[i*cn + j] = ((double*)(rot->data.ptr + rot->step*i))[j];
            }
        }
        else if( CV_MAT_TYPE( rot->type ) == CV_32FC1 )
        {
            for( i = 0; i < cn; i++ )
            {
                buffer[i*cn + cn-1] = 0;

                for( j = 0; j < rot->width; j++ )
                    buffer[i*cn + j] = ((float*)(rot->data.ptr + rot->step*i))[j];
            }
        }
        else
        {
            CV_ERROR( CV_StsUnsupportedFormat, "Rotation matrix must be 32fC1 or 64fC1" );
        }
    }
    else
    {
        CV_ERROR( CV_StsUnmatchedSizes, "If the source array has <cn> channels, "
           "the transformation matrix must have <cn> x <cn>+1 or <cn> x <cn> size" );
    }