cvemd.cpp

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

    state->s = (float *) buffer;
    buffer += (size1 + 1) * sizeof( float );

    state->d = (float *) buffer;
    buffer += (size2 + 1) * sizeof( float );

    /* sum up the supply and demand */
    for( i = 0; i < size1; i++ )
    {
        float weight = signature1[i * (dims + 1)];

        if( weight > 0 )
        {
            s_sum += weight;
            state->s[ssize] = weight;
            state->idx1[ssize++] = i;
            
        }
        else if( weight < 0 )
            return CV_BADRANGE_ERR;
    }

    for( i = 0; i < size2; i++ )
    {
        float weight = signature2[i * (dims + 1)];

        if( weight > 0 )
        {
            d_sum += weight;
            state->d[dsize] = weight;
            state->idx2[dsize++] = i;
        }
        else if( weight < 0 )
            return CV_BADRANGE_ERR;
    }

    if( ssize == 0 || dsize == 0 )
        return CV_BADRANGE_ERR;

    /* if supply different than the demand, add a zero-cost dummy cluster */
    diff = s_sum - d_sum;
    if( fabs( diff ) >= CV_EMD_EPS * s_sum )
    {
        equal_sums = 0;
        if( diff < 0 )
        {
            state->s[ssize] = -diff;
            state->idx1[ssize++] = -1;
        }    
        else
        {
            state->d[dsize] = diff;
            state->idx2[dsize++] = -1;
        }
    }

    state->ssize = ssize;
    state->dsize = dsize;
    state->weight = s_sum > d_sum ? s_sum : d_sum;

    if( lower_bound && equal_sums )     /* check lower bound */
    {
        int sz1 = size1 * (dims + 1), sz2 = size2 * (dims + 1);
        float lb = 0;

        float* xs = (float *) buffer;
        float* xd = xs + dims;

        memset( xs, 0, dims*sizeof(xs[0]));
        memset( xd, 0, dims*sizeof(xd[0]));

        for( j = 0; j < sz1; j += dims + 1 )
        {
            float weight = signature1[j];
            for( i = 0; i < dims; i++ )
                xs[i] += signature1[j + i + 1] * weight;
        }

        for( j = 0; j < sz2; j += dims + 1 )
        {
            float weight = signature2[j];
            for( i = 0; i < dims; i++ )
                xd[i] += signature2[j + i + 1] * weight;
        }

        lb = dist_func( xs, xd, user_param ) / state->weight;
        i = *lower_bound <= lb;
        *lower_bound = lb;
        if( i )
            return ( CvStatus ) 1;
    }

    /* assign pointers */
    state->is_used = (char *) buffer;
    /* init delta matrix */
    state->delta = (float **) buffer;
    buffer += ssize * sizeof( float * );

    for( i = 0; i < ssize; i++ )
    {
        state->delta[i] = (float *) buffer;
        buffer += dsize * sizeof( float );
    }

    state->loop = (CvNode2D **) buffer;
    buffer += (ssize + dsize + 1) * sizeof(CvNode2D*);

    state->_x = state->end_x = (CvNode2D *) buffer;
    buffer += (ssize + dsize) * sizeof( CvNode2D );

    /* init cost matrix */
    state->cost = (float **) buffer;
    buffer += ssize * sizeof( float * );

    /* compute the distance matrix */
    for( i = 0; i < ssize; i++ )
    {
        int ci = state->idx1[i];

        state->cost[i] = (float *) buffer;
        buffer += dsize * sizeof( float );

        if( ci >= 0 )
        {
            for( j = 0; j < dsize; j++ )
            {
                int cj = state->idx2[j];
                if( cj < 0 )
                    state->cost[i][j] = 0;
                else
                {
                    float val;
                    if( dist_func )
                    {
                        val = dist_func( signature1 + ci * (dims + 1) + 1,
                                         signature2 + cj * (dims + 1) + 1,
                                         user_param );
                    }
                    else
                    {
                        assert( cost );
                        val = cost[cost_step*ci + cj];
                    }
                    state->cost[i][j] = val;
                    if( max_cost < val )
                        max_cost = val;
                }
            }
        }
        else
        {
            for( j = 0; j < dsize; j++ )
                state->cost[i][j] = 0;
        }
    }

    state->max_cost = max_cost;
    
    memset( buffer, 0, buffer_end - buffer );

    state->rows_x = (CvNode2D **) buffer;
    buffer += ssize * sizeof( CvNode2D * );

    state->cols_x = (CvNode2D **) buffer;
    buffer += dsize * sizeof( CvNode2D * );

    state->u = (CvNode1D *) buffer;
    buffer += ssize * sizeof( CvNode1D );

    state->v = (CvNode1D *) buffer;
    buffer += dsize * sizeof( CvNode1D );

    /* init is_x matrix */
    state->is_x = (char **) buffer;
    buffer += ssize * sizeof( char * );

    for( i = 0; i < ssize; i++ )
    {
        state->is_x[i] = buffer;
        buffer += dsize;
    }

    assert( buffer <= buffer_end );

    icvRussel( state );

    state->enter_x = (state->end_x)++;
    return CV_NO_ERR;
}


/****************************************************************************************\
*                              icvFindBasicVariables                                   *
\****************************************************************************************/
static CvStatus
icvFindBasicVariables( float **cost, char **is_x,
                       CvNode1D * u, CvNode1D * v, int ssize, int dsize )
{
    int i, j, found;
    int u_cfound, v_cfound;
    CvNode1D u0_head, u1_head, *cur_u, *prev_u;
    CvNode1D v0_head, v1_head, *cur_v, *prev_v;

    /* initialize the rows list (u) and the columns list (v) */
    u0_head.next = u;
    for( i = 0; i < ssize; i++ )
    {
        u[i].next = u + i + 1;
    }
    u[ssize - 1].next = 0;
    u1_head.next = 0;

    v0_head.next = ssize > 1 ? v + 1 : 0;
    for( i = 1; i < dsize; i++ )
    {
        v[i].next = v + i + 1;
    }
    v[dsize - 1].next = 0;
    v1_head.next = 0;

    /* there are ssize+dsize variables but only ssize+dsize-1 independent equations,
       so set v[0]=0 */
    v[0].val = 0;
    v1_head.next = v;
    v1_head.next->next = 0;

    /* loop until all variables are found */
    u_cfound = v_cfound = 0;
    while( u_cfound < ssize || v_cfound < dsize )
    {
        found = 0;
        if( v_cfound < dsize )
        {
            /* loop over all marked columns */
            prev_v = &v1_head;

            for( found |= (cur_v = v1_head.next) != 0; cur_v != 0; cur_v = cur_v->next )
            {
                float cur_v_val = cur_v->val;

                j = (int)(cur_v - v);
                /* find the variables in column j */
                prev_u = &u0_head;
                for( cur_u = u0_head.next; cur_u != 0; )
                {
                    i = (int)(cur_u - u);
                    if( is_x[i][j] )
                    {
                        /* compute u[i] */
                        cur_u->val = cost[i][j] - cur_v_val;
                        /* ...and add it to the marked list */
                        prev_u->next = cur_u->next;
                        cur_u->next = u1_head.next;
                        u1_head.next = cur_u;
                        cur_u = prev_u->next;
                    }
                    else
                    {
                        prev_u = cur_u;
                        cur_u = cur_u->next;
                    }
                }
                prev_v->next = cur_v->next;
                v_cfound++;
            }
        }

        if( u_cfound < ssize )
        {
            /* loop over all marked rows */
            prev_u = &u1_head;
            for( found |= (cur_u = u1_head.next) != 0; cur_u != 0; cur_u = cur_u->next )
            {
                float cur_u_val = cur_u->val;
                float *_cost;
                char *_is_x;

                i = (int)(cur_u - u);
                _cost = cost[i];
                _is_x = is_x[i];
                /* find the variables in rows i */
                prev_v = &v0_head;
                for( cur_v = v0_head.next; cur_v != 0; )
                {
                    j = (int)(cur_v - v);
                    if( _is_x[j] )
                    {
                        /* compute v[j] */
                        cur_v->val = _cost[j] - cur_u_val;
                        /* ...and add it to the marked list */
                        prev_v->next = cur_v->next;
                        cur_v->next = v1_head.next;
                        v1_head.next = cur_v;
                        cur_v = prev_v->next;
                    }
                    else
                    {
                        prev_v = cur_v;
                        cur_v = cur_v->next;
                    }
                }
                prev_u->next = cur_u->next;
                u_cfound++;
            }
        }

        if( !found )
        {
            return CV_NOTDEFINED_ERR;
        }
    }

    return CV_NO_ERR;
}


/****************************************************************************************\
*                                   icvIsOptimal                                       *
\****************************************************************************************/
static float
icvIsOptimal( float **cost, char **is_x,
              CvNode1D * u, CvNode1D * v, int ssize, int dsize, CvNode2D * enter_x )
{
    float delta, min_delta = CV_EMD_INF;
    int i, j, min_i = 0, min_j = 0;

    /* find the minimal cij-ui-vj over all i,j */
    for( i = 0; i < ssize; i++ )
    {
        float u_val = u[i].val;
        float *_cost = cost[i];
        char *_is_x = is_x[i];

        for( j = 0; j < dsize; j++ )
        {
            if( !_is_x[j] )
            {
                delta = _cost[j] - u_val - v[j].val;
                if( min_delta > delta )
                {
                    min_delta = delta;
                    min_i = i;
                    min_j = j;
                }
            }
        }
    }

    enter_x->i = min_i;
    enter_x->j = min_j;

    return min_delta;
}

/****************************************************************************************\
*                                   icvNewSolution                                     *
\****************************************************************************************/
static CvStatus
icvNewSolution( CvEMDState * state )
{
    int i, j;
    float min_val = CV_EMD_INF;
    int steps;
    CvNode2D head, *cur_x, *next_x, *leave_x = 0;
    CvNode2D *enter_x = state->enter_x;
    CvNode2D **loop = state->loop;

    /* enter the new basic variable */
    i = enter_x->i;
    j = enter_x->j;
    state->is_x[i][j] = 1;
    enter_x->next[0] = state->rows_x[i];
    enter_x->next[1] = state->cols_x[j];
    enter_x->val = 0;
    state->rows_x[i] = enter_x;
    state->cols_x[j] = enter_x;

    /* find a chain reaction */
    steps = icvFindLoop( state );

    if( steps == 0 )
        return CV_NOTDEFINED_ERR;

    /* find the largest value in the loop */
    for( i = 1; i < steps; i += 2 )
    {
        float temp = loop[i]->val;