cvmatrix.cpp.svn-base

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

                    float t[3];
                    d = 1./d;

                    t[0] = (float)(d*
                           (bf(0)*(Sf(1,1)*Sf(2,2) - Sf(1,2)*Sf(2,1)) -
                            Sf(0,1)*(bf(1)*Sf(2,2) - Sf(1,2)*bf(2)) +
                            Sf(0,2)*(bf(1)*Sf(2,1) - Sf(1,1)*bf(2))));

                    t[1] = (float)(d*
                           (Sf(0,0)*(bf(1)*Sf(2,2) - Sf(1,2)*bf(2)) -
                            bf(0)*(Sf(1,0)*Sf(2,2) - Sf(1,2)*Sf(2,0)) +
                            Sf(0,2)*(Sf(1,0)*bf(2) - bf(1)*Sf(2,0))));

                    t[2] = (float)(d*
                           (Sf(0,0)*(Sf(1,1)*bf(2) - bf(1)*Sf(2,1)) -
                            Sf(0,1)*(Sf(1,0)*bf(2) - bf(1)*Sf(2,0)) +
                            bf(0)*(Sf(1,0)*Sf(2,1) - Sf(1,1)*Sf(2,0))));

                    Df(0,0) = t[0];
                    Df(1,0) = t[1];
                    Df(2,0) = t[2];
                }
                else
                    result = 0;
            }
            else
            {
                double d = det3(Sd);
                if( d != 0. )
                {
                    double t[9];

                    d = 1./d;
                    
                    t[0] = ((Sd(1,1) * Sd(2,2) - Sd(1,2) * Sd(2,1))*bd(0) +
                            (Sd(0,2) * Sd(2,1) - Sd(0,1) * Sd(2,2))*bd(1) +
                            (Sd(0,1) * Sd(1,2) - Sd(0,2) * Sd(1,1))*bd(2))*d;

                    t[1] = ((Sd(1,2) * Sd(2,0) - Sd(1,0) * Sd(2,2))*bd(0) +
                            (Sd(0,0) * Sd(2,2) - Sd(0,2) * Sd(2,0))*bd(1) +
                            (Sd(0,2) * Sd(1,0) - Sd(0,0) * Sd(1,2))*bd(2))*d;

                    t[2] = ((Sd(1,0) * Sd(2,1) - Sd(1,1) * Sd(2,0))*bd(0) +
                            (Sd(0,1) * Sd(2,0) - Sd(0,0) * Sd(2,1))*bd(1) +
                            (Sd(0,0) * Sd(1,1) - Sd(0,1) * Sd(1,0))*bd(2))*d;

                    Dd(0,0) = t[0];
                    Dd(1,0) = t[1];
                    Dd(2,0) = t[2];
                }
                else
                    result = 0;
            }
        }
        else
        {
            assert( src->width == 1 );

            if( type == CV_32FC1 )
            {
                double d = Sf(0,0);
                if( d != 0. )
                    Df(0,0) = (float)(bf(0)/d);
                else
                    result = 0;
            }
            else
            {
                double d = Sd(0,0);
                if( d != 0. )
                    Dd(0,0) = (bd(0)/d);
                else
                    result = 0;
            }
        }
    }
    else
    {
        CvLUDecompFunc decomp_func;
        CvLUBackFunc back_func;
        CvSize size = icvGetMatSize( src );
        CvSize dstsize = icvGetMatSize( dst );
        int buf_size = size.width*size.height*icvPixSize[type];
        double d = 0;
        CvMat tmat;
        
        if( !lu_inittab )
        {
            icvInitLUTable( &lu_decomp_tab, &lu_back_tab );
            lu_inittab = 1;
        }

        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, size.height, size.width, type, buffer ));
        CV_CALL( cvCopy( src, &tmat ));
        
        if( src2->data.ptr != dst->data.ptr )
        {
            CV_CALL( cvCopy( src2, dst ));
        }

        decomp_func = (CvLUDecompFunc)(lu_decomp_tab.fn_2d[CV_MAT_DEPTH(type)-CV_32F]);
        back_func = (CvLUBackFunc)(lu_back_tab.fn_2d[CV_MAT_DEPTH(type)-CV_32F]);
        assert( decomp_func && back_func );

        IPPI_CALL( decomp_func( tmat.data.ptr, tmat.step, size,
                                dst->data.ptr, dst->step, dstsize, &d ));

        if( d != 0 )
        {
            IPPI_CALL( back_func( tmat.data.ptr, tmat.step, size,
                                  dst->data.ptr, dst->step, dstsize ));
        }
        else
            result = 0;
    }

    if( !result )
        CV_CALL( cvSetZero( dst ));

    __END__;

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

    if( u || v || w )
    {
        cvReleaseMat( &u );
        cvReleaseMat( &v );
        cvReleaseMat( &w );
    }

    return result;
}


#define ICV_DEF_CROSS_PRODUCT_FUNC( flavor, arrtype )                                   \
IPCVAPI_IMPL( CvStatus,                                                                 \
icvCrossProduct2L_##flavor, ( const arrtype* src1, const arrtype* src2, arrtype* dst )) \
{                                                                                       \
    dst[2] = src1[0] * src2[1] - src1[1] * src2[0];                                     \
    dst[0] = src1[1] * src2[2] - src1[2] * src2[1];                                     \
    dst[1] = src1[2] * src2[0] - src1[0] * src2[2];                                     \
                                                                                        \
    return CV_NO_ERR;                                                                   \
}

ICV_DEF_CROSS_PRODUCT_FUNC( 32f, float )
ICV_DEF_CROSS_PRODUCT_FUNC( 64d, double )

CV_IMPL void
cvCrossProduct( const CvArr* srcAarr, const CvArr* srcBarr, CvArr* dstarr )
{
    CV_FUNCNAME( "cvCrossProduct" );
    
    __BEGIN__;

    CvMat stubA, *srcA = (CvMat*)srcAarr;
    CvMat stubB, *srcB = (CvMat*)srcBarr;
    CvMat dstub, *dst = (CvMat*)dstarr;
    int type;

    if( !CV_IS_MAT(srcA))
        CV_CALL( srcA = cvGetMat( srcA, &stubA ));

    type = CV_MAT_TYPE( srcA->type );

    if( !CV_IS_MAT_CONT(srcA->type) || srcA->width*srcA->height*CV_MAT_CN(type) != 3 )
        CV_ERROR( CV_StsBadArg, "All the input arrays must be continuous 3-vectors" );

    if( !srcB || !dst )
        CV_ERROR( CV_StsNullPtr, "" );

    if( srcA->type == srcB->type && srcA->type == dst->type )
    {
        if( !srcB->data.ptr || !dst->data.ptr )
            CV_ERROR( CV_StsNullPtr, "" );
    }
    else
    {
        if( !CV_IS_MAT(srcB))
            CV_CALL( srcB = cvGetMat( srcB, &stubB ));

        if( !CV_IS_MAT(dst))
            CV_CALL( dst = cvGetMat( dst, &dstub ));

        if( !CV_ARE_TYPES_EQ( srcA, srcB ) ||
            !CV_ARE_TYPES_EQ( srcB, dst ))
            CV_ERROR( CV_StsUnmatchedFormats, "" );

        if( !CV_IS_MAT_CONT( srcB->type & dst->type ))
            CV_ERROR( CV_StsBadArg, "All the input arrays must be continuous 3-vectors" );
    }

    if( !CV_ARE_SIZES_EQ( srcA, srcB ) || !CV_ARE_SIZES_EQ( srcB, dst ))
        CV_ERROR( CV_StsUnmatchedSizes, "" );

    if( CV_MAT_DEPTH(type) == CV_32F )
    {
        float* dstdata = (float*)(dst->data.ptr);
        const float* src1data = (float*)(srcA->data.ptr);
        const float* src2data = (float*)(srcB->data.ptr);
        
        dstdata[2] = src1data[0] * src2data[1] - src1data[1] * src2data[0];
        dstdata[0] = src1data[1] * src2data[2] - src1data[2] * src2data[1];
        dstdata[1] = src1data[2] * src2data[0] - src1data[0] * src2data[2];
    }
    else if( CV_MAT_DEPTH(type) == CV_64F )
    {
        double* dstdata = (double*)(dst->data.ptr);
        const double* src1data = (double*)(srcA->data.ptr);
        const double* src2data = (double*)(srcB->data.ptr);
        
        dstdata[2] = src1data[0] * src2data[1] - src1data[1] * src2data[0];
        dstdata[0] = src1data[1] * src2data[2] - src1data[2] * src2data[1];
        dstdata[1] = src1data[2] * src2data[0] - src1data[0] * src2data[2];
    }
    else
    {
        CV_ERROR( CV_StsUnsupportedFormat, "" );
    }

    CV_CHECK_NANS( srcA );
    CV_CHECK_NANS( srcB );
    CV_CHECK_NANS( dst );

    __END__;
}


CV_IMPL void
cvRodrigues( CvMat* mat, CvMat* vec, CvMat* jacobian,
             CvRodriguesType convType )
{
    CV_FUNCNAME( "cvRodrigues" );

    __BEGIN__;

    if( !CV_IS_MAT( mat ) || !CV_IS_MAT( vec ))
        CV_ERROR( CV_StsBadArg, "" );

    if( !CV_ARE_DEPTHS_EQ( mat, vec ))
        CV_ERROR( CV_StsUnmatchedFormats, "" );

    if( mat->width != 3 || mat->height != 3 )
        CV_ERROR( CV_StsBadSize, "" );

    if( vec->width*vec->height*CV_MAT_CN(vec->type) != 3 )
        CV_ERROR( CV_StsBadSize, "" );

    if( !CV_IS_MAT_CONT( mat->type & vec->type ))
        CV_ERROR( CV_BadStep, "All the input/output arrays must be conitnuous" );

    if( jacobian )
    {
        if( !CV_IS_MAT( jacobian ))
            CV_ERROR( CV_StsBadArg, "" );

        if( !CV_ARE_TYPES_EQ( mat, jacobian ))
            CV_ERROR( CV_StsUnmatchedFormats, "" );

        if( jacobian->width != 9 || jacobian->height != 3 )
            CV_ERROR( CV_StsBadSize, "" );

        if( !CV_IS_MAT_CONT( jacobian->type ))
            CV_ERROR( CV_BadStep, "All the input/output arrays must be conitnuous" );
    }

    if( CV_MAT_TYPE( mat->type ) == CV_64FC1 )
    {
        IPPI_CALL( icvRodrigues_64d( mat->data.db, vec->data.db,
                                     jacobian ? jacobian->data.db : 0, convType ));
    }
    else if( CV_MAT_TYPE( mat->type ) == CV_32FC1 )
    {
        IPPI_CALL( icvRodrigues( mat->data.fl, vec->data.fl,
                                 jacobian ? jacobian->data.fl : 0, convType ));
    }
    else
    {
        CV_ERROR( CV_StsUnsupportedFormat, "" );
    }

    CV_CHECK_NANS( mat );
    CV_CHECK_NANS( vec );
    
#ifdef  CV_CHECK_FOR_NANS    
    if( jacobian )
        CV_CHECK_NANS( jacobian );
#endif

    __END__;
}

/* End of file. */