pkg/src/hungarian.c

   1 /********************************************************************
   2  ********************************************************************
   3  **
   4  ** libhungarian by Cyrill Stachniss, 2004
   5  ** Modified by Benjamin Auder to work on floating point number,
   6  ** and to fit into a R package (2017)
   7  **
   8  ** Solving the Minimum Assignment Problem using the Hungarian Method.
   9  **
  10  ** ** This file may be freely copied and distributed! **
  11  **
  12  ** Parts of the used code was originally provided by the
  13  ** "Stanford GraphGase", but I made changes to this code.
  14  ** As asked by the copyright node of the "Stanford GraphGase",
  15  ** I hereby proclaim that this file are *NOT* part of the
  16  ** "Stanford GraphGase" distrubition!
  17  **
  18  ** This file is distributed in the hope that it will be useful,
  19  ** but WITHOUT ANY WARRANTY; without even the implied
  20  ** warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  21  ** PURPOSE.
  22  **
  23  ********************************************************************
  24  ********************************************************************/
  25
  26 #include <stdio.h>
  27 #include <stdlib.h>
  28
  29 #define HUNGARIAN_NOT_ASSIGNED 0
  30 #define HUNGARIAN_ASSIGNED 1
  31
  32 #define HUNGARIAN_MODE_MINIMIZE_COST 0
  33 #define HUNGARIAN_MODE_MAXIMIZE_UTIL 1
  34
  35 #define INF (1e32)
  36 #define verbose (0)
  37
  38 typedef struct {
  39     int num_rows;
  40     int num_cols;
  41     double** cost;
  42     int** assignment;
  43 } hungarian_problem_t;
  44
  45 int hungarian_imax(int a, int b) {
  46     return (a<b)?b:a;
  47 }
  48
  49 /** This method initialize the hungarian_problem structure and init
  50  *  the  cost matrices (missing lines or columns are filled with 0).
  51  *  It returns the size of the quadratic(!) assignment matrix. **/
  52 int hungarian_init(hungarian_problem_t* p, double** cost_matrix, int rows, int cols,
  53     int mode)
  54 {
  55     int i,j;
  56     double max_cost = 0.;
  57     int org_cols = cols,
  58         org_rows = rows;
  59
  60     // is the number of cols    not equal to number of rows ?
  61     // if yes, expand with 0-cols / 0-cols
  62     rows = hungarian_imax(cols, rows);
  63     cols = rows;
  64
  65     p->num_rows = rows;
  66     p->num_cols = cols;
  67
  68     p->cost = (double**)calloc(rows,sizeof(double*));
  69     p->assignment = (int**)calloc(rows,sizeof(int*));
  70
  71     for(i=0; i<p->num_rows; i++) {
  72         p->cost[i] = (double*)calloc(cols,sizeof(double));
  73         p->assignment[i] = (int*)calloc(cols,sizeof(int));
  74         for(j=0; j<p->num_cols; j++) {
  75             p->cost[i][j] = (i < org_rows && j < org_cols) ? cost_matrix[i][j] : 0.;
  76             p->assignment[i][j] = 0;
  77
  78             if (max_cost < p->cost[i][j])
  79                 max_cost = p->cost[i][j];
  80         }
  81     }
  82
  83     if (mode == HUNGARIAN_MODE_MAXIMIZE_UTIL) {
  84         for(i=0; i<p->num_rows; i++) {
  85             for(j=0; j<p->num_cols; j++)
  86                 p->cost[i][j] = max_cost - p->cost[i][j];
  87         }
  88     }
  89     else if (mode == HUNGARIAN_MODE_MINIMIZE_COST) {
  90         // nothing to do
  91     }
  92 //  else
  93 //      fprintf(stderr,"%s: unknown mode. Mode was set to HUNGARIAN_MODE_MINIMIZE_COST !\n", __FUNCTION__);
  94
  95     return rows;
  96 }
  97
  98 /** Free the memory allocated by init. **/
  99 void hungarian_free(hungarian_problem_t* p) {
 100     int i;
 101     for(i=0; i<p->num_rows; i++) {
 102         free(p->cost[i]);
 103         free(p->assignment[i]);
 104     }
 105     free(p->cost);
 106     free(p->assignment);
 107     p->cost = NULL;
 108     p->assignment = NULL;
 109 }
 110
 111 /** This method computes the optimal assignment. **/
 112 void hungarian_solve(hungarian_problem_t* p)
 113 {
 114     int i, j, m, n, k, l, t, q, unmatched;
 115     double cost, s;
 116     int* col_mate;
 117     int* row_mate;
 118     int* parent_row;
 119     int* unchosen_row;
 120     double* row_dec;
 121     double* col_inc;
 122     double* slack;
 123     int* slack_row;
 124
 125     cost = 0.;
 126     m =p->num_rows;
 127     n =p->num_cols;
 128
 129     col_mate = (int*)calloc(p->num_rows,sizeof(int));
 130     unchosen_row = (int*)calloc(p->num_rows,sizeof(int));
 131     row_dec = (double*)calloc(p->num_rows,sizeof(double));
 132     slack_row   = (int*)calloc(p->num_rows,sizeof(int));
 133
 134     row_mate = (int*)calloc(p->num_cols,sizeof(int));
 135     parent_row = (int*)calloc(p->num_cols,sizeof(int));
 136     col_inc = (double*)calloc(p->num_cols,sizeof(double));
 137     slack = (double*)calloc(p->num_cols,sizeof(double));
 138
 139     for (i=0; i<p->num_rows; i++) {
 140         col_mate[i]=0;
 141         unchosen_row[i]=0;
 142         row_dec[i]=0.;
 143         slack_row[i]=0;
 144     }
 145     for (j=0; j<p->num_cols; j++) {
 146         row_mate[j]=0;
 147         parent_row[j] = 0;
 148         col_inc[j]=0.;
 149         slack[j]=0.;
 150     }
 151
 152     for (i=0; i<p->num_rows; ++i)
 153         for (j=0; j<p->num_cols; ++j)
 154             p->assignment[i][j]=HUNGARIAN_NOT_ASSIGNED;
 155
 156     // Begin subtract column minima in order to start with lots of zeroes 12
 157     for (l=0; l<n; l++)
 158     {
 159         s=p->cost[0][l];
 160         for (k=1; k<m; k++)
 161             if (p->cost[k][l]<s)
 162                 s=p->cost[k][l];
 163         cost+=s;
 164         if (s!=0.)
 165             for (k=0; k<m; k++)
 166                 p->cost[k][l]-=s;
 167     }
 168     // End subtract column minima in order to start with lots of zeroes 12
 169
 170     // Begin initial state 16
 171     t=0;
 172     for (l=0; l<n; l++)
 173     {
 174         row_mate[l]= -1;
 175         parent_row[l]= -1;
 176         col_inc[l]=0.;
 177         slack[l]=INF;
 178     }
 179     for (k=0; k<m; k++)
 180     {
 181         s=p->cost[k][0];
 182         for (l=1; l<n; l++)
 183             if (p->cost[k][l]<s)
 184                 s=p->cost[k][l];
 185         row_dec[k]=s;
 186         for (l=0; l<n; l++)
 187             if (s==p->cost[k][l] && row_mate[l]<0)
 188             {
 189                 col_mate[k]=l;
 190                 row_mate[l]=k;
 191                 goto row_done;
 192             }
 193         col_mate[k]= -1;
 194         unchosen_row[t++]=k;
 195 row_done:
 196         ;
 197     }
 198     // End initial state 16
 199
 200     // Begin Hungarian algorithm 18
 201     if (t==0)
 202         goto done;
 203     unmatched=t;
 204     while (1)
 205     {
 206         q=0;
 207         while (1)
 208         {
 209             while (q<t)
 210             {
 211                 // Begin explore node q of the forest 19
 212                 {
 213                     k=unchosen_row[q];
 214                     s=row_dec[k];
 215                     for (l=0; l<n; l++)
 216                         if (slack[l] > 0.)
 217                         {
 218                             double del=p->cost[k][l]-s+col_inc[l];
 219                             if (del<slack[l])
 220                             {
 221                                 if (del==0.)
 222                                 {
 223                                     if (row_mate[l]<0)
 224                                         goto breakthru;
 225                                     slack[l]=0.;
 226                                     parent_row[l]=k;
 227                                     unchosen_row[t++]=row_mate[l];
 228                                 }
 229                                 else
 230                                 {
 231                                     slack[l]=del;
 232                                     slack_row[l]=k;
 233                                 }
 234                             }
 235                         }
 236                 }
 237                 // End explore node q of the forest 19
 238                 q++;
 239             }
 240
 241             // Begin introduce a new zero into the matrix 21
 242             s=INF;
 243             for (l=0; l<n; l++)
 244                 if (slack[l]>0. && slack[l]<s)
 245                     s=slack[l];
 246             for (q=0; q<t; q++)
 247                 row_dec[unchosen_row[q]]+=s;
 248             for (l=0; l<n; l++)
 249                 if (slack[l]>0.)
 250                 {
 251                     slack[l]-=s;
 252                     if (slack[l]==0.)
 253                     {
 254                         // Begin look at a new zero 22
 255                         k=slack_row[l];
 256                         if (row_mate[l]<0)
 257                         {
 258                             for (j=l+1; j<n; j++)
 259                                 if (slack[j]==0.)
 260                                     col_inc[j]+=s;
 261                             goto breakthru;
 262                         }
 263                         else
 264                         {
 265                             parent_row[l]=k;
 266                             unchosen_row[t++]=row_mate[l];
 267                         }
 268                         // End look at a new zero 22
 269                     }
 270                 }
 271                 else
 272                     col_inc[l]+=s;
 273             // End introduce a new zero into the matrix 21
 274         }
 275 breakthru:
 276         // Begin update the matching 20
 277         while (1)
 278         {
 279             j=col_mate[k];
 280             col_mate[k]=l;
 281             row_mate[l]=k;
 282             if (j<0)
 283                 break;
 284             k=parent_row[j];
 285             l=j;
 286         }
 287         // End update the matching 20
 288         if (--unmatched==0)
 289             goto done;
 290         // Begin get ready for another stage 17
 291         t=0;
 292         for (l=0; l<n; l++)
 293         {
 294             parent_row[l]= -1;
 295             slack[l]=INF;
 296         }
 297         for (k=0; k<m; k++)
 298             if (col_mate[k]<0)
 299                 unchosen_row[t++]=k;
 300         // End get ready for another stage 17
 301     }
 302 done:
 303
 304 /*  // Begin doublecheck the solution 23
 305     for (k=0; k<m; k++)
 306         for (l=0; l<n; l++)
 307             if (p->cost[k][l]<row_dec[k]-col_inc[l])
 308                 exit(0);
 309     for (k=0; k<m; k++)
 310     {
 311         l=col_mate[k];
 312         if (l<0 || p->cost[k][l]!=row_dec[k]-col_inc[l])
 313             exit(0);
 314     }
 315     k=0;
 316     for (l=0; l<n; l++)
 317         if (col_inc[l])
 318             k++;
 319     if (k>m)
 320         exit(0);
 321     // End doublecheck the solution 23
 322 */  // End Hungarian algorithm 18
 323
 324     for (i=0; i<m; ++i)
 325     {
 326         p->assignment[i][col_mate[i]]=HUNGARIAN_ASSIGNED;
 327         /*TRACE("%d - %d\n", i, col_mate[i]);*/
 328     }
 329     for (k=0; k<m; ++k)
 330     {
 331         for (l=0; l<n; ++l)
 332         {
 333             /*TRACE("%d ",p->cost[k][l]-row_dec[k]+col_inc[l]);*/
 334             p->cost[k][l]=p->cost[k][l]-row_dec[k]+col_inc[l];
 335         }
 336         /*TRACE("\n");*/
 337     }
 338     for (i=0; i<m; i++)
 339         cost+=row_dec[i];
 340     for (i=0; i<n; i++)
 341         cost-=col_inc[i];
 342
 343     free(slack);
 344     free(col_inc);
 345     free(parent_row);
 346     free(row_mate);
 347     free(slack_row);
 348     free(row_dec);
 349     free(unchosen_row);
 350     free(col_mate);
 351 }
 352
 353
 354 /***************
 355  * Usage in R :
 356  **************/
 357
 358 // Auxiliary function for hungarianAlgorithm below
 359 double** array_to_matrix(double* m, int rows, int cols)
 360 {
 361   double** r = (double**)calloc(rows,sizeof(double*));
 362   for (int i=0; i<rows; i++)
 363   {
 364     r[i] = (double*)calloc(cols,sizeof(double));
 365     for (int j=0; j<cols; j++)
 366       r[i][j] = m[i*cols+j];
 367   }
 368   return r;
 369 }
 370
 371 //TODO: re-code this algorithm in a more readable way, based on
 372 //https://www.topcoder.com/community/data-science/data-science-tutorials/assignment-problem-and-hungarian-algorithm/
 373 // Get the optimal assignment, by calling hungarian_solve above; "distances" in columns
 374 void hungarianAlgorithm(double* distances, int* pn, int* assignment)
 375 {
 376     int n = *pn;
 377     double** mat = array_to_matrix(distances, n, n);
 378
 379   hungarian_problem_t p;
 380     hungarian_init(&p, mat, n, n, HUNGARIAN_MODE_MINIMIZE_COST);
 381
 382     hungarian_solve(&p);
 383
 384     // Copy the optimal assignment in a R-friendly structure
 385     for (int i=0; i < n; i++) {
 386         for (int j=0; j < n; j++) {
 387             if (p.assignment[i][j])
 388                 assignment[i] = j+1; //add 1 since we work with R
 389         }
 390     }
 391
 392   hungarian_free(&p);
 393     for (int i=0; i<n; i++)
 394         free(mat[i]);
 395     free(mat);
 396 }