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 s; //,cost
 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 }