emd.cpp

00001 /*M///////////////////////////////////////////////////////////////////////////////////////
00002 //
00003 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
00004 //
00005 //  By downloading, copying, installing or using the software you agree to this license.
00006 //  If you do not agree to this license, do not download, install,
00007 //  copy or use the software.
00008 //
00009 //
00010 //                        Intel License Agreement
00011 //                For Open Source Computer Vision Library
00012 //
00013 // Copyright (C) 2000, Intel Corporation, all rights reserved.
00014 // Third party copyrights are property of their respective owners.
00015 //
00016 // Redistribution and use in source and binary forms, with or without modification,
00017 // are permitted provided that the following conditions are met:
00018 //
00019 //   * Redistribution's of source code must retain the above copyright notice,
00020 //     this list of conditions and the following disclaimer.
00021 //
00022 //   * Redistribution's in binary form must reproduce the above copyright notice,
00023 //     this list of conditions and the following disclaimer in the documentation
00024 //     and/or other materials provided with the distribution.
00025 //
00026 //   * The name of Intel Corporation may not be used to endorse or promote products
00027 //     derived from this software without specific prior written permission.
00028 //
00029 // This software is provided by the copyright holders and contributors "as is" and
00030 // any express or implied warranties, including, but not limited to, the implied
00031 // warranties of merchantability and fitness for a particular purpose are disclaimed.
00032 // In no event shall the Intel Corporation or contributors be liable for any direct,
00033 // indirect, incidental, special, exemplary, or consequential damages
00034 // (including, but not limited to, procurement of substitute goods or services;
00035 // loss of use, data, or profits; or business interruption) however caused
00036 // and on any theory of liability, whether in contract, strict liability,
00037 // or tort (including negligence or otherwise) arising in any way out of
00038 // the use of this software, even if advised of the possibility of such damage.
00039 //
00040 //M*/
00041 
00042 /*
00043     Partially based on Yossi Rubner code:
00044     =========================================================================
00045     emd.c
00046 
00047     Last update: 3/14/98
00048 
00049     An implementation of the Earth Movers Distance.
00050     Based of the solution for the Transportation problem as described in
00051     "Introduction to Mathematical Programming" by F. S. Hillier and
00052     G. J. Lieberman, McGraw-Hill, 1990.
00053 
00054     Copyright (C) 1998 Yossi Rubner
00055     Computer Science Department, Stanford University
00056     E-Mail: rubner@cs.stanford.edu   URL: http://vision.stanford.edu/~rubner
00057     ==========================================================================
00058 */
00059 #include "precomp.hpp"
00060 
00061 #define MAX_ITERATIONS 500
00062 #define CV_EMD_INF   ((float)1e20)
00063 #define CV_EMD_EPS   ((float)1e-5)
00064 
00065 /* CvNode1D is used for lists, representing 1D sparse array */
00066 typedef struct CvNode1D
00067 {
00068     float val;
00069     struct CvNode1D *next;
00070 }
00071 CvNode1D;
00072 
00073 /* CvNode2D is used for lists, representing 2D sparse matrix */
00074 typedef struct CvNode2D
00075 {
00076     float val;
00077     struct CvNode2D *next[2];  /* next row & next column */
00078     int i, j;
00079 }
00080 CvNode2D;
00081 
00082 
00083 typedef struct CvEMDState
00084 {
00085     int ssize, dsize;
00086 
00087     float **cost;
00088     CvNode2D *_x;
00089     CvNode2D *end_x;
00090     CvNode2D *enter_x;
00091     char **is_x;
00092 
00093     CvNode2D **rows_x;
00094     CvNode2D **cols_x;
00095 
00096     CvNode1D *u;
00097     CvNode1D *v;
00098 
00099     int* idx1;
00100     int* idx2;
00101 
00102     /* find_loop buffers */
00103     CvNode2D **loop;
00104     char *is_used;
00105 
00106     /* russel buffers */
00107     float *s;
00108     float *d;
00109     float **delta;
00110 
00111     float weight, max_cost;
00112     char *buffer;
00113 }
00114 CvEMDState;
00115 
00116 /* static function declaration */
00117 static int icvInitEMD( const float *signature1, int size1,
00118                        const float *signature2, int size2,
00119                        int dims, CvDistanceFunction dist_func, void *user_param,
00120                        const float* cost, int cost_step,
00121                        CvEMDState * state, float *lower_bound,
00122                        cv::AutoBuffer<char>& _buffer );
00123 
00124 static int icvFindBasicVariables( float **cost, char **is_x,
00125                                   CvNode1D * u, CvNode1D * v, int ssize, int dsize );
00126 
00127 static float icvIsOptimal( float **cost, char **is_x,
00128                            CvNode1D * u, CvNode1D * v,
00129                            int ssize, int dsize, CvNode2D * enter_x );
00130 
00131 static void icvRussel( CvEMDState * state );
00132 
00133 
00134 static bool icvNewSolution( CvEMDState * state );
00135 static int icvFindLoop( CvEMDState * state );
00136 
00137 static void icvAddBasicVariable( CvEMDState * state,
00138                                  int min_i, int min_j,
00139                                  CvNode1D * prev_u_min_i,
00140                                  CvNode1D * prev_v_min_j,
00141                                  CvNode1D * u_head );
00142 
00143 static float icvDistL2( const float *x, const float *y, void *user_param );
00144 static float icvDistL1( const float *x, const float *y, void *user_param );
00145 static float icvDistC( const float *x, const float *y, void *user_param );
00146 
00147 /* The main function */
00148 CV_IMPL float cvCalcEMD2( const CvArr* signature_arr1,
00149             const CvArr* signature_arr2,
00150             int dist_type,
00151             CvDistanceFunction dist_func,
00152             const CvArr* cost_matrix,
00153             CvArr* flow_matrix,
00154             float *lower_bound,
00155             void *user_param )
00156 {
00157     cv::AutoBuffer<char> local_buf;
00158     CvEMDState state;
00159     float emd = 0;
00160 
00161     memset( &state, 0, sizeof(state));
00162 
00163     double total_cost = 0;
00164     int result = 0;
00165     float eps, min_delta;
00166     CvNode2D *xp = 0;
00167     CvMat sign_stub1, *signature1 = (CvMat*)signature_arr1;
00168     CvMat sign_stub2, *signature2 = (CvMat*)signature_arr2;
00169     CvMat cost_stub, *cost = &cost_stub;
00170     CvMat flow_stub, *flow = (CvMat*)flow_matrix;
00171     int dims, size1, size2;
00172 
00173     signature1 = cvGetMat( signature1, &sign_stub1 );
00174     signature2 = cvGetMat( signature2, &sign_stub2 );
00175 
00176     if( signature1->cols != signature2->cols )
00177         CV_Error( CV_StsUnmatchedSizes, "The arrays must have equal number of columns (which is number of dimensions but 1)" );
00178 
00179     dims = signature1->cols - 1;
00180     size1 = signature1->rows;
00181     size2 = signature2->rows;
00182 
00183     if( !CV_ARE_TYPES_EQ( signature1, signature2 ))
00184         CV_Error( CV_StsUnmatchedFormats, "The array must have equal types" );
00185 
00186     if( CV_MAT_TYPE( signature1->type ) != CV_32FC1 )
00187         CV_Error( CV_StsUnsupportedFormat, "The signatures must be 32fC1" );
00188 
00189     if( flow )
00190     {
00191         flow = cvGetMat( flow, &flow_stub );
00192 
00193         if( flow->rows != size1 || flow->cols != size2 )
00194             CV_Error( CV_StsUnmatchedSizes,
00195             "The flow matrix size does not match to the signatures' sizes" );
00196 
00197         if( CV_MAT_TYPE( flow->type ) != CV_32FC1 )
00198             CV_Error( CV_StsUnsupportedFormat, "The flow matrix must be 32fC1" );
00199     }
00200 
00201     cost->data.fl = 0;
00202     cost->step = 0;
00203 
00204     if( dist_type < 0 )
00205     {
00206         if( cost_matrix )
00207         {
00208             if( dist_func )
00209                 CV_Error( CV_StsBadArg,
00210                 "Only one of cost matrix or distance function should be non-NULL in case of user-defined distance" );
00211 
00212             if( lower_bound )
00213                 CV_Error( CV_StsBadArg,
00214                 "The lower boundary can not be calculated if the cost matrix is used" );
00215 
00216             cost = cvGetMat( cost_matrix, &cost_stub );
00217             if( cost->rows != size1 || cost->cols != size2 )
00218                 CV_Error( CV_StsUnmatchedSizes,
00219                 "The cost matrix size does not match to the signatures' sizes" );
00220 
00221             if( CV_MAT_TYPE( cost->type ) != CV_32FC1 )
00222                 CV_Error( CV_StsUnsupportedFormat, "The cost matrix must be 32fC1" );
00223         }
00224         else if( !dist_func )
00225             CV_Error( CV_StsNullPtr, "In case of user-defined distance Distance function is undefined" );
00226     }
00227     else
00228     {
00229         if( dims == 0 )
00230             CV_Error( CV_StsBadSize,
00231             "Number of dimensions can be 0 only if a user-defined metric is used" );
00232         user_param = (void *) (size_t)dims;
00233         switch (dist_type)
00234         {
00235         case CV_DIST_L1:
00236             dist_func = icvDistL1;
00237             break;
00238         case CV_DIST_L2:
00239             dist_func = icvDistL2;
00240             break;
00241         case CV_DIST_C:
00242             dist_func = icvDistC;
00243             break;
00244         default:
00245             CV_Error( CV_StsBadFlag, "Bad or unsupported metric type" );
00246         }
00247     }
00248 
00249     result = icvInitEMD( signature1->data.fl, size1,
00250                         signature2->data.fl, size2,
00251                         dims, dist_func, user_param,
00252                         cost->data.fl, cost->step,
00253                         &state, lower_bound, local_buf );
00254 
00255     if( result > 0 && lower_bound )
00256     {
00257         emd = *lower_bound;
00258         return emd;
00259     }
00260 
00261     eps = CV_EMD_EPS * state.max_cost;
00262 
00263     /* if ssize = 1 or dsize = 1 then we are done, else ... */
00264     if( state.ssize > 1 && state.dsize > 1 )
00265     {
00266         int itr;
00267 
00268         for( itr = 1; itr < MAX_ITERATIONS; itr++ )
00269         {
00270             /* find basic variables */
00271             result = icvFindBasicVariables( state.cost, state.is_x,
00272                                             state.u, state.v, state.ssize, state.dsize );
00273             if( result < 0 )
00274                 break;
00275 
00276             /* check for optimality */
00277             min_delta = icvIsOptimal( state.cost, state.is_x,
00278                                       state.u, state.v,
00279                                       state.ssize, state.dsize, state.enter_x );
00280 
00281             if( min_delta == CV_EMD_INF )
00282                 CV_Error( CV_StsNoConv, "" );
00283 
00284             /* if no negative deltamin, we found the optimal solution */
00285             if( min_delta >= -eps )
00286                 break;
00287 
00288             /* improve solution */
00289             if(!icvNewSolution( &state ))
00290                 CV_Error( CV_StsNoConv, "" );
00291         }
00292     }
00293 
00294     /* compute the total flow */
00295     for( xp = state._x; xp < state.end_x; xp++ )
00296     {
00297         float val = xp->val;
00298         int i = xp->i;
00299         int j = xp->j;
00300 
00301         if( xp == state.enter_x )
00302           continue;
00303 
00304         int ci = state.idx1[i];
00305         int cj = state.idx2[j];
00306 
00307         if( ci >= 0 && cj >= 0 )
00308         {
00309             total_cost += (double)val * state.cost[i][j];
00310             if( flow )
00311                 ((float*)(flow->data.ptr + flow->step*ci))[cj] = val;
00312         }
00313     }
00314 
00315     emd = (float) (total_cost / state.weight);
00316     return emd;
00317 }
00318 
00319 
00320 /************************************************************************************\
00321 *          initialize structure, allocate buffers and generate initial golution      *
00322 \************************************************************************************/
00323 static int icvInitEMD( const float* signature1, int size1,
00324             const float* signature2, int size2,
00325             int dims, CvDistanceFunction dist_func, void* user_param,
00326             const float* cost, int cost_step,
00327             CvEMDState* state, float* lower_bound,
00328             cv::AutoBuffer<char>& _buffer )
00329 {
00330     float s_sum = 0, d_sum = 0, diff;
00331     int i, j;
00332     int ssize = 0, dsize = 0;
00333     int equal_sums = 1;
00334     int buffer_size;
00335     float max_cost = 0;
00336     char *buffer, *buffer_end;
00337 
00338     memset( state, 0, sizeof( *state ));
00339     assert( cost_step % sizeof(float) == 0 );
00340     cost_step /= sizeof(float);
00341 
00342     /* calculate buffer size */
00343     buffer_size = (size1+1) * (size2+1) * (sizeof( float ) +    /* cost */
00344                                    sizeof( char ) +     /* is_x */
00345                                    sizeof( float )) +   /* delta matrix */
00346         (size1 + size2 + 2) * (sizeof( CvNode2D ) + /* _x */
00347                            sizeof( CvNode2D * ) +  /* cols_x & rows_x */
00348                            sizeof( CvNode1D ) + /* u & v */
00349                            sizeof( float ) + /* s & d */
00350                            sizeof( int ) + sizeof(CvNode2D*)) +  /* idx1 & idx2 */
00351         (size1+1) * (sizeof( float * ) + sizeof( char * ) + /* rows pointers for */
00352                  sizeof( float * )) + 256;      /*  cost, is_x and delta */
00353 
00354     if( buffer_size < (int) (dims * 2 * sizeof( float )))
00355     {
00356         buffer_size = dims * 2 * sizeof( float );
00357     }
00358 
00359     /* allocate buffers */
00360     _buffer.allocate(buffer_size);
00361 
00362     state->buffer = buffer = _buffer;
00363     buffer_end = buffer + buffer_size;
00364 
00365     state->idx1 = (int*) buffer;
00366     buffer += (size1 + 1) * sizeof( int );
00367 
00368     state->idx2 = (int*) buffer;
00369     buffer += (size2 + 1) * sizeof( int );
00370 
00371     state->s = (float *) buffer;
00372     buffer += (size1 + 1) * sizeof( float );
00373 
00374     state->d = (float *) buffer;
00375     buffer += (size2 + 1) * sizeof( float );
00376 
00377     /* sum up the supply and demand */
00378     for( i = 0; i < size1; i++ )
00379     {
00380         float weight = signature1[i * (dims + 1)];
00381 
00382         if( weight > 0 )
00383         {
00384             s_sum += weight;
00385             state->s[ssize] = weight;
00386             state->idx1[ssize++] = i;
00387 
00388         }
00389         else if( weight < 0 )
00390             CV_Error(CV_StsOutOfRange, "");
00391     }
00392 
00393     for( i = 0; i < size2; i++ )
00394     {
00395         float weight = signature2[i * (dims + 1)];
00396 
00397         if( weight > 0 )
00398         {
00399             d_sum += weight;
00400             state->d[dsize] = weight;
00401             state->idx2[dsize++] = i;
00402         }
00403         else if( weight < 0 )
00404             CV_Error(CV_StsOutOfRange, "");
00405     }
00406 
00407     if( ssize == 0 || dsize == 0 )
00408         CV_Error(CV_StsOutOfRange, "");
00409 
00410     /* if supply different than the demand, add a zero-cost dummy cluster */
00411     diff = s_sum - d_sum;
00412     if( fabs( diff ) >= CV_EMD_EPS * s_sum )
00413     {
00414         equal_sums = 0;
00415         if( diff < 0 )
00416         {
00417             state->s[ssize] = -diff;
00418             state->idx1[ssize++] = -1;
00419         }
00420         else
00421         {
00422             state->d[dsize] = diff;
00423             state->idx2[dsize++] = -1;
00424         }
00425     }
00426 
00427     state->ssize = ssize;
00428     state->dsize = dsize;
00429     state->weight = s_sum > d_sum ? s_sum : d_sum;
00430 
00431     if( lower_bound && equal_sums )     /* check lower bound */
00432     {
00433         int sz1 = size1 * (dims + 1), sz2 = size2 * (dims + 1);
00434         float lb = 0;
00435 
00436         float* xs = (float *) buffer;
00437         float* xd = xs + dims;
00438 
00439         memset( xs, 0, dims*sizeof(xs[0]));
00440         memset( xd, 0, dims*sizeof(xd[0]));
00441 
00442         for( j = 0; j < sz1; j += dims + 1 )
00443         {
00444             float weight = signature1[j];
00445             for( i = 0; i < dims; i++ )
00446                 xs[i] += signature1[j + i + 1] * weight;
00447         }
00448 
00449         for( j = 0; j < sz2; j += dims + 1 )
00450         {
00451             float weight = signature2[j];
00452             for( i = 0; i < dims; i++ )
00453                 xd[i] += signature2[j + i + 1] * weight;
00454         }
00455 
00456         lb = dist_func( xs, xd, user_param ) / state->weight;
00457         i = *lower_bound <= lb;
00458         *lower_bound = lb;
00459         if( i )
00460             return 1;
00461     }
00462 
00463     /* assign pointers */
00464     state->is_used = (char *) buffer;
00465     /* init delta matrix */
00466     state->delta = (float **) buffer;
00467     buffer += ssize * sizeof( float * );
00468 
00469     for( i = 0; i < ssize; i++ )
00470     {
00471         state->delta[i] = (float *) buffer;
00472         buffer += dsize * sizeof( float );
00473     }
00474 
00475     state->loop = (CvNode2D **) buffer;
00476     buffer += (ssize + dsize + 1) * sizeof(CvNode2D*);
00477 
00478     state->_x = state->end_x = (CvNode2D *) buffer;
00479     buffer += (ssize + dsize) * sizeof( CvNode2D );
00480 
00481     /* init cost matrix */
00482     state->cost = (float **) buffer;
00483     buffer += ssize * sizeof( float * );
00484 
00485     /* compute the distance matrix */
00486     for( i = 0; i < ssize; i++ )
00487     {
00488         int ci = state->idx1[i];
00489 
00490         state->cost[i] = (float *) buffer;
00491         buffer += dsize * sizeof( float );
00492 
00493         if( ci >= 0 )
00494         {
00495             for( j = 0; j < dsize; j++ )
00496             {
00497                 int cj = state->idx2[j];
00498                 if( cj < 0 )
00499                     state->cost[i][j] = 0;
00500                 else
00501                 {
00502                     float val;
00503                     if( dist_func )
00504                     {
00505                         val = dist_func( signature1 + ci * (dims + 1) + 1,
00506                                          signature2 + cj * (dims + 1) + 1,
00507                                          user_param );
00508                     }
00509                     else
00510                     {
00511                         assert( cost );
00512                         val = cost[cost_step*ci + cj];
00513                     }
00514                     state->cost[i][j] = val;
00515                     if( max_cost < val )
00516                         max_cost = val;
00517                 }
00518             }
00519         }
00520         else
00521         {
00522             for( j = 0; j < dsize; j++ )
00523                 state->cost[i][j] = 0;
00524         }
00525     }
00526 
00527     state->max_cost = max_cost;
00528 
00529     memset( buffer, 0, buffer_end - buffer );
00530 
00531     state->rows_x = (CvNode2D **) buffer;
00532     buffer += ssize * sizeof( CvNode2D * );
00533 
00534     state->cols_x = (CvNode2D **) buffer;
00535     buffer += dsize * sizeof( CvNode2D * );
00536 
00537     state->u = (CvNode1D *) buffer;
00538     buffer += ssize * sizeof( CvNode1D );
00539 
00540     state->v = (CvNode1D *) buffer;
00541     buffer += dsize * sizeof( CvNode1D );
00542 
00543     /* init is_x matrix */
00544     state->is_x = (char **) buffer;
00545     buffer += ssize * sizeof( char * );
00546 
00547     for( i = 0; i < ssize; i++ )
00548     {
00549         state->is_x[i] = buffer;
00550         buffer += dsize;
00551     }
00552 
00553     assert( buffer <= buffer_end );
00554 
00555     icvRussel( state );
00556 
00557     state->enter_x = (state->end_x)++;
00558     return 0;
00559 }
00560 
00561 
00562 /****************************************************************************************\
00563 *                              icvFindBasicVariables                                   *
00564 \****************************************************************************************/
00565 static int icvFindBasicVariables( float **cost, char **is_x,
00566                        CvNode1D * u, CvNode1D * v, int ssize, int dsize )
00567 {
00568     int i, j, found;
00569     int u_cfound, v_cfound;
00570     CvNode1D u0_head, u1_head, *cur_u, *prev_u;
00571     CvNode1D v0_head, v1_head, *cur_v, *prev_v;
00572 
00573     /* initialize the rows list (u) and the columns list (v) */
00574     u0_head.next = u;
00575     for( i = 0; i < ssize; i++ )
00576     {
00577         u[i].next = u + i + 1;
00578     }
00579     u[ssize - 1].next = 0;
00580     u1_head.next = 0;
00581 
00582     v0_head.next = ssize > 1 ? v + 1 : 0;
00583     for( i = 1; i < dsize; i++ )
00584     {
00585         v[i].next = v + i + 1;
00586     }
00587     v[dsize - 1].next = 0;
00588     v1_head.next = 0;
00589 
00590     /* there are ssize+dsize variables but only ssize+dsize-1 independent equations,
00591        so set v[0]=0 */
00592     v[0].val = 0;
00593     v1_head.next = v;
00594     v1_head.next->next = 0;
00595 
00596     /* loop until all variables are found */
00597     u_cfound = v_cfound = 0;
00598     while( u_cfound < ssize || v_cfound < dsize )
00599     {
00600         found = 0;
00601         if( v_cfound < dsize )
00602         {
00603             /* loop over all marked columns */
00604             prev_v = &v1_head;
00605 
00606             for( found |= (cur_v = v1_head.next) != 0; cur_v != 0; cur_v = cur_v->next )
00607             {
00608                 float cur_v_val = cur_v->val;
00609 
00610                 j = (int)(cur_v - v);
00611                 /* find the variables in column j */
00612                 prev_u = &u0_head;
00613                 for( cur_u = u0_head.next; cur_u != 0; )
00614                 {
00615                     i = (int)(cur_u - u);
00616                     if( is_x[i][j] )
00617                     {
00618                         /* compute u[i] */
00619                         cur_u->val = cost[i][j] - cur_v_val;
00620                         /* ...and add it to the marked list */
00621                         prev_u->next = cur_u->next;
00622                         cur_u->next = u1_head.next;
00623                         u1_head.next = cur_u;
00624                         cur_u = prev_u->next;
00625                     }
00626                     else
00627                     {
00628                         prev_u = cur_u;
00629                         cur_u = cur_u->next;
00630                     }
00631                 }
00632                 prev_v->next = cur_v->next;
00633                 v_cfound++;
00634             }
00635         }
00636 
00637         if( u_cfound < ssize )
00638         {
00639             /* loop over all marked rows */
00640             prev_u = &u1_head;
00641             for( found |= (cur_u = u1_head.next) != 0; cur_u != 0; cur_u = cur_u->next )
00642             {
00643                 float cur_u_val = cur_u->val;
00644                 float *_cost;
00645                 char *_is_x;
00646 
00647                 i = (int)(cur_u - u);
00648                 _cost = cost[i];
00649                 _is_x = is_x[i];
00650                 /* find the variables in rows i */
00651                 prev_v = &v0_head;
00652                 for( cur_v = v0_head.next; cur_v != 0; )
00653                 {
00654                     j = (int)(cur_v - v);
00655                     if( _is_x[j] )
00656                     {
00657                         /* compute v[j] */
00658                         cur_v->val = _cost[j] - cur_u_val;
00659                         /* ...and add it to the marked list */
00660                         prev_v->next = cur_v->next;
00661                         cur_v->next = v1_head.next;
00662                         v1_head.next = cur_v;
00663                         cur_v = prev_v->next;
00664                     }
00665                     else
00666                     {
00667                         prev_v = cur_v;
00668                         cur_v = cur_v->next;
00669                     }
00670                 }
00671                 prev_u->next = cur_u->next;
00672                 u_cfound++;
00673             }
00674         }
00675 
00676         if( !found )
00677             return -1;
00678     }
00679 
00680     return 0;
00681 }
00682 
00683 
00684 /****************************************************************************************\
00685 *                                   icvIsOptimal                                       *
00686 \****************************************************************************************/
00687 static float
00688 icvIsOptimal( float **cost, char **is_x,
00689               CvNode1D * u, CvNode1D * v, int ssize, int dsize, CvNode2D * enter_x )
00690 {
00691     float delta, min_delta = CV_EMD_INF;
00692     int i, j, min_i = 0, min_j = 0;
00693 
00694     /* find the minimal cij-ui-vj over all i,j */
00695     for( i = 0; i < ssize; i++ )
00696     {
00697         float u_val = u[i].val;
00698         float *_cost = cost[i];
00699         char *_is_x = is_x[i];
00700 
00701         for( j = 0; j < dsize; j++ )
00702         {
00703             if( !_is_x[j] )
00704             {
00705                 delta = _cost[j] - u_val - v[j].val;
00706                 if( min_delta > delta )
00707                 {
00708                     min_delta = delta;
00709                     min_i = i;
00710                     min_j = j;
00711                 }
00712             }
00713         }
00714     }
00715 
00716     enter_x->i = min_i;
00717     enter_x->j = min_j;
00718 
00719     return min_delta;
00720 }
00721 
00722 /****************************************************************************************\
00723 *                                   icvNewSolution                                     *
00724 \****************************************************************************************/
00725 static bool
00726 icvNewSolution( CvEMDState * state )
00727 {
00728     int i, j;
00729     float min_val = CV_EMD_INF;
00730     int steps;
00731     CvNode2D head, *cur_x, *next_x, *leave_x = 0;
00732     CvNode2D *enter_x = state->enter_x;
00733     CvNode2D **loop = state->loop;
00734 
00735     /* enter the new basic variable */
00736     i = enter_x->i;
00737     j = enter_x->j;
00738     state->is_x[i][j] = 1;
00739     enter_x->next[0] = state->rows_x[i];
00740     enter_x->next[1] = state->cols_x[j];
00741     enter_x->val = 0;
00742     state->rows_x[i] = enter_x;
00743     state->cols_x[j] = enter_x;
00744 
00745     /* find a chain reaction */
00746     steps = icvFindLoop( state );
00747 
00748     if( steps == 0 )
00749         return false;
00750 
00751     /* find the largest value in the loop */
00752     for( i = 1; i < steps; i += 2 )
00753     {
00754         float temp = loop[i]->val;
00755 
00756         if( min_val > temp )
00757         {
00758             leave_x = loop[i];
00759             min_val = temp;
00760         }
00761     }
00762 
00763     /* update the loop */
00764     for( i = 0; i < steps; i += 2 )
00765     {
00766         float temp0 = loop[i]->val + min_val;
00767         float temp1 = loop[i + 1]->val - min_val;
00768 
00769         loop[i]->val = temp0;
00770         loop[i + 1]->val = temp1;
00771     }
00772 
00773     /* remove the leaving basic variable */
00774     i = leave_x->i;
00775     j = leave_x->j;
00776     state->is_x[i][j] = 0;
00777 
00778     head.next[0] = state->rows_x[i];
00779     cur_x = &head;
00780     while( (next_x = cur_x->next[0]) != leave_x )
00781     {
00782         cur_x = next_x;
00783         assert( cur_x );
00784     }
00785     cur_x->next[0] = next_x->next[0];
00786     state->rows_x[i] = head.next[0];
00787 
00788     head.next[1] = state->cols_x[j];
00789     cur_x = &head;
00790     while( (next_x = cur_x->next[1]) != leave_x )
00791     {
00792         cur_x = next_x;
00793         assert( cur_x );
00794     }
00795     cur_x->next[1] = next_x->next[1];
00796     state->cols_x[j] = head.next[1];
00797 
00798     /* set enter_x to be the new empty slot */
00799     state->enter_x = leave_x;
00800 
00801     return true;
00802 }
00803 
00804 
00805 
00806 /****************************************************************************************\
00807 *                                    icvFindLoop                                       *
00808 \****************************************************************************************/
00809 static int
00810 icvFindLoop( CvEMDState * state )
00811 {
00812     int i, steps = 1;
00813     CvNode2D *new_x;
00814     CvNode2D **loop = state->loop;
00815     CvNode2D *enter_x = state->enter_x, *_x = state->_x;
00816     char *is_used = state->is_used;
00817 
00818     memset( is_used, 0, state->ssize + state->dsize );
00819 
00820     new_x = loop[0] = enter_x;
00821     is_used[enter_x - _x] = 1;
00822     steps = 1;
00823 
00824     do
00825     {
00826         if( (steps & 1) == 1 )
00827         {
00828             /* find an unused x in the row */
00829             new_x = state->rows_x[new_x->i];
00830             while( new_x != 0 && is_used[new_x - _x] )
00831                 new_x = new_x->next[0];
00832         }
00833         else
00834         {
00835             /* find an unused x in the column, or the entering x */
00836             new_x = state->cols_x[new_x->j];
00837             while( new_x != 0 && is_used[new_x - _x] && new_x != enter_x )
00838                 new_x = new_x->next[1];
00839             if( new_x == enter_x )
00840                 break;
00841         }
00842 
00843         if( new_x != 0 )        /* found the next x */
00844         {
00845             /* add x to the loop */
00846             loop[steps++] = new_x;
00847             is_used[new_x - _x] = 1;
00848         }
00849         else                    /* didn't find the next x */
00850         {
00851             /* backtrack */
00852             do
00853             {
00854                 i = steps & 1;
00855                 new_x = loop[steps - 1];
00856                 do
00857                 {
00858                     new_x = new_x->next[i];
00859                 }
00860                 while( new_x != 0 && is_used[new_x - _x] );
00861 
00862                 if( new_x == 0 )
00863                 {
00864                     is_used[loop[--steps] - _x] = 0;
00865                 }
00866             }
00867             while( new_x == 0 && steps > 0 );
00868 
00869             is_used[loop[steps - 1] - _x] = 0;
00870             loop[steps - 1] = new_x;
00871             is_used[new_x - _x] = 1;
00872         }
00873     }
00874     while( steps > 0 );
00875 
00876     return steps;
00877 }
00878 
00879 
00880 
00881 /****************************************************************************************\
00882 *                                        icvRussel                                     *
00883 \****************************************************************************************/
00884 static void
00885 icvRussel( CvEMDState * state )
00886 {
00887     int i, j, min_i = -1, min_j = -1;
00888     float min_delta, diff;
00889     CvNode1D u_head, *cur_u, *prev_u;
00890     CvNode1D v_head, *cur_v, *prev_v;
00891     CvNode1D *prev_u_min_i = 0, *prev_v_min_j = 0, *remember;
00892     CvNode1D *u = state->u, *v = state->v;
00893     int ssize = state->ssize, dsize = state->dsize;
00894     float eps = CV_EMD_EPS * state->max_cost;
00895     float **cost = state->cost;
00896     float **delta = state->delta;
00897 
00898     /* initialize the rows list (ur), and the columns list (vr) */
00899     u_head.next = u;
00900     for( i = 0; i < ssize; i++ )
00901     {
00902         u[i].next = u + i + 1;
00903     }
00904     u[ssize - 1].next = 0;
00905 
00906     v_head.next = v;
00907     for( i = 0; i < dsize; i++ )
00908     {
00909         v[i].val = -CV_EMD_INF;
00910         v[i].next = v + i + 1;
00911     }
00912     v[dsize - 1].next = 0;
00913 
00914     /* find the maximum row and column values (ur[i] and vr[j]) */
00915     for( i = 0; i < ssize; i++ )
00916     {
00917         float u_val = -CV_EMD_INF;
00918         float *cost_row = cost[i];
00919 
00920         for( j = 0; j < dsize; j++ )
00921         {
00922             float temp = cost_row[j];
00923 
00924             if( u_val < temp )
00925                 u_val = temp;
00926             if( v[j].val < temp )
00927                 v[j].val = temp;
00928         }
00929         u[i].val = u_val;
00930     }
00931 
00932     /* compute the delta matrix */
00933     for( i = 0; i < ssize; i++ )
00934     {
00935         float u_val = u[i].val;
00936         float *delta_row = delta[i];
00937         float *cost_row = cost[i];
00938 
00939         for( j = 0; j < dsize; j++ )
00940         {
00941             delta_row[j] = cost_row[j] - u_val - v[j].val;
00942         }
00943     }
00944 
00945     /* find the basic variables */
00946     do
00947     {
00948         /* find the smallest delta[i][j] */
00949         min_i = -1;
00950         min_delta = CV_EMD_INF;
00951         prev_u = &u_head;
00952         for( cur_u = u_head.next; cur_u != 0; cur_u = cur_u->next )
00953         {
00954             i = (int)(cur_u - u);
00955             float *delta_row = delta[i];
00956 
00957             prev_v = &v_head;
00958             for( cur_v = v_head.next; cur_v != 0; cur_v = cur_v->next )
00959             {
00960                 j = (int)(cur_v - v);
00961                 if( min_delta > delta_row[j] )
00962                 {
00963                     min_delta = delta_row[j];
00964                     min_i = i;
00965                     min_j = j;
00966                     prev_u_min_i = prev_u;
00967                     prev_v_min_j = prev_v;
00968                 }
00969                 prev_v = cur_v;
00970             }
00971             prev_u = cur_u;
00972         }
00973 
00974         if( min_i < 0 )
00975             break;
00976 
00977         /* add x[min_i][min_j] to the basis, and adjust supplies and cost */
00978         remember = prev_u_min_i->next;
00979         icvAddBasicVariable( state, min_i, min_j, prev_u_min_i, prev_v_min_j, &u_head );
00980 
00981         /* update the necessary delta[][] */
00982         if( remember == prev_u_min_i->next )    /* line min_i was deleted */
00983         {
00984             for( cur_v = v_head.next; cur_v != 0; cur_v = cur_v->next )
00985             {
00986                 j = (int)(cur_v - v);
00987                 if( cur_v->val == cost[min_i][j] )      /* column j needs updating */
00988                 {
00989                     float max_val = -CV_EMD_INF;
00990 
00991                     /* find the new maximum value in the column */
00992                     for( cur_u = u_head.next; cur_u != 0; cur_u = cur_u->next )
00993                     {
00994                         float temp = cost[cur_u - u][j];
00995 
00996                         if( max_val < temp )
00997                             max_val = temp;
00998                     }
00999 
01000                     /* if needed, adjust the relevant delta[*][j] */
01001                     diff = max_val - cur_v->val;
01002                     cur_v->val = max_val;
01003                     if( fabs( diff ) < eps )
01004                     {
01005                         for( cur_u = u_head.next; cur_u != 0; cur_u = cur_u->next )
01006                             delta[cur_u - u][j] += diff;
01007                     }
01008                 }
01009             }
01010         }
01011         else                    /* column min_j was deleted */
01012         {
01013             for( cur_u = u_head.next; cur_u != 0; cur_u = cur_u->next )
01014             {
01015                 i = (int)(cur_u - u);
01016                 if( cur_u->val == cost[i][min_j] )      /* row i needs updating */
01017                 {
01018                     float max_val = -CV_EMD_INF;
01019 
01020                     /* find the new maximum value in the row */
01021                     for( cur_v = v_head.next; cur_v != 0; cur_v = cur_v->next )
01022                     {
01023                         float temp = cost[i][cur_v - v];
01024 
01025                         if( max_val < temp )
01026                             max_val = temp;
01027                     }
01028 
01029                     /* if needed, adjust the relevant delta[i][*] */
01030                     diff = max_val - cur_u->val;
01031                     cur_u->val = max_val;
01032 
01033                     if( fabs( diff ) < eps )
01034                     {
01035                         for( cur_v = v_head.next; cur_v != 0; cur_v = cur_v->next )
01036                             delta[i][cur_v - v] += diff;
01037                     }
01038                 }
01039             }
01040         }
01041     }
01042     while( u_head.next != 0 || v_head.next != 0 );
01043 }
01044 
01045 
01046 
01047 /****************************************************************************************\
01048 *                                   icvAddBasicVariable                                *
01049 \****************************************************************************************/
01050 static void
01051 icvAddBasicVariable( CvEMDState * state,
01052                      int min_i, int min_j,
01053                      CvNode1D * prev_u_min_i, CvNode1D * prev_v_min_j, CvNode1D * u_head )
01054 {
01055     float temp;
01056     CvNode2D *end_x = state->end_x;
01057 
01058     if( state->s[min_i] < state->d[min_j] + state->weight * CV_EMD_EPS )
01059     {                           /* supply exhausted */
01060         temp = state->s[min_i];
01061         state->s[min_i] = 0;
01062         state->d[min_j] -= temp;
01063     }
01064     else                        /* demand exhausted */
01065     {
01066         temp = state->d[min_j];
01067         state->d[min_j] = 0;
01068         state->s[min_i] -= temp;
01069     }
01070 
01071     /* x(min_i,min_j) is a basic variable */
01072     state->is_x[min_i][min_j] = 1;
01073 
01074     end_x->val = temp;
01075     end_x->i = min_i;
01076     end_x->j = min_j;
01077     end_x->next[0] = state->rows_x[min_i];
01078     end_x->next[1] = state->cols_x[min_j];
01079     state->rows_x[min_i] = end_x;
01080     state->cols_x[min_j] = end_x;
01081     state->end_x = end_x + 1;
01082 
01083     /* delete supply row only if the empty, and if not last row */
01084     if( state->s[min_i] == 0 && u_head->next->next != 0 )
01085         prev_u_min_i->next = prev_u_min_i->next->next;  /* remove row from list */
01086     else
01087         prev_v_min_j->next = prev_v_min_j->next->next;  /* remove column from list */
01088 }
01089 
01090 
01091 /****************************************************************************************\
01092 *                                  standard  metrics                                     *
01093 \****************************************************************************************/
01094 static float
01095 icvDistL1( const float *x, const float *y, void *user_param )
01096 {
01097     int i, dims = (int)(size_t)user_param;
01098     double s = 0;
01099 
01100     for( i = 0; i < dims; i++ )
01101     {
01102         double t = x[i] - y[i];
01103 
01104         s += fabs( t );
01105     }
01106     return (float)s;
01107 }
01108 
01109 static float
01110 icvDistL2( const float *x, const float *y, void *user_param )
01111 {
01112     int i, dims = (int)(size_t)user_param;
01113     double s = 0;
01114 
01115     for( i = 0; i < dims; i++ )
01116     {
01117         double t = x[i] - y[i];
01118 
01119         s += t * t;
01120     }
01121     return cvSqrt( (float)s );
01122 }
01123 
01124 static float
01125 icvDistC( const float *x, const float *y, void *user_param )
01126 {
01127     int i, dims = (int)(size_t)user_param;
01128     double s = 0;
01129 
01130     for( i = 0; i < dims; i++ )
01131     {
01132         double t = fabs( x[i] - y[i] );
01133 
01134         if( s < t )
01135             s = t;
01136     }
01137     return (float)s;
01138 }
01139 
01140 
01141 float cv::EMD( InputArray _signature1, InputArray _signature2,
01142                int distType, InputArray _cost,
01143                float* lowerBound, OutputArray _flow )
01144 {
01145     Mat signature1 = _signature1.getMat(), signature2 = _signature2.getMat();
01146     Mat cost = _cost.getMat(), flow;
01147 
01148     CvMat _csignature1 = signature1;
01149     CvMat _csignature2 = signature2;
01150     CvMat _ccost = cost, _cflow;
01151     if( _flow.needed() )
01152     {
01153         _flow.create(signature1.rows, signature2.rows, CV_32F);
01154         flow = _flow.getMat();
01155         flow = Scalar::all(0);
01156         _cflow = flow;
01157     }
01158 
01159     return cvCalcEMD2( &_csignature1, &_csignature2, distType, 0, cost.empty() ? 0 : &_ccost,
01160                        _flow.needed() ? &_cflow : 0, lowerBound, 0 );
01161 }
01162 
01163 /* End of file. */
01164