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 \
  94 //          HUNGARIAN_MODE_MINIMIZE_COST !\n", __FUNCTION__);
  95
  96     return rows;
  97 }
  98
  99 /** Free the memory allocated by init. **/
 100 void hungarian_free(hungarian_problem_t* p) {
 101     int i;
 102     for(i=0; i<p->num_rows; i++) {
 103         free(p->cost[i]);
 104         free(p->assignment[i]);
 105     }
 106     free(p->cost);
 107     free(p->assignment);
 108     p->cost = NULL;
 109     p->assignment = NULL;
 110 }
 111
 112 /** This method computes the optimal assignment. **/
 113 void hungarian_solve(hungarian_problem_t* p)
 114 {
 115     int i, j, m, n, k, l, t, q, unmatched;
 116     double cost, s;
 117     int* col_mate;
 118     int* row_mate;
 119     int* parent_row;
 120     int* unchosen_row;
 121     double* row_dec;
 122     double* col_inc;
 123     double* slack;
 124     int* slack_row;
 125
 126     cost = 0.;
 127     m =p->num_rows;
 128     n =p->num_cols;
 129
 130     col_mate = (int*)calloc(p->num_rows,sizeof(int));
 131     unchosen_row = (int*)calloc(p->num_rows,sizeof(int));
 132     row_dec = (double*)calloc(p->num_rows,sizeof(double));
 133     slack_row   = (int*)calloc(p->num_rows,sizeof(int));
 134
 135     row_mate = (int*)calloc(p->num_cols,sizeof(int));
 136     parent_row = (int*)calloc(p->num_cols,sizeof(int));
 137     col_inc = (double*)calloc(p->num_cols,sizeof(double));
 138     slack = (double*)calloc(p->num_cols,sizeof(double));
 139
 140     for (i=0; i<p->num_rows; i++) {
 141         col_mate[i]=0;
 142         unchosen_row[i]=0;
 143         row_dec[i]=0.;
 144         slack_row[i]=0;
 145     }
 146     for (j=0; j<p->num_cols; j++) {
 147         row_mate[j]=0;
 148         parent_row[j] = 0;
 149         col_inc[j]=0.;
 150         slack[j]=0.;
 151     }
 152
 153     for (i=0; i<p->num_rows; ++i)
 154         for (j=0; j<p->num_cols; ++j)
 155             p->assignment[i][j]=HUNGARIAN_NOT_ASSIGNED;
 156
 157     // Begin subtract column minima in order to start with lots of zeroes 12
 158     for (l=0; l<n; l++)
 159     {
 160         s=p->cost[0][l];
 161         for (k=1; k<m; k++)
 162             if (p->cost[k][l]<s)
 163                 s=p->cost[k][l];
 164         cost+=s;
 165         if (s!=0.)
 166             for (k=0; k<m; k++)
 167                 p->cost[k][l]-=s;
 168     }
 169     // End subtract column minima in order to start with lots of zeroes 12
 170
 171     // Begin initial state 16
 172     t=0;
 173     for (l=0; l<n; l++)
 174     {
 175         row_mate[l]= -1;
 176         parent_row[l]= -1;
 177         col_inc[l]=0.;
 178         slack[l]=INF;
 179     }
 180     for (k=0; k<m; k++)
 181     {
 182         s=p->cost[k][0];
 183         for (l=1; l<n; l++)
 184             if (p->cost[k][l]<s)
 185                 s=p->cost[k][l];
 186         row_dec[k]=s;
 187         for (l=0; l<n; l++)
 188             if (s==p->cost[k][l] && row_mate[l]<0)
 189             {
 190                 col_mate[k]=l;
 191                 row_mate[l]=k;
 192                 goto row_done;
 193             }
 194         col_mate[k]= -1;
 195         unchosen_row[t++]=k;
 196 row_done:
 197         ;
 198     }
 199     // End initial state 16
 200
 201     // Begin Hungarian algorithm 18
 202     if (t==0)
 203         goto done;
 204     unmatched=t;
 205     while (1)
 206     {
 207         q=0;
 208         while (1)
 209         {
 210             while (q<t)
 211             {
 212                 // Begin explore node q of the forest 19
 213                 {
 214                     k=unchosen_row[q];
 215                     s=row_dec[k];
 216                     for (l=0; l<n; l++)
 217                         if (slack[l] > 0.)
 218                         {
 219                             double del=p->cost[k][l]-s+col_inc[l];
 220                             if (del<slack[l])
 221                             {
 222                                 if (del==0.)
 223                                 {
 224                                     if (row_mate[l]<0)
 225                                         goto breakthru;
 226                                     slack[l]=0.;
 227                                     parent_row[l]=k;
 228                                     unchosen_row[t++]=row_mate[l];
 229                                 }
 230                                 else
 231                                 {
 232                                     slack[l]=del;
 233                                     slack_row[l]=k;
 234                                 }
 235                             }
 236                         }
 237                 }
 238                 // End explore node q of the forest 19
 239                 q++;
 240             }
 241
 242             // Begin introduce a new zero into the matrix 21
 243             s=INF;
 244             for (l=0; l<n; l++)
 245                 if (slack[l]>0. && slack[l]<s)
 246                     s=slack[l];
 247             for (q=0; q<t; q++)
 248                 row_dec[unchosen_row[q]]+=s;
 249             for (l=0; l<n; l++)
 250                 if (slack[l]>0.)
 251                 {
 252                     slack[l]-=s;
 253                     if (slack[l]==0.)
 254                     {
 255                         // Begin look at a new zero 22
 256                         k=slack_row[l];
 257                         if (row_mate[l]<0)
 258                         {
 259                             for (j=l+1; j<n; j++)
 260                                 if (slack[j]==0.)
 261                                     col_inc[j]+=s;
 262                             goto breakthru;
 263                         }
 264                         else
 265                         {
 266                             parent_row[l]=k;
 267                             unchosen_row[t++]=row_mate[l];
 268                         }
 269                         // End look at a new zero 22
 270                     }
 271                 }
 272                 else
 273                     col_inc[l]+=s;
 274             // End introduce a new zero into the matrix 21
 275         }
 276 breakthru:
 277         // Begin update the matching 20
 278         while (1)
 279         {
 280             j=col_mate[k];
 281             col_mate[k]=l;
 282             row_mate[l]=k;
 283             if (j<0)
 284                 break;
 285             k=parent_row[j];
 286             l=j;
 287         }
 288         // End update the matching 20
 289         if (--unmatched==0)
 290             goto done;
 291         // Begin get ready for another stage 17
 292         t=0;
 293         for (l=0; l<n; l++)
 294         {
 295             parent_row[l]= -1;
 296             slack[l]=INF;
 297         }
 298         for (k=0; k<m; k++)
 299             if (col_mate[k]<0)
 300                 unchosen_row[t++]=k;
 301         // End get ready for another stage 17
 302     }
 303 done:
 304
 305 /*  // Begin doublecheck the solution 23
 306     for (k=0; k<m; k++)
 307         for (l=0; l<n; l++)
 308             if (p->cost[k][l]<row_dec[k]-col_inc[l])
 309                 exit(0);
 310     for (k=0; k<m; k++)
 311     {
 312         l=col_mate[k];
 313         if (l<0 || p->cost[k][l]!=row_dec[k]-col_inc[l])
 314             exit(0);
 315     }
 316     k=0;
 317     for (l=0; l<n; l++)
 318         if (col_inc[l])
 319             k++;
 320     if (k>m)
 321         exit(0);
 322     // End doublecheck the solution 23
 323 */  // End Hungarian algorithm 18
 324
 325     for (i=0; i<m; ++i)
 326     {
 327         p->assignment[i][col_mate[i]]=HUNGARIAN_ASSIGNED;
 328         /*TRACE("%d - %d\n", i, col_mate[i]);*/
 329     }
 330     for (k=0; k<m; ++k)
 331     {
 332         for (l=0; l<n; ++l)
 333         {
 334             /*TRACE("%d ",p->cost[k][l]-row_dec[k]+col_inc[l]);*/
 335             p->cost[k][l]=p->cost[k][l]-row_dec[k]+col_inc[l];
 336         }
 337         /*TRACE("\n");*/
 338     }
 339     for (i=0; i<m; i++)
 340         cost+=row_dec[i];
 341     for (i=0; i<n; i++)
 342         cost-=col_inc[i];
 343
 344     free(slack);
 345     free(col_inc);
 346     free(parent_row);
 347     free(row_mate);
 348     free(slack_row);
 349     free(row_dec);
 350     free(unchosen_row);
 351     free(col_mate);
 352 }
 353
 354
 355 /***************
 356  * Usage in R :
 357  **************/
 358
 359 // Auxiliary function for hungarianAlgorithm below
 360 double** array_to_matrix(double* m, int rows, int cols)
 361 {
 362   double** r = (double**)calloc(rows,sizeof(double*));
 363   for (int i=0; i<rows; i++)
 364   {
 365     r[i] = (double*)calloc(cols,sizeof(double));
 366     for (int j=0; j<cols; j++)
 367       r[i][j] = m[i*cols+j];
 368   }
 369   return r;
 370 }
 371
 372 //TODO: re-code this algorithm in a more readable way, based on
 373 //https://www.topcoder.com/community/data-science/data-science-tutorials/\
 374 //  assignment-problem-and-hungarian-algorithm/
 375 // Get the optimal assignment, by calling hungarian_solve above; "distances" in columns
 376 void hungarianAlgorithm(double* distances, int* pn, int* assignment)
 377 {
 378     int n = *pn;
 379     double** mat = array_to_matrix(distances, n, n);
 380
 381   hungarian_problem_t p;
 382     hungarian_init(&p, mat, n, n, HUNGARIAN_MODE_MINIMIZE_COST);
 383
 384     hungarian_solve(&p);
 385
 386     // Copy the optimal assignment in a R-friendly structure
 387     for (int i=0; i < n; i++) {
 388         for (int j=0; j < n; j++) {
 389             if (p.assignment[i][j])
 390                 assignment[i] = j+1; //add 1 since we work with R
 391         }
 392     }
 393
 394   hungarian_free(&p);
 395     for (int i=0; i<n; i++)
 396         free(mat[i]);
 397     free(mat);
 398 }