Fix script for cluster + a few other fixes
[morpheus.git] / 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 }