cvlmeds.cpp.svn-base

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

        for( j = i + 1; j < length; j++ )
        {

            if( array[j] < array[index] )
                index = j;
        }                       /* for */

        if( index - i )
        {

            swapd = array[i];
            array[i] = array[index];
            array[index] = swapd;
        }                       /* if */
    }                           /* for */

    return CV_NO_ERR;

}                               /* cs_Sort */

/*===========================================================================*/
int
icvBoltingPoints( int *ml, int *mr,
                  int num, double *F, double Mj, int **new_ml, int **new_mr, int *new_num )
{
    double l1, l2, l3, d1, d2, sigma;
    int i, j, length;
    int *index;

    if( !ml || !mr || num < 1 || !F || Mj < 0 )
        return -1;

    index = (int *) icvAlloc( (num) * sizeof( int ));

    if( !index )
        return -1;

    length = 0;
    sigma = (double) (2.5 * 1.4826 * (1 + 5. / (num - 7)) * sqrt( Mj ));

    for( i = 0; i < num * 3; i += 3 )
    {

        l1 = F[0] * mr[i] + F[1] * mr[i + 1] + F[2];
        l2 = F[3] * mr[i] + F[4] * mr[i + 1] + F[5];
        l3 = F[6] * mr[i] + F[7] * mr[i + 1] + F[8];

        d1 = (l1 * ml[i] + l2 * ml[i + 1] + l3) / sqrt( l1 * l1 + l2 * l2 );

        l1 = F[0] * ml[i] + F[3] * ml[i + 1] + F[6];
        l2 = F[1] * ml[i] + F[4] * ml[i + 1] + F[7];
        l3 = F[2] * ml[i] + F[5] * ml[i + 1] + F[8];

        d2 = (l1 * mr[i] + l2 * mr[i + 1] + l3) / sqrt( l1 * l1 + l2 * l2 );

        if( d1 * d1 + d2 * d2 <= sigma * sigma )
        {

            index[i / 3] = 1;
            length++;
        }
        else
        {

            index[i / 3] = 0;
        }                       /* if */
    }                           /* for */

    *new_num = length;

    *new_ml = (int *) icvAlloc( (length * 3) * sizeof( int ));

    if( !new_ml )
    {

        icvFree( &index );
        return -1;
    }                           /* if */

    *new_mr = (int *) icvAlloc( (length * 3) * sizeof( int ));

    if( !new_mr )
    {

        icvFree( &new_ml );
        icvFree( &index );
        return -1;
    }                           /* if */

    j = 0;

    for( i = 0; i < num * 3; )
    {

        if( index[i / 3] )
        {

            (*new_ml)[j] = ml[i];
            (*new_mr)[j++] = mr[i++];
            (*new_ml)[j] = ml[i];
            (*new_mr)[j++] = mr[i++];
            (*new_ml)[j] = ml[i];
            (*new_mr)[j++] = mr[i++];
        }
        else
            i += 3;
    }                           /* for */

    icvFree( &index );
    return length;

}                               /* cs_BoltingPoints */

/*===========================================================================*/
CvStatus
icvPoints8( int *ml, int *mr, int num, double *F )
{
    double *U;
    double l1, l2, w, old_norm = -1, new_norm = -2, summ;
    int i3, i9, j, num3, its = 0, a, t;

    if( !ml || !mr || num < 8 || !F )
        return CV_BADFACTOR_ERR;

    U = (double *) icvAlloc( (num * 9) * sizeof( double ));

    if( !U )
        return CV_OUTOFMEM_ERR;

    num3 = num * 3;

    while( !REAL_ZERO( new_norm - old_norm ))
    {

        if( its++ > 1e+2 )
        {

            icvFree( &U );
            return CV_BADFACTOR_ERR;
        }                       /* if */

        old_norm = new_norm;

        for( i3 = 0, i9 = 0; i3 < num3; i3 += 3, i9 += 9 )
        {

            l1 = F[0] * mr[i3] + F[1] * mr[i3 + 1] + F[2];
            l2 = F[3] * mr[i3] + F[4] * mr[i3 + 1] + F[5];

            if( REAL_ZERO( l1 ) && REAL_ZERO( l2 ))
            {

                icvFree( &U );
                return CV_BADFACTOR_ERR;
            }                   /* if */

            w = 1 / (l1 * l1 + l2 * l2);

            l1 = F[0] * ml[i3] + F[3] * ml[i3 + 1] + F[6];
            l2 = F[1] * ml[i3] + F[4] * ml[i3 + 1] + F[7];

            if( REAL_ZERO( l1 ) && REAL_ZERO( l2 ))
            {

                icvFree( &U );
                return CV_BADFACTOR_ERR;
            }                   /* if */

            w += 1 / (l1 * l1 + l2 * l2);
            w = sqrt( w );

            for( j = 0; j < 9; j++ )
            {

                U[i9 + j] = w * (double) ml[i3 + j / 3] * (double) mr[i3 + j % 3];
            }                   /* for */
        }                       /* for */

        new_norm = 0;

        for( a = 0; a < num; a++ )
        {                       /* row */

            summ = 0;

            for( t = 0; t < 9; t++ )
            {

                summ += U[a * 9 + t] * F[t];
            }                   /* for */

            new_norm += summ * summ;
        }                       /* for */

        new_norm = sqrt( new_norm );

        icvAnalyticPoints8( U, num, F );
    }                           /* while */

    icvFree( &U );
    return CV_NO_ERR;

}                               /* cs_Points8 */

/*===========================================================================*/
double
icvAnalyticPoints8( double *A, int num, double *F )
{
    double *U;
    double V[8 * 8];
    double W[8];
    double *f;
    double solution[9];
    double temp1[8 * 8];
    double *temp2;
    double *A_short;
    double norm, summ, best_norm;
    int num8 = num * 8, num9 = num * 9;
    int i, j, j8, j9, value, a, a8, a9, a_num, b, b8, t;

    /* --------- Initialization data ------------------ */

    if( !A || num < 8 || !F )
        return -1;

    best_norm = -1;
    U = (double *) icvAlloc( (num8) * sizeof( double ));

    if( !U )
        return -1;

    f = (double *) icvAlloc( (num) * sizeof( double ));

    if( !f )
    {
        icvFree( &U );
        return -1;
    }                           /* if */

    temp2 = (double *) icvAlloc( (num8) * sizeof( double ));

    if( !temp2 )
    {
        icvFree( &f );
        icvFree( &U );
        return -1;
    }                           /* if */

    A_short = (double *) icvAlloc( (num8) * sizeof( double ));

    if( !A_short )
    {
        icvFree( &temp2 );
        icvFree( &f );
        icvFree( &U );
        return -1;
    }                           /* if */

    for( i = 0; i < 8; i++ )
    {
        for( j8 = 0, j9 = 0; j9 < num9; j8 += 8, j9 += 9 )
        {
            A_short[j8 + i] = A[j9 + i + 1];
        }                       /* for */
    }                           /* for */

    for( i = 0; i < 9; i++ )
    {

        for( j = 0, j8 = 0, j9 = 0; j < num; j++, j8 += 8, j9 += 9 )
        {

            f[j] = -A[j9 + i];

            if( i )
                A_short[j8 + i - 1] = A[j9 + i - 1];
        }                       /* for */

        value = icvSingularValueDecomposition( num, 8, A_short, W, 1, U, 1, V );

        if( !value )
        {                       /* -----------  computing the solution  ----------- */

            /*  -----------  W = W(-1)  ----------- */
            for( j = 0; j < 8; j++ )
            {
                if( !REAL_ZERO( W[j] ))
                    W[j] = 1 / W[j];
            }                   /* for */

            /* -----------  temp1 = V * W(-1)  ----------- */
            for( a8 = 0; a8 < 64; a8 += 8 )
            {                   /* row */
                for( b = 0; b < 8; b++ )
                {               /* column */
                    temp1[a8 + b] = V[a8 + b] * W[b];
                }               /* for */
            }                   /* for */

            /*  -----------  temp2 = V * W(-1) * U(T)  ----------- */
            for( a8 = 0, a_num = 0; a8 < 64; a8 += 8, a_num += num )
            {                   /* row */
                for( b = 0, b8 = 0; b < num; b++, b8 += 8 )
                {               /* column */

                    temp2[a_num + b] = 0;

                    for( t = 0; t < 8; t++ )
                    {

                        temp2[a_num + b] += temp1[a8 + t] * U[b8 + t];
                    }           /* for */
                }               /* for */
            }                   /* for */

            /* -----------  solution = V * W(-1) * U(T) * f  ----------- */
            for( a = 0, a_num = 0; a < 8; a++, a_num += num )
            {                   /* row */
                for( b = 0; b < num; b++ )
                {               /* column */

                    solution[a] = 0;

                    for( t = 0; t < num && W[a]; t++ )
                    {
                        solution[a] += temp2[a_num + t] * f[t];
                    }           /* for */
                }               /* for */
            }                   /* for */

            for( a = 8; a > 0; a-- )
            {

                if( a == i )
                    break;
                solution[a] = solution[a - 1];
            }                   /* for */

            solution[a] = 1;

            norm = 0;

            for( a9 = 0; a9 < num9; a9 += 9 )
            {                   /* row */

                summ = 0;

                for( t = 0; t < 9; t++ )
                {

                    summ += A[a9 + t] * solution[t];
                }               /* for */

                norm += summ * summ;
            }                   /* for */

            norm = sqrt( norm );

            if( best_norm == -1 || norm < best_norm )
            {

                for( j = 0; j < 9; j++ )
                    F[j] = solution[j];

                best_norm = norm;
            }                   /* if */
        }                       /* if */
    }                           /* for */

    icvFree( &A_short );
    icvFree( &temp2 );
    icvFree( &f );
    icvFree( &U );

    return best_norm;

}                               /* cs_AnalyticPoints8 */

/*===========================================================================*/
CvStatus
icvRank2Constraint( double *F )
{
    double U[9], V[9], W[3];
    int i, i3, j, j3, t;

    if( F == 0 )
        return CV_BADFACTOR_ERR;

    if( icvSingularValueDecomposition( 3, 3, F, W, 1, U, 1, V ))
        return CV_BADFACTOR_ERR;

    if( fabs( W[0] ) < fabs( W[1] ))
    {

        if( fabs( W[0] ) < fabs( W[2] ))
        {

            if( REAL_ZERO( W[0] ))
                return CV_NO_ERR;
            else
                W[0] = 0;
        }
        else
        {

            if( REAL_ZERO( W[2] ))
                return CV_NO_ERR;
            else
                W[2] = 0;
        }                       /* if */
    }
    else
    {

        if( fabs( W[1] ) < fabs( W[2] ))
        {

            if( REAL_ZERO( W[1] ))
                return CV_NO_ERR;
            else
                W[1] = 0;
        }
        else
        {
            if( REAL_ZERO( W[2] ))
                return CV_NO_ERR;
            else
                W[2] = 0;
        }                       /* if */
    }                           /* if */

    for( i = 0; i < 3; i++ )
    {
        for( j3 = 0; j3 < 9; j3 += 3 )
        {
            U[j3 + i] *= W[i];
        }                       /* for */
    }                           /* for */

    for( i = 0, i3 = 0; i < 3; i++, i3 += 3 )
    {
        for( j = 0, j3 = 0; j < 3; j++, j3 += 3 )
        {

            F[i3 + j] = 0;

            for( t = 0; t < 3; t++ )
            {
                F[i3 + j] += U[i3 + t] * V[j3 + t];
            }                   /* for */
        }                       /* for */
    }                           /* for */

    return CV_NO_ERR;
}                               /* cs_Rank2Constraint */


/*===========================================================================*/

int
icvSingularValueDecomposition( int M,
                               int N,
                               double *A,
                               double *W, int get_U, double *U, int get_V, double *V )
{
    int i = 0, j, k, l = 0, i1, k1, l1 = 0;
    int iterations, error = 0, jN, iN, kN, lN = 0;
    double *rv1;
    double c, f, g, h, s, x, y, z, scale, anorm;