| /*M/////////////////////////////////////////////////////////////////////////////////////// |
| // |
| // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. |
| // |
| // By downloading, copying, installing or using the software you agree to this license. |
| // If you do not agree to this license, do not download, install, |
| // copy or use the software. |
| // |
| // |
| // Intel License Agreement |
| // For Open Source Computer Vision Library |
| // |
| // Copyright (C) 2000, Intel Corporation, all rights reserved. |
| // Third party copyrights are property of their respective owners. |
| // |
| // Redistribution and use in source and binary forms, with or without modification, |
| // are permitted provided that the following conditions are met: |
| // |
| // * Redistribution's of source code must retain the above copyright notice, |
| // this list of conditions and the following disclaimer. |
| // |
| // * Redistribution's in binary form must reproduce the above copyright notice, |
| // this list of conditions and the following disclaimer in the documentation |
| // and/or other materials provided with the distribution. |
| // |
| // * The name of Intel Corporation may not be used to endorse or promote products |
| // derived from this software without specific prior written permission. |
| // |
| // This software is provided by the copyright holders and contributors "as is" and |
| // any express or implied warranties, including, but not limited to, the implied |
| // warranties of merchantability and fitness for a particular purpose are disclaimed. |
| // In no event shall the Intel Corporation or contributors be liable for any direct, |
| // indirect, incidental, special, exemplary, or consequential damages |
| // (including, but not limited to, procurement of substitute goods or services; |
| // loss of use, data, or profits; or business interruption) however caused |
| // and on any theory of liability, whether in contract, strict liability, |
| // or tort (including negligence or otherwise) arising in any way out of |
| // the use of this software, even if advised of the possibility of such damage. |
| // |
| //M*/ |
| |
| /* |
| Partially based on Yossi Rubner code: |
| ========================================================================= |
| emd.c |
| |
| Last update: 3/14/98 |
| |
| An implementation of the Earth Movers Distance. |
| Based of the solution for the Transportation problem as described in |
| "Introduction to Mathematical Programming" by F. S. Hillier and |
| G. J. Lieberman, McGraw-Hill, 1990. |
| |
| Copyright (C) 1998 Yossi Rubner |
| Computer Science Department, Stanford University |
| E-Mail: rubner@cs.stanford.edu URL: http://vision.stanford.edu/~rubner |
| ========================================================================== |
| */ |
| #include "_cv.h" |
| |
| #define MAX_ITERATIONS 500 |
| #define CV_EMD_INF ((float)1e20) |
| #define CV_EMD_EPS ((float)1e-5) |
| |
| /* CvNode1D is used for lists, representing 1D sparse array */ |
| typedef struct CvNode1D |
| { |
| float val; |
| struct CvNode1D *next; |
| } |
| CvNode1D; |
| |
| /* CvNode2D is used for lists, representing 2D sparse matrix */ |
| typedef struct CvNode2D |
| { |
| float val; |
| struct CvNode2D *next[2]; /* next row & next column */ |
| int i, j; |
| } |
| CvNode2D; |
| |
| |
| typedef struct CvEMDState |
| { |
| int ssize, dsize; |
| |
| float **cost; |
| CvNode2D *_x; |
| CvNode2D *end_x; |
| CvNode2D *enter_x; |
| char **is_x; |
| |
| CvNode2D **rows_x; |
| CvNode2D **cols_x; |
| |
| CvNode1D *u; |
| CvNode1D *v; |
| |
| int* idx1; |
| int* idx2; |
| |
| /* find_loop buffers */ |
| CvNode2D **loop; |
| char *is_used; |
| |
| /* russel buffers */ |
| float *s; |
| float *d; |
| float **delta; |
| |
| float weight, max_cost; |
| char *buffer; |
| } |
| CvEMDState; |
| |
| /* static function declaration */ |
| static CvStatus icvInitEMD( const float *signature1, int size1, |
| const float *signature2, int size2, |
| int dims, CvDistanceFunction dist_func, void *user_param, |
| const float* cost, int cost_step, |
| CvEMDState * state, float *lower_bound, |
| char *local_buffer, int local_buffer_size ); |
| |
| static CvStatus icvFindBasicVariables( float **cost, char **is_x, |
| CvNode1D * u, CvNode1D * v, int ssize, int dsize ); |
| |
| static float icvIsOptimal( float **cost, char **is_x, |
| CvNode1D * u, CvNode1D * v, |
| int ssize, int dsize, CvNode2D * enter_x ); |
| |
| static void icvRussel( CvEMDState * state ); |
| |
| |
| static CvStatus icvNewSolution( CvEMDState * state ); |
| static int icvFindLoop( CvEMDState * state ); |
| |
| static void icvAddBasicVariable( CvEMDState * state, |
| int min_i, int min_j, |
| CvNode1D * prev_u_min_i, |
| CvNode1D * prev_v_min_j, |
| CvNode1D * u_head ); |
| |
| static float icvDistL2( const float *x, const float *y, void *user_param ); |
| static float icvDistL1( const float *x, const float *y, void *user_param ); |
| static float icvDistC( const float *x, const float *y, void *user_param ); |
| |
| /* The main function */ |
| CV_IMPL float |
| cvCalcEMD2( const CvArr* signature_arr1, |
| const CvArr* signature_arr2, |
| int dist_type, |
| CvDistanceFunction dist_func, |
| const CvArr* cost_matrix, |
| CvArr* flow_matrix, |
| float *lower_bound, |
| void *user_param ) |
| { |
| char local_buffer[16384]; |
| char *local_buffer_ptr = (char *)cvAlignPtr(local_buffer,16); |
| CvEMDState state; |
| float emd = 0; |
| |
| CV_FUNCNAME( "cvCalcEMD2" ); |
| |
| memset( &state, 0, sizeof(state)); |
| |
| __BEGIN__; |
| |
| double total_cost = 0; |
| CvStatus result = CV_NO_ERR; |
| float eps, min_delta; |
| CvNode2D *xp = 0; |
| CvMat sign_stub1, *signature1 = (CvMat*)signature_arr1; |
| CvMat sign_stub2, *signature2 = (CvMat*)signature_arr2; |
| CvMat cost_stub, *cost = &cost_stub; |
| CvMat flow_stub, *flow = (CvMat*)flow_matrix; |
| int dims, size1, size2; |
| |
| CV_CALL( signature1 = cvGetMat( signature1, &sign_stub1 )); |
| CV_CALL( signature2 = cvGetMat( signature2, &sign_stub2 )); |
| |
| if( signature1->cols != signature2->cols ) |
| CV_ERROR( CV_StsUnmatchedSizes, "The arrays must have equal number of columns (which is number of dimensions but 1)" ); |
| |
| dims = signature1->cols - 1; |
| size1 = signature1->rows; |
| size2 = signature2->rows; |
| |
| if( !CV_ARE_TYPES_EQ( signature1, signature2 )) |
| CV_ERROR( CV_StsUnmatchedFormats, "The array must have equal types" ); |
| |
| if( CV_MAT_TYPE( signature1->type ) != CV_32FC1 ) |
| CV_ERROR( CV_StsUnsupportedFormat, "The signatures must be 32fC1" ); |
| |
| if( flow ) |
| { |
| CV_CALL( flow = cvGetMat( flow, &flow_stub )); |
| |
| if( flow->rows != size1 || flow->cols != size2 ) |
| CV_ERROR( CV_StsUnmatchedSizes, |
| "The flow matrix size does not match to the signatures' sizes" ); |
| |
| if( CV_MAT_TYPE( flow->type ) != CV_32FC1 ) |
| CV_ERROR( CV_StsUnsupportedFormat, "The flow matrix must be 32fC1" ); |
| } |
| |
| cost->data.fl = 0; |
| cost->step = 0; |
| |
| if( dist_type < 0 ) |
| { |
| if( cost_matrix ) |
| { |
| if( dist_func ) |
| CV_ERROR( CV_StsBadArg, |
| "Only one of cost matrix or distance function should be non-NULL in case of user-defined distance" ); |
| |
| if( lower_bound ) |
| CV_ERROR( CV_StsBadArg, |
| "The lower boundary can not be calculated if the cost matrix is used" ); |
| |
| CV_CALL( cost = cvGetMat( cost_matrix, &cost_stub )); |
| if( cost->rows != size1 || cost->cols != size2 ) |
| CV_ERROR( CV_StsUnmatchedSizes, |
| "The cost matrix size does not match to the signatures' sizes" ); |
| |
| if( CV_MAT_TYPE( cost->type ) != CV_32FC1 ) |
| CV_ERROR( CV_StsUnsupportedFormat, "The cost matrix must be 32fC1" ); |
| } |
| else if( !dist_func ) |
| CV_ERROR( CV_StsNullPtr, "In case of user-defined distance Distance function is undefined" ); |
| } |
| else |
| { |
| if( dims == 0 ) |
| CV_ERROR( CV_StsBadSize, |
| "Number of dimensions can be 0 only if a user-defined metric is used" ); |
| user_param = (void *) (size_t)dims; |
| switch (dist_type) |
| { |
| case CV_DIST_L1: |
| dist_func = icvDistL1; |
| break; |
| case CV_DIST_L2: |
| dist_func = icvDistL2; |
| break; |
| case CV_DIST_C: |
| dist_func = icvDistC; |
| break; |
| default: |
| CV_ERROR( CV_StsBadFlag, "Bad or unsupported metric type" ); |
| } |
| } |
| |
| IPPI_CALL( result = icvInitEMD( signature1->data.fl, size1, |
| signature2->data.fl, size2, |
| dims, dist_func, user_param, |
| cost->data.fl, cost->step, |
| &state, lower_bound, local_buffer_ptr, |
| sizeof( local_buffer ) - 16 )); |
| |
| if( result > 0 && lower_bound ) |
| { |
| emd = *lower_bound; |
| EXIT; |
| } |
| |
| eps = CV_EMD_EPS * state.max_cost; |
| |
| /* if ssize = 1 or dsize = 1 then we are done, else ... */ |
| if( state.ssize > 1 && state.dsize > 1 ) |
| { |
| int itr; |
| |
| for( itr = 1; itr < MAX_ITERATIONS; itr++ ) |
| { |
| /* find basic variables */ |
| result = icvFindBasicVariables( state.cost, state.is_x, |
| state.u, state.v, state.ssize, state.dsize ); |
| if( result < 0 ) |
| break; |
| |
| /* check for optimality */ |
| min_delta = icvIsOptimal( state.cost, state.is_x, |
| state.u, state.v, |
| state.ssize, state.dsize, state.enter_x ); |
| |
| if( min_delta == CV_EMD_INF ) |
| { |
| CV_ERROR( CV_StsNoConv, "" ); |
| } |
| |
| /* if no negative deltamin, we found the optimal solution */ |
| if( min_delta >= -eps ) |
| break; |
| |
| /* improve solution */ |
| IPPI_CALL( icvNewSolution( &state )); |
| } |
| } |
| |
| /* compute the total flow */ |
| for( xp = state._x; xp < state.end_x; xp++ ) |
| { |
| float val = xp->val; |
| int i = xp->i; |
| int j = xp->j; |
| int ci = state.idx1[i]; |
| int cj = state.idx2[j]; |
| |
| if( xp != state.enter_x && ci >= 0 && cj >= 0 ) |
| { |
| total_cost += (double)val * state.cost[i][j]; |
| if( flow ) |
| ((float*)(flow->data.ptr + flow->step*ci))[cj] = val; |
| } |
| } |
| |
| emd = (float) (total_cost / state.weight); |
| |
| __END__; |
| |
| if( state.buffer && state.buffer != local_buffer_ptr ) |
| cvFree( &state.buffer ); |
| |
| return emd; |
| } |
| |
| |
| /************************************************************************************\ |
| * initialize structure, allocate buffers and generate initial golution * |
| \************************************************************************************/ |
| static CvStatus |
| icvInitEMD( const float* signature1, int size1, |
| const float* signature2, int size2, |
| int dims, CvDistanceFunction dist_func, void* user_param, |
| const float* cost, int cost_step, |
| CvEMDState* state, float* lower_bound, |
| char* local_buffer, int local_buffer_size ) |
| { |
| float s_sum = 0, d_sum = 0, diff; |
| int i, j; |
| int ssize = 0, dsize = 0; |
| int equal_sums = 1; |
| int buffer_size; |
| float max_cost = 0; |
| char *buffer, *buffer_end; |
| |
| memset( state, 0, sizeof( *state )); |
| assert( cost_step % sizeof(float) == 0 ); |
| cost_step /= sizeof(float); |
| |
| /* calculate buffer size */ |
| buffer_size = (size1+1) * (size2+1) * (sizeof( float ) + /* cost */ |
| sizeof( char ) + /* is_x */ |
| sizeof( float )) + /* delta matrix */ |
| (size1 + size2 + 2) * (sizeof( CvNode2D ) + /* _x */ |
| sizeof( CvNode2D * ) + /* cols_x & rows_x */ |
| sizeof( CvNode1D ) + /* u & v */ |
| sizeof( float ) + /* s & d */ |
| sizeof( int ) + sizeof(CvNode2D*)) + /* idx1 & idx2 */ |
| (size1+1) * (sizeof( float * ) + sizeof( char * ) + /* rows pointers for */ |
| sizeof( float * )) + 256; /* cost, is_x and delta */ |
| |
| if( buffer_size < (int) (dims * 2 * sizeof( float ))) |
| { |
| buffer_size = dims * 2 * sizeof( float ); |
| } |
| |
| /* allocate buffers */ |
| if( local_buffer != 0 && local_buffer_size >= buffer_size ) |
| { |
| buffer = local_buffer; |
| } |
| else |
| { |
| buffer = (char*)cvAlloc( buffer_size ); |
| if( !buffer ) |
| return CV_OUTOFMEM_ERR; |
| } |
| |
| state->buffer = buffer; |
| buffer_end = buffer + buffer_size; |
| |
| state->idx1 = (int*) buffer; |
| buffer += (size1 + 1) * sizeof( int ); |
| |
| state->idx2 = (int*) buffer; |
| buffer += (size2 + 1) * sizeof( int ); |
| |
| 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; |
| |
| if( min_val > temp ) |
| { |
| leave_x = loop[i]; |
| min_val = temp; |
| } |
| } |
| |
| /* update the loop */ |
| for( i = 0; i < steps; i += 2 ) |
| { |
| float temp0 = loop[i]->val + min_val; |
| float temp1 = loop[i + 1]->val - min_val; |
| |
| loop[i]->val = temp0; |
| loop[i + 1]->val = temp1; |
| } |
| |
| /* remove the leaving basic variable */ |
| i = leave_x->i; |
| j = leave_x->j; |
| state->is_x[i][j] = 0; |
| |
| head.next[0] = state->rows_x[i]; |
| cur_x = &head; |
| while( (next_x = cur_x->next[0]) != leave_x ) |
| { |
| cur_x = next_x; |
| assert( cur_x ); |
| } |
| cur_x->next[0] = next_x->next[0]; |
| state->rows_x[i] = head.next[0]; |
| |
| head.next[1] = state->cols_x[j]; |
| cur_x = &head; |
| while( (next_x = cur_x->next[1]) != leave_x ) |
| { |
| cur_x = next_x; |
| assert( cur_x ); |
| } |
| cur_x->next[1] = next_x->next[1]; |
| state->cols_x[j] = head.next[1]; |
| |
| /* set enter_x to be the new empty slot */ |
| state->enter_x = leave_x; |
| |
| return CV_NO_ERR; |
| } |
| |
| |
| |
| /****************************************************************************************\ |
| * icvFindLoop * |
| \****************************************************************************************/ |
| static int |
| icvFindLoop( CvEMDState * state ) |
| { |
| int i, steps = 1; |
| CvNode2D *new_x; |
| CvNode2D **loop = state->loop; |
| CvNode2D *enter_x = state->enter_x, *_x = state->_x; |
| char *is_used = state->is_used; |
| |
| memset( is_used, 0, state->ssize + state->dsize ); |
| |
| new_x = loop[0] = enter_x; |
| is_used[enter_x - _x] = 1; |
| steps = 1; |
| |
| do |
| { |
| if( (steps & 1) == 1 ) |
| { |
| /* find an unused x in the row */ |
| new_x = state->rows_x[new_x->i]; |
| while( new_x != 0 && is_used[new_x - _x] ) |
| new_x = new_x->next[0]; |
| } |
| else |
| { |
| /* find an unused x in the column, or the entering x */ |
| new_x = state->cols_x[new_x->j]; |
| while( new_x != 0 && is_used[new_x - _x] && new_x != enter_x ) |
| new_x = new_x->next[1]; |
| if( new_x == enter_x ) |
| break; |
| } |
| |
| if( new_x != 0 ) /* found the next x */ |
| { |
| /* add x to the loop */ |
| loop[steps++] = new_x; |
| is_used[new_x - _x] = 1; |
| } |
| else /* didn't find the next x */ |
| { |
| /* backtrack */ |
| do |
| { |
| i = steps & 1; |
| new_x = loop[steps - 1]; |
| do |
| { |
| new_x = new_x->next[i]; |
| } |
| while( new_x != 0 && is_used[new_x - _x] ); |
| |
| if( new_x == 0 ) |
| { |
| is_used[loop[--steps] - _x] = 0; |
| } |
| } |
| while( new_x == 0 && steps > 0 ); |
| |
| is_used[loop[steps - 1] - _x] = 0; |
| loop[steps - 1] = new_x; |
| is_used[new_x - _x] = 1; |
| } |
| } |
| while( steps > 0 ); |
| |
| return steps; |
| } |
| |
| |
| |
| /****************************************************************************************\ |
| * icvRussel * |
| \****************************************************************************************/ |
| static void |
| icvRussel( CvEMDState * state ) |
| { |
| int i, j, min_i = -1, min_j = -1; |
| float min_delta, diff; |
| CvNode1D u_head, *cur_u, *prev_u; |
| CvNode1D v_head, *cur_v, *prev_v; |
| CvNode1D *prev_u_min_i = 0, *prev_v_min_j = 0, *remember; |
| CvNode1D *u = state->u, *v = state->v; |
| int ssize = state->ssize, dsize = state->dsize; |
| float eps = CV_EMD_EPS * state->max_cost; |
| float **cost = state->cost; |
| float **delta = state->delta; |
| |
| /* initialize the rows list (ur), and the columns list (vr) */ |
| u_head.next = u; |
| for( i = 0; i < ssize; i++ ) |
| { |
| u[i].next = u + i + 1; |
| } |
| u[ssize - 1].next = 0; |
| |
| v_head.next = v; |
| for( i = 0; i < dsize; i++ ) |
| { |
| v[i].val = -CV_EMD_INF; |
| v[i].next = v + i + 1; |
| } |
| v[dsize - 1].next = 0; |
| |
| /* find the maximum row and column values (ur[i] and vr[j]) */ |
| for( i = 0; i < ssize; i++ ) |
| { |
| float u_val = -CV_EMD_INF; |
| float *cost_row = cost[i]; |
| |
| for( j = 0; j < dsize; j++ ) |
| { |
| float temp = cost_row[j]; |
| |
| if( u_val < temp ) |
| u_val = temp; |
| if( v[j].val < temp ) |
| v[j].val = temp; |
| } |
| u[i].val = u_val; |
| } |
| |
| /* compute the delta matrix */ |
| for( i = 0; i < ssize; i++ ) |
| { |
| float u_val = u[i].val; |
| float *delta_row = delta[i]; |
| float *cost_row = cost[i]; |
| |
| for( j = 0; j < dsize; j++ ) |
| { |
| delta_row[j] = cost_row[j] - u_val - v[j].val; |
| } |
| } |
| |
| /* find the basic variables */ |
| do |
| { |
| /* find the smallest delta[i][j] */ |
| min_i = -1; |
| min_delta = CV_EMD_INF; |
| prev_u = &u_head; |
| for( cur_u = u_head.next; cur_u != 0; cur_u = cur_u->next ) |
| { |
| i = (int)(cur_u - u); |
| float *delta_row = delta[i]; |
| |
| prev_v = &v_head; |
| for( cur_v = v_head.next; cur_v != 0; cur_v = cur_v->next ) |
| { |
| j = (int)(cur_v - v); |
| if( min_delta > delta_row[j] ) |
| { |
| min_delta = delta_row[j]; |
| min_i = i; |
| min_j = j; |
| prev_u_min_i = prev_u; |
| prev_v_min_j = prev_v; |
| } |
| prev_v = cur_v; |
| } |
| prev_u = cur_u; |
| } |
| |
| if( min_i < 0 ) |
| break; |
| |
| /* add x[min_i][min_j] to the basis, and adjust supplies and cost */ |
| remember = prev_u_min_i->next; |
| icvAddBasicVariable( state, min_i, min_j, prev_u_min_i, prev_v_min_j, &u_head ); |
| |
| /* update the necessary delta[][] */ |
| if( remember == prev_u_min_i->next ) /* line min_i was deleted */ |
| { |
| for( cur_v = v_head.next; cur_v != 0; cur_v = cur_v->next ) |
| { |
| j = (int)(cur_v - v); |
| if( cur_v->val == cost[min_i][j] ) /* column j needs updating */ |
| { |
| float max_val = -CV_EMD_INF; |
| |
| /* find the new maximum value in the column */ |
| for( cur_u = u_head.next; cur_u != 0; cur_u = cur_u->next ) |
| { |
| float temp = cost[cur_u - u][j]; |
| |
| if( max_val < temp ) |
| max_val = temp; |
| } |
| |
| /* if needed, adjust the relevant delta[*][j] */ |
| diff = max_val - cur_v->val; |
| cur_v->val = max_val; |
| if( fabs( diff ) < eps ) |
| { |
| for( cur_u = u_head.next; cur_u != 0; cur_u = cur_u->next ) |
| delta[cur_u - u][j] += diff; |
| } |
| } |
| } |
| } |
| else /* column min_j was deleted */ |
| { |
| for( cur_u = u_head.next; cur_u != 0; cur_u = cur_u->next ) |
| { |
| i = (int)(cur_u - u); |
| if( cur_u->val == cost[i][min_j] ) /* row i needs updating */ |
| { |
| float max_val = -CV_EMD_INF; |
| |
| /* find the new maximum value in the row */ |
| for( cur_v = v_head.next; cur_v != 0; cur_v = cur_v->next ) |
| { |
| float temp = cost[i][cur_v - v]; |
| |
| if( max_val < temp ) |
| max_val = temp; |
| } |
| |
| /* if needed, adjust the relevant delta[i][*] */ |
| diff = max_val - cur_u->val; |
| cur_u->val = max_val; |
| |
| if( fabs( diff ) < eps ) |
| { |
| for( cur_v = v_head.next; cur_v != 0; cur_v = cur_v->next ) |
| delta[i][cur_v - v] += diff; |
| } |
| } |
| } |
| } |
| } |
| while( u_head.next != 0 || v_head.next != 0 ); |
| } |
| |
| |
| |
| /****************************************************************************************\ |
| * icvAddBasicVariable * |
| \****************************************************************************************/ |
| static void |
| icvAddBasicVariable( CvEMDState * state, |
| int min_i, int min_j, |
| CvNode1D * prev_u_min_i, CvNode1D * prev_v_min_j, CvNode1D * u_head ) |
| { |
| float temp; |
| CvNode2D *end_x = state->end_x; |
| |
| if( state->s[min_i] < state->d[min_j] + state->weight * CV_EMD_EPS ) |
| { /* supply exhausted */ |
| temp = state->s[min_i]; |
| state->s[min_i] = 0; |
| state->d[min_j] -= temp; |
| } |
| else /* demand exhausted */ |
| { |
| temp = state->d[min_j]; |
| state->d[min_j] = 0; |
| state->s[min_i] -= temp; |
| } |
| |
| /* x(min_i,min_j) is a basic variable */ |
| state->is_x[min_i][min_j] = 1; |
| |
| end_x->val = temp; |
| end_x->i = min_i; |
| end_x->j = min_j; |
| end_x->next[0] = state->rows_x[min_i]; |
| end_x->next[1] = state->cols_x[min_j]; |
| state->rows_x[min_i] = end_x; |
| state->cols_x[min_j] = end_x; |
| state->end_x = end_x + 1; |
| |
| /* delete supply row only if the empty, and if not last row */ |
| if( state->s[min_i] == 0 && u_head->next->next != 0 ) |
| prev_u_min_i->next = prev_u_min_i->next->next; /* remove row from list */ |
| else |
| prev_v_min_j->next = prev_v_min_j->next->next; /* remove column from list */ |
| } |
| |
| |
| /****************************************************************************************\ |
| * standard metrics * |
| \****************************************************************************************/ |
| static float |
| icvDistL1( const float *x, const float *y, void *user_param ) |
| { |
| int i, dims = (int)(size_t)user_param; |
| double s = 0; |
| |
| for( i = 0; i < dims; i++ ) |
| { |
| double t = x[i] - y[i]; |
| |
| s += fabs( t ); |
| } |
| return (float)s; |
| } |
| |
| static float |
| icvDistL2( const float *x, const float *y, void *user_param ) |
| { |
| int i, dims = (int)(size_t)user_param; |
| double s = 0; |
| |
| for( i = 0; i < dims; i++ ) |
| { |
| double t = x[i] - y[i]; |
| |
| s += t * t; |
| } |
| return cvSqrt( (float)s ); |
| } |
| |
| static float |
| icvDistC( const float *x, const float *y, void *user_param ) |
| { |
| int i, dims = (int)(size_t)user_param; |
| double s = 0; |
| |
| for( i = 0; i < dims; i++ ) |
| { |
| double t = fabs( x[i] - y[i] ); |
| |
| if( s < t ) |
| s = t; |
| } |
| return (float)s; |
| } |
| |
| /* End of file. */ |
| |