cxsvd.cpp

opencv库在TI DM6437上的移植,目前包括两个库cv.lib和cxcore.lib的工程

                        x[j+3] = t1;
                    }

                    for( ; j < n; j++ )
                        x[j] += s*vT[j];
                }
                else
                {
                    for( j = 0; j < n; j++ )
                        x[j*ldx] += s*vT[j];
                }
            }
            else
            {
                if( b )
                {
                    memset( buffer, 0, nb*sizeof(buffer[0]));
                    icvMatrAXPY_64f( m, nb, b, ldb, uT, buffer, 0 );
                    for( j = 0; j < nb; j++ )
                        buffer[j] *= wi;
                }
                else
                {
                    for( j = 0; j < nb; j++ )
                        buffer[j] = uT[j]*wi;
                }
                icvMatrAXPY_64f( n, nb, buffer, 0, vT, x, ldx );
            }
        }
    }
}


static void
icvSVBkSb_32f( int m, int n, const float* w,
               const float* uT, int lduT,
               const float* vT, int ldvT,
               const float* b, int ldb, int nb,
               float* x, int ldx, float* buffer )
{
    float threshold = 0.f;
    int i, j, nm = MIN( m, n );

    if( !b )
        nb = m;

    for( i = 0; i < n; i++ )
        memset( x + i*ldx, 0, nb*sizeof(x[0]));

    for( i = 0; i < nm; i++ )
        threshold += w[i];
    threshold *= 2*FLT_EPSILON;

    /* vT * inv(w) * uT * b */
    for( i = 0; i < nm; i++, uT += lduT, vT += ldvT )
    {
        double wi = w[i];
        
        if( wi > threshold )
        {
            wi = 1./wi;

            if( nb == 1 )
            {
                double s = 0;
                if( b )
                {
                    if( ldb == 1 )
                    {
                        for( j = 0; j <= m - 4; j += 4 )
                            s += uT[j]*b[j] + uT[j+1]*b[j+1] + uT[j+2]*b[j+2] + uT[j+3]*b[j+3];
                        for( ; j < m; j++ )
                            s += uT[j]*b[j];
                    }
                    else
                    {
                        for( j = 0; j < m; j++ )
                            s += uT[j]*b[j*ldb];
                    }
                }
                else
                    s = uT[0];
                s *= wi;

                if( ldx == 1 )
                {
                    for( j = 0; j <= n - 4; j += 4 )
                    {
                        double t0 = x[j] + s*vT[j];
                        double t1 = x[j+1] + s*vT[j+1];
                        x[j] = (float)t0;
                        x[j+1] = (float)t1;
                        t0 = x[j+2] + s*vT[j+2];
                        t1 = x[j+3] + s*vT[j+3];
                        x[j+2] = (float)t0;
                        x[j+3] = (float)t1;
                    }

                    for( ; j < n; j++ )
                        x[j] = (float)(x[j] + s*vT[j]);
                }
                else
                {
                    for( j = 0; j < n; j++ )
                        x[j*ldx] = (float)(x[j*ldx] + s*vT[j]);
                }
            }
            else
            {
                if( b )
                {
                    memset( buffer, 0, nb*sizeof(buffer[0]));
                    icvMatrAXPY_32f( m, nb, b, ldb, uT, buffer, 0 );
                    for( j = 0; j < nb; j++ )
                        buffer[j] = (float)(buffer[j]*wi);
                }
                else
                {
                    for( j = 0; j < nb; j++ )
                        buffer[j] = (float)(uT[j]*wi);
                }
                icvMatrAXPY_32f( n, nb, buffer, 0, vT, x, ldx );
            }
        }
    }
}


CV_IMPL  void
cvSVD( CvArr* aarr, CvArr* warr, CvArr* uarr, CvArr* varr, int flags )
{
    uchar* buffer = 0;
    int local_alloc = 0;

    CV_FUNCNAME( "cvSVD" );

    __BEGIN__;

    CvMat astub, *a = (CvMat*)aarr;
    CvMat wstub, *w = (CvMat*)warr;
    CvMat ustub, *u;
    CvMat vstub, *v;
    CvMat tmat;
    uchar* tw = 0;
    int type;
    int a_buf_offset = 0, u_buf_offset = 0, buf_size, pix_size;
    int temp_u = 0, /* temporary storage for U is needed */
        t_svd; /* special case: a->rows < a->cols */
    int m, n;
    int w_rows, w_cols;
    int u_rows = 0, u_cols = 0;
    int w_is_mat = 0;

    if( !CV_IS_MAT( a ))
        CV_CALL( a = cvGetMat( a, &astub ));

    if( !CV_IS_MAT( w ))
        CV_CALL( w = cvGetMat( w, &wstub ));

    if( !CV_ARE_TYPES_EQ( a, w ))
        CV_ERROR( CV_StsUnmatchedFormats, "" );

    if( a->rows >= a->cols )
    {
        m = a->rows;
        n = a->cols;
        w_rows = w->rows;
        w_cols = w->cols;
        t_svd = 0;
    }
    else
    {
        CvArr* t;
        CV_SWAP( uarr, varr, t );

        flags = (flags & CV_SVD_U_T ? CV_SVD_V_T : 0)|
                (flags & CV_SVD_V_T ? CV_SVD_U_T : 0);
        m = a->cols;
        n = a->rows;
        w_rows = w->cols;
        w_cols = w->rows;
        t_svd = 1;
    }

    u = (CvMat*)uarr;
    v = (CvMat*)varr;

    w_is_mat = w_cols > 1 && w_rows > 1;

    if( !w_is_mat && CV_IS_MAT_CONT(w->type) && w_cols + w_rows - 1 == n )
        tw = w->data.ptr;

    if( u )
    {
        if( !CV_IS_MAT( u ))
            CV_CALL( u = cvGetMat( u, &ustub ));

        if( !(flags & CV_SVD_U_T) )
        {
            u_rows = u->rows;
            u_cols = u->cols;
        }
        else
        {
            u_rows = u->cols;
            u_cols = u->rows;
        }

        if( !CV_ARE_TYPES_EQ( a, u ))
            CV_ERROR( CV_StsUnmatchedFormats, "" );

        if( u_rows != m || (u_cols != m && u_cols != n))
            CV_ERROR( CV_StsUnmatchedSizes, !t_svd ? "U matrix has unappropriate size" :
                                                     "V matrix has unappropriate size" );
            
        temp_u = (u_rows != u_cols && !(flags & CV_SVD_U_T)) || u->data.ptr==a->data.ptr;

        if( w_is_mat && u_cols != w_rows )
            CV_ERROR( CV_StsUnmatchedSizes, !t_svd ? "U and W have incompatible sizes" :
                                                     "V and W have incompatible sizes" );
    }
    else
    {
        u = &ustub;
        u->data.ptr = 0;
        u->step = 0;
    }

    if( v )
    {
        int v_rows, v_cols;

        if( !CV_IS_MAT( v ))
            CV_CALL( v = cvGetMat( v, &vstub ));

        if( !(flags & CV_SVD_V_T) )
        {
            v_rows = v->rows;
            v_cols = v->cols;
        }
        else
        {
            v_rows = v->cols;
            v_cols = v->rows;
        }

        if( !CV_ARE_TYPES_EQ( a, v ))
            CV_ERROR( CV_StsUnmatchedFormats, "" );

        if( v_rows != n || v_cols != n )
            CV_ERROR( CV_StsUnmatchedSizes, t_svd ? "U matrix has unappropriate size" :
                                                    "V matrix has unappropriate size" );

        if( w_is_mat && w_cols != v_cols )
            CV_ERROR( CV_StsUnmatchedSizes, t_svd ? "U and W have incompatible sizes" :
                                                    "V and W have incompatible sizes" );
    }
    else
    {
        v = &vstub;
        v->data.ptr = 0;
        v->step = 0;
    }

    type = CV_MAT_TYPE( a->type );
    pix_size = CV_ELEM_SIZE(type);
    buf_size = n*2 + m;

    if( !(flags & CV_SVD_MODIFY_A) )
    {
        a_buf_offset = buf_size;
        buf_size += a->rows*a->cols;
    }

    if( temp_u )
    {
        u_buf_offset = buf_size;
        buf_size += u->rows*u->cols;
    }

    buf_size *= pix_size;

    if( buf_size <= CV_MAX_LOCAL_SIZE )
    {
        buffer = (uchar*)cvStackAlloc( buf_size );
        local_alloc = 1;
    }
    else
    {
        CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));
    }
    
    if( !(flags & CV_SVD_MODIFY_A) )
    {
        cvInitMatHeader( &tmat, m, n, type,
                         buffer + a_buf_offset*pix_size );
        if( !t_svd )
            cvCopy( a, &tmat );
        else
            cvT( a, &tmat );
        a = &tmat;
    }

    if( temp_u )
    {
        cvInitMatHeader( &ustub, u_cols, u_rows, type, buffer + u_buf_offset*pix_size );
        u = &ustub;
    }

    if( !tw )
        tw = buffer + (n + m)*pix_size;

    if( type == CV_32FC1 )
    {
        icvSVD_32f( a->data.fl, a->step/sizeof(float), a->rows, a->cols,
                   (float*)tw, u->data.fl, u->step/sizeof(float), u_cols,
                   v->data.fl, v->step/sizeof(float), (float*)buffer );
    }
    else if( type == CV_64FC1 )
    {
        icvSVD_64f( a->data.db, a->step/sizeof(double), a->rows, a->cols,
                    (double*)tw, u->data.db, u->step/sizeof(double), u_cols,
                    v->data.db, v->step/sizeof(double), (double*)buffer );
    }
    else
    {
        CV_ERROR( CV_StsUnsupportedFormat, "" );
    }

    if( tw != w->data.ptr )
    {
        int shift = w->cols != 1;
        cvSetZero( w );
        if( type == CV_32FC1 )
            for( int i = 0; i < n; i++ )
                ((float*)(w->data.ptr + i*w->step))[i*shift] = ((float*)tw)[i];
        else
            for( int i = 0; i < n; i++ )
                ((double*)(w->data.ptr + i*w->step))[i*shift] = ((double*)tw)[i];
    }

    if( uarr )
    {
        if( !(flags & CV_SVD_U_T))
            cvT( u, uarr );
        else if( temp_u )
            cvCopy( u, uarr );
        /*CV_CHECK_NANS( uarr );*/
    }

    if( varr )
    {
        if( !(flags & CV_SVD_V_T))
            cvT( v, varr );
        /*CV_CHECK_NANS( varr );*/
    }

    CV_CHECK_NANS( w );

    __END__;

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


CV_IMPL void
cvSVBkSb( const CvArr* warr, const CvArr* uarr,
          const CvArr* varr, const CvArr* barr,
          CvArr* xarr, int flags )
{
    uchar* buffer = 0;
    int local_alloc = 0;

    CV_FUNCNAME( "cvSVBkSb" );

    __BEGIN__;

    CvMat wstub, *w = (CvMat*)warr;
    CvMat bstub, *b = (CvMat*)barr;
    CvMat xstub, *x = (CvMat*)xarr;
    CvMat ustub, ustub2, *u = (CvMat*)uarr;
    CvMat vstub, vstub2, *v = (CvMat*)varr;
    uchar* tw = 0;
    int type;
    int temp_u = 0, temp_v = 0;
    int u_buf_offset = 0, v_buf_offset = 0, w_buf_offset = 0, t_buf_offset = 0;
    int buf_size = 0, pix_size;
    int m, n, nm;
    int u_rows, u_cols;
    int v_rows, v_cols;

    if( !CV_IS_MAT( w ))
        CV_CALL( w = cvGetMat( w, &wstub ));

    if( !CV_IS_MAT( u ))
        CV_CALL( u = cvGetMat( u, &ustub ));

    if( !CV_IS_MAT( v ))
        CV_CALL( v = cvGetMat( v, &vstub ));

    if( !CV_IS_MAT( x ))
        CV_CALL( x = cvGetMat( x, &xstub ));

    if( !CV_ARE_TYPES_EQ( w, u ) || !CV_ARE_TYPES_EQ( w, v ) || !CV_ARE_TYPES_EQ( w, x ))
        CV_ERROR( CV_StsUnmatchedFormats, "All matrices must have the same type" );

    type = CV_MAT_TYPE( w->type );
    pix_size = CV_ELEM_SIZE(type);

    if( !(flags & CV_SVD_U_T) )
    {
        temp_u = 1;
        u_buf_offset = buf_size;
        buf_size += u->cols*u->rows*pix_size;
        u_rows = u->rows;
        u_cols = u->cols;
    }
    else
    {
        u_rows = u->cols;
        u_cols = u->rows;
    }

    if( !(flags & CV_SVD_V_T) )
    {
        temp_v = 1;
        v_buf_offset = buf_size;
        buf_size += v->cols*v->rows*pix_size;
        v_rows = v->rows;
        v_cols = v->cols;
    }
    else
    {
        v_rows = v->cols;
        v_cols = v->rows;
    }

    m = u_rows;
    n = v_rows;
    nm = MIN(n,m);

    if( (u_rows != u_cols && v_rows != v_cols) || x->rows != v_rows )
        CV_ERROR( CV_StsBadSize, "V or U matrix must be square" );

    if( (w->rows == 1 || w->cols == 1) && w->rows + w->cols - 1 == nm )
    {
        if( CV_IS_MAT_CONT(w->type) )
            tw = w->data.ptr;
        else
        {
            w_buf_offset = buf_size;
            buf_size += nm*pix_size;
        }
    }
    else
    {
        if( w->cols != v_cols || w->rows != u_cols )
            CV_ERROR( CV_StsBadSize, "W must be 1d array of MIN(m,n) elements or "
                                    "matrix which size matches to U and V" );
        w_buf_offset = buf_size;
        buf_size += nm*pix_size;
    }

    if( b )
    {
        if( !CV_IS_MAT( b ))
            CV_CALL( b = cvGetMat( b, &bstub ));
        if( !CV_ARE_TYPES_EQ( w, b ))
            CV_ERROR( CV_StsUnmatchedFormats, "All matrices must have the same type" );
        if( b->cols != x->cols || b->rows != m )
            CV_ERROR( CV_StsUnmatchedSizes, "b matrix must have (m x x->cols) size" );
    }
    else
    {
        b = &bstub;
        memset( b, 0, sizeof(*b));
    }

    t_buf_offset = buf_size;
    buf_size += (MAX(m,n) + b->cols)*pix_size;

    if( buf_size <= CV_MAX_LOCAL_SIZE )
    {
        buffer = (uchar*)cvStackAlloc( buf_size );
        local_alloc = 1;
    }
    else
        CV_CALL( buffer = (uchar*)cvAlloc( buf_size ));

    if( temp_u )
    {
        cvInitMatHeader( &ustub2, u_cols, u_rows, type, buffer + u_buf_offset );
        cvT( u, &ustub2 );
        u = &ustub2;
    }

    if( temp_v )
    {
        cvInitMatHeader( &vstub2, v_cols, v_rows, type, buffer + v_buf_offset );
        cvT( v, &vstub2 );
        v = &vstub2;
    }

    if( !tw )
    {
        int i, shift = w->cols > 1 ? pix_size : 0;
        tw = buffer + w_buf_offset;
        for( i = 0; i < nm; i++ )
            memcpy( tw + i*pix_size, w->data.ptr + i*(w->step + shift), pix_size );
    }

    if( type == CV_32FC1 )
    {
        icvSVBkSb_32f( m, n, (float*)tw, u->data.fl, u->step/sizeof(float),
                       v->data.fl, v->step/sizeof(float),
                       b->data.fl, b->step/sizeof(float), b->cols,
                       x->data.fl, x->step/sizeof(float),
                       (float*)(buffer + t_buf_offset) );
    }
    else if( type == CV_64FC1 )
    {
        icvSVBkSb_64f( m, n, (double*)tw, u->data.db, u->step/sizeof(double),
                       v->data.db, v->step/sizeof(double),
                       b->data.db, b->step/sizeof(double), b->cols,
                       x->data.db, x->step/sizeof(double),
                       (double*)(buffer + t_buf_offset) );
    }
    else
    {
        CV_ERROR( CV_StsUnsupportedFormat, "" );
    }

    __END__;

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

/* End of file. */