Fawkes API  Fawkes Development Version
hungarian.cpp
1 
2 /***************************************************************************
3  * hungarian.cpp - Hungarian Method
4  *
5  * Created: Thu Apr 18 17:09:58 2013
6  * Copyright 2004 Cyrill Stachniss
7  * 2008 Masrur Doostdar
8  * 2008 Stefan Schiffer
9  * 2013 Tim Niemueller [www.niemueller.de]
10  ****************************************************************************/
11 
12 /* This program is free software; you can redistribute it and/or modify
13  * it under the terms of the GNU General Public License as published by
14  * the Free Software Foundation; either version 2 of the License, or
15  * (at your option) any later version. A runtime exception applies to
16  * this software (see LICENSE.GPL_WRE file mentioned below for details).
17  *
18  * This program is distributed in the hope that it will be useful,
19  * but WITHOUT ANY WARRANTY; without even the implied warranty of
20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
21  * GNU Library General Public License for more details.
22  *
23  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
24  */
25 
26 #include <cstdio>
27 #include <cstdlib>
28 #include <iostream>
29 #include <utils/hungarian_method/hungarian.h>
30 
31 #define INF (0x7FFFFFFF)
32 #define verbose (0)
33 
34 namespace fawkes {
35 #if 0 /* just to make Emacs auto-indent happy */
36 }
37 #endif
38 
39 #define hungarian_test_alloc(X) do {if ((void *)(X) == NULL) \
40  fprintf(stderr, "Out of memory in %s, (%s, line %d).\n", \
41  __FUNCTION__, __FILE__, __LINE__); } while (0)
42 
43 // 'faked' class member
44 //hungarian_problem_t hp;
45 
46 /** @class HungarianMethod <utils/hungarian_method/hungarian.h>
47  * Hungarian method assignment solver.
48  * @author Stefan Schiffer
49  */
50 
51 /** Constructor. */
53 {
54  p = (hungarian_problem_t *)malloc(sizeof(hungarian_problem_t));
55  num_cols_ = 0;
56  num_rows_ = 0;
57  available_ = false;
58 }
59 
60 /** Destructor. */
62 {
63  ::free(p);
64 }
65 
66 /** Print matrix to stdout.
67  * @param C values
68  * @param rows number of rows
69  * @param cols number of columns
70  */
71 void
72 HungarianMethod::print_matrix( int** C, int rows, int cols )
73 {
74  int i,j;
75  std::cerr << std::endl;
76  for(i=0; i<rows; i++) {
77  std::cerr << " " << i << "'th row [" << std::flush;
78  for(j=0; j<cols; j++) {
79  std::cerr << C[i][j] << " ";
80 // char s[5] = "\0";
81 // sprintf( s, "%5d", C[i][j]);
82 // std::cerr << s << " " << std::flush;
83  }
84  std::cerr << " ]" << std::endl << std::flush;
85  }
86  std::cerr << std::endl;
87 }
88 
89 /** Convert an array to a matrix.
90  * @param m array to convert
91  * @param rows number of rows in array
92  * @param cols number of columns in array
93  * @return matrix from array
94  */
95 int**
96 HungarianMethod::array_to_matrix(int* m, int rows, int cols)
97 {
98  int i,j;
99  int** r;
100  r = (int**)calloc(rows,sizeof(int*));
101  for(i=0;i<rows;i++)
102  {
103  r[i] = (int*)calloc(cols,sizeof(int));
104  for(j=0;j<cols;j++)
105  r[i][j] = m[i*cols+j];
106  }
107  return r;
108 }
109 
110 
111 /** Print the assignment matrix. */
112 void
114 {
115  if( available_ ) {
116  std::cerr << "HungarianMethod: == Assignment ==" << std::endl;
117  print_matrix(p->assignment, p->num_rows, p->num_cols) ;
118  }
119 }
120 
121 /** Print the cost matrix. */
122 void
124 {
125  if( available_ ) {
126  std::cerr << "HungarianMethod: == CostMatrix ==" << std::endl;
127  print_matrix(p->cost, p->num_rows, p->num_cols) ;
128  }
129 }
130 
131 /** Print the current status.
132  * Prints cost matrix followed by assignment. */
133 void
135 {
138 }
139 
140 /** Initialize hungarian method.
141  * @param cost_matrix initial cost matrix
142  * @param rows number of rows in matrix
143  * @param cols number of columns in matrix
144  * @param mode One of HUNGARIAN_MODE_MINIMIZE_COST and HUNGARIAN_MODE_MAXIMIZE_UTIL
145  * @return number of rows in quadratic matrix
146  */
147 int
148 HungarianMethod::init( int** cost_matrix, int rows, int cols, int mode )
149 {
150 // std::cout << "HungarianMethod(init): entering ..." << std::endl;
151 
152  int i,j, org_cols, org_rows;
153  int max_cost;
154  max_cost = 0;
155 
156  org_cols = cols;
157  org_rows = rows;
158 
159  // is the number of cols not equal to number of rows ?
160  // if yes, expand with 0-cols / 0-cols
161  rows = std::max(cols, rows);
162  cols = rows;
163 
164  p->num_rows = rows;
165  p->num_cols = cols;
166 
167  p->cost = (int**)calloc(rows,sizeof(int*));
168  hungarian_test_alloc(p->cost);
169  p->assignment = (int**)calloc(rows,sizeof(int*));
170  hungarian_test_alloc(p->assignment);
171 
172  //std::cout << "HungarianMethod(init): loop rows" << std::endl;
173  for(i=0; i<p->num_rows; i++) {
174  p->cost[i] = (int*)calloc(cols,sizeof(int));
175  hungarian_test_alloc(p->cost[i]);
176  p->assignment[i] = (int*)calloc(cols,sizeof(int));
177  hungarian_test_alloc(p->assignment[i]);
178  for(j=0; j<p->num_cols; j++) {
179  p->cost[i][j] = (i < org_rows && j < org_cols) ? cost_matrix[i][j] : 0;
180  p->assignment[i][j] = 0;
181 
182  if (max_cost < p->cost[i][j])
183  max_cost = p->cost[i][j];
184  }
185  }
186 
187 
188  if (mode == HUNGARIAN_MODE_MAXIMIZE_UTIL) {
189  for(i=0; i<p->num_rows; i++) {
190  for(j=0; j<p->num_cols; j++) {
191  p->cost[i][j] = max_cost - p->cost[i][j];
192  }
193  }
194  }
195  else if (mode == HUNGARIAN_MODE_MINIMIZE_COST) {
196  // nothing to do
197  }
198  else
199  fprintf(stderr,"%s: unknown mode. Mode was set to HUNGARIAN_MODE_MINIMIZE_COST !\n", __FUNCTION__);
200 
201  // /////////////////////////////////////
202  //std::cout << "HungarianMethod(init): init assignment save" << std::endl;
203  //
204  num_cols_ = cols;
205  col_mates_ = (int*)calloc(cols,sizeof(int));
206  hungarian_test_alloc(col_mates_);
207  for( int j = 0; j < num_cols_; ++j ) {
208  col_mates_[j] = -1;
209  }
210  //
211  num_rows_ = rows;
212  row_mates_ = (int*)calloc(rows,sizeof(int));
213  hungarian_test_alloc(row_mates_);
214  for( int i = 0; i < num_rows_; ++i ) {
215  row_mates_[i] = -1;
216  }
217  // /////////////////////////////////////
218 
219 // std::cout << "HungarianMethod(init): ... leaving." << std::endl;
220  return rows;
221 }
222 
223 /** Free space alloacted by method. */
224 void
226 {
227 // std::cout << "HungarianMethod(free): entering ..." << std::endl;
228  int i;
229  for(i=0; i<p->num_rows; i++) {
230  ::free(p->cost[i]);
231  ::free(p->assignment[i]);
232  }
233  ::free(p->cost);
234  ::free(p->assignment);
235  p->cost = NULL;
236  p->assignment = NULL;
237  //
238  num_cols_ = 0;
239  num_rows_ = 0;
240  ::free( col_mates_ );
241  ::free( row_mates_ );
242  available_ = false;
243 // std::cout << "HungarianMethod(free): ... leaving." << std::endl;
244 }
245 
246 
247 /** Solve the assignment problem.
248  * This method computes the optimal assignment.
249  */
250 void
252 {
253  int i, j, m, n, k, l, s, t, q, unmatched, cost;
254  int* col_mate;
255  int* row_mate;
256  int* parent_row;
257  int* unchosen_row;
258  int* row_dec;
259  int* col_inc;
260  int* slack;
261  int* slack_row;
262 
263  cost=0;
264  m =p->num_rows;
265  n =p->num_cols;
266 
267  col_mate = (int*)calloc(p->num_rows,sizeof(int));
268  hungarian_test_alloc(col_mate);
269  unchosen_row = (int*)calloc(p->num_rows,sizeof(int));
270  hungarian_test_alloc(unchosen_row);
271  row_dec = (int*)calloc(p->num_rows,sizeof(int));
272  hungarian_test_alloc(row_dec);
273  slack_row = (int*)calloc(p->num_rows,sizeof(int));
274  hungarian_test_alloc(slack_row);
275 
276  row_mate = (int*)calloc(p->num_cols,sizeof(int));
277  hungarian_test_alloc(row_mate);
278  parent_row = (int*)calloc(p->num_cols,sizeof(int));
279  hungarian_test_alloc(parent_row);
280  col_inc = (int*)calloc(p->num_cols,sizeof(int));
281  hungarian_test_alloc(col_inc);
282  slack = (int*)calloc(p->num_cols,sizeof(int));
283  hungarian_test_alloc(slack);
284 
285  for (i=0;i<p->num_rows;i++) {
286  col_mate[i]=0;
287  unchosen_row[i]=0;
288  row_dec[i]=0;
289  slack_row[i]=0;
290  }
291  for (j=0;j<p->num_cols;j++) {
292  row_mate[j]=0;
293  parent_row[j] = 0;
294  col_inc[j]=0;
295  slack[j]=0;
296  }
297 
298  for (i=0;i<p->num_rows;++i)
299  for (j=0;j<p->num_cols;++j)
300  p->assignment[i][j]=HUNGARIAN_NOT_ASSIGNED;
301 
302  // Begin subtract column minima in order to start with lots of zeroes 12
303  if (verbose)
304  fprintf(stderr, "Using heuristic\n");
305  for (l=0;l<n;l++)
306  {
307  s=p->cost[0][l];
308  for (k=1;k<m;k++)
309  if (p->cost[k][l]<s)
310  s=p->cost[k][l];
311  cost+=s;
312  if (s!=0)
313  for (k=0;k<m;k++)
314  p->cost[k][l]-=s;
315  }
316  // End subtract column minima in order to start with lots of zeroes 12
317 
318  // Begin initial state 16
319  t=0;
320  for (l=0;l<n;l++)
321  {
322  row_mate[l]= -1;
323  parent_row[l]= -1;
324  col_inc[l]=0;
325  slack[l]=INF;
326  }
327  for (k=0;k<m;k++)
328  {
329  s=p->cost[k][0];
330  for (l=1;l<n;l++)
331  if (p->cost[k][l]<s)
332  s=p->cost[k][l];
333  row_dec[k]=s;
334  for (l=0;l<n;l++)
335  if (s==p->cost[k][l] && row_mate[l]<0)
336  {
337  col_mate[k]=l;
338  row_mate[l]=k;
339  if (verbose)
340  fprintf(stderr, "matching col %d==row %d\n",l,k);
341  goto row_done;
342  }
343  col_mate[k]= -1;
344  if (verbose)
345  fprintf(stderr, "node %d: unmatched row %d\n",t,k);
346  unchosen_row[t++]=k;
347  row_done:
348  ;
349  }
350  // End initial state 16
351 
352  // Begin Hungarian algorithm 18
353  if (t==0)
354  goto done;
355  unmatched=t;
356  while (1)
357  {
358  if (verbose)
359  fprintf(stderr, "Matched %d rows.\n",m-t);
360  q=0;
361  while (1)
362  {
363  while (q<t)
364  {
365  // Begin explore node q of the forest 19
366  {
367  k=unchosen_row[q];
368  s=row_dec[k];
369  for (l=0;l<n;l++)
370  if (slack[l])
371  {
372  int del;
373  del=p->cost[k][l]-s+col_inc[l];
374  if (del<slack[l])
375  {
376  if (del==0)
377  {
378  if (row_mate[l]<0)
379  goto breakthru;
380  slack[l]=0;
381  parent_row[l]=k;
382  if (verbose)
383  fprintf(stderr, "node %d: row %d==col %d--row %d\n",
384  t,row_mate[l],l,k);
385  unchosen_row[t++]=row_mate[l];
386  }
387  else
388  {
389  slack[l]=del;
390  slack_row[l]=k;
391  }
392  }
393  }
394  }
395  // End explore node q of the forest 19
396  q++;
397  }
398 
399  // Begin introduce a new zero into the matrix 21
400  s=INF;
401  for (l=0;l<n;l++)
402  if (slack[l] && slack[l]<s)
403  s=slack[l];
404  for (q=0;q<t;q++)
405  row_dec[unchosen_row[q]]+=s;
406  for (l=0;l<n;l++)
407  if (slack[l])
408  {
409  slack[l]-=s;
410  if (slack[l]==0)
411  {
412  // Begin look at a new zero 22
413  k=slack_row[l];
414  if (verbose)
415  fprintf(stderr,
416  "Decreasing uncovered elements by %d produces zero at [%d,%d]\n",
417  s,k,l);
418  if (row_mate[l]<0)
419  {
420  for (j=l+1;j<n;j++)
421  if (slack[j]==0)
422  col_inc[j]+=s;
423  goto breakthru;
424  }
425  else
426  {
427  parent_row[l]=k;
428  if (verbose)
429  fprintf(stderr, "node %d: row %d==col %d--row %d\n",t,row_mate[l],l,k);
430  unchosen_row[t++]=row_mate[l];
431  }
432  // End look at a new zero 22
433  }
434  }
435  else
436  col_inc[l]+=s;
437  // End introduce a new zero into the matrix 21
438  }
439  breakthru:
440  // Begin update the matching 20
441  if (verbose)
442  fprintf(stderr, "Breakthrough at node %d of %d!\n",q,t);
443  while (1)
444  {
445  j=col_mate[k];
446  col_mate[k]=l;
447  row_mate[l]=k;
448  if (verbose)
449  fprintf(stderr, "rematching col %d==row %d\n",l,k);
450  if (j<0)
451  break;
452  k=parent_row[j];
453  l=j;
454  }
455  // End update the matching 20
456  if (--unmatched==0)
457  goto done;
458  // Begin get ready for another stage 17
459  t=0;
460  for (l=0;l<n;l++)
461  {
462  parent_row[l]= -1;
463  slack[l]=INF;
464  }
465  for (k=0;k<m;k++)
466  if (col_mate[k]<0)
467  {
468  if (verbose)
469  fprintf(stderr, "node %d: unmatched row %d\n",t,k);
470  unchosen_row[t++]=k;
471  }
472  // End get ready for another stage 17
473  }
474  done:
475 
476  // Begin doublecheck the solution 23
477  for (k=0;k<m;k++)
478  for (l=0;l<n;l++)
479  if (p->cost[k][l]<row_dec[k]-col_inc[l]) {
480  printf("boom1: p->cost[%i][%i]=%i < row_dec[%i]-col_inc[%i]=%i\n", k, l, p->cost[k][l], k, l, row_dec[k]-col_inc[l]);
481 // exit(0);
482  }
483  for (k=0;k<m;k++)
484  {
485  l=col_mate[k];
486  if (l<0 || p->cost[k][l]!=row_dec[k]-col_inc[l]) {
487  printf("boom2: %i<0 || p->cost[%i][%i]=%i != row_dec[%i]-col_inc[%i]=%i\n", l, k, l, p->cost[k][l], k, l, row_dec[k]-col_inc[l]);
488 // exit(0);
489  }
490  }
491  k=0;
492  for (l=0;l<n;l++)
493  if (col_inc[l])
494  k++;
495  if (k>m) {
496  printf("boom3: %i > %i\n", k, m);
497  // exit(0);
498  }
499  // End doublecheck the solution 23
500  // End Hungarian algorithm 18
501 
502  for (i=0;i<m;++i)
503  {
504  p->assignment[i][col_mate[i]]=HUNGARIAN_ASSIGNED;
505  /*TRACE("%d - %d\n", i, col_mate[i]);*/
506  }
507  for (k=0;k<m;++k)
508  {
509  for (l=0;l<n;++l)
510  {
511  /*TRACE("%d ",p->cost[k][l]-row_dec[k]+col_inc[l]);*/
512  p->cost[k][l]=p->cost[k][l]-row_dec[k]+col_inc[l];
513  }
514  /*TRACE("\n");*/
515  }
516  for (i=0;i<m;i++)
517  cost+=row_dec[i];
518  for (i=0;i<n;i++)
519  cost-=col_inc[i];
520  if (verbose)
521  fprintf(stderr, "Cost is %d\n",cost);
522 
523  // /////////////////////////////////////
524  // Save Assignment
525  //
526  for( int i = 0; i < num_rows_; ++i ) {
527  row_mates_[i] = row_mate[i];
528  }
529  for( int j = 0; j < num_cols_; ++j ) {
530  col_mates_[j] = col_mate[j];
531  }
532  // /////////////////////////////////////
533 
534  ::free(slack);
535  ::free(col_inc);
536  ::free(parent_row);
537  ::free(row_mate);
538  ::free(slack_row);
539  ::free(row_dec);
540  ::free(unchosen_row);
541  ::free(col_mate);
542 
543  available_ = true;
544 }
545 
546 
547 /** Get column assignment.
548  * @param col column index
549  * @return column assignment, or -1 if @p col is out of bounds.
550  */
551 int
553 {
554  //std::cout << "HungarianMethod(get_column_assignment): for col '" << col << "'" << std::endl;
555  if( col < num_cols_ ) {
556  return (int)col_mates_[col];
557  }
558  return -1;
559 }
560 
561 
562 /** Get row assignment.
563  * @param row row index
564  * @return row assignment, or -1 if @p row is out of bounds.
565  */
566 int
568 {
569  //std::cout << "HungarianMethod(get_row_assignment): for row '" << row << "'" << std::endl;
570  if( row < num_rows_ ) {
571  return (int)row_mates_[row];
572  }
573  return -1;
574 }
575 
576 /** Check if data is available.
577  * solve done and not freed yet.
578  * @return true if data is available, false otherwise
579  */
580 bool
582 {
583  return available_;
584 }
585 
586 
587 /** Get assignment and size.
588  * @param size number of rows/columns in quadratic matrix
589  * @return pointer to columns.
590  */
591 int *
593 {
594  size=p->num_rows;
595  return col_mates_;
596 }
597 
598 
599 } // end of namespace fawkes
int get_column_assignment(const int &col)
Get column assignment.
Definition: hungarian.cpp:552
int get_row_assignment(const int &row)
Get row assignment.
Definition: hungarian.cpp:567
void print_status()
Print the current status.
Definition: hungarian.cpp:134
void print_matrix(int **C, int rows, int cols)
Print matrix to stdout.
Definition: hungarian.cpp:72
~HungarianMethod()
Destructor.
Definition: hungarian.cpp:61
void solve()
Solve the assignment problem.
Definition: hungarian.cpp:251
Fawkes library namespace.
bool is_available()
Check if data is available.
Definition: hungarian.cpp:581
hungarian_problem_t * p
our problem instance member.
Definition: hungarian.h:75
void print_cost_matrix()
Print the cost matrix.
Definition: hungarian.cpp:123
void print_assignment()
Print the assignment matrix.
Definition: hungarian.cpp:113
int init(int **cost_matrix, int rows, int cols, int mode)
Initialize hungarian method.
Definition: hungarian.cpp:148
int * get_assignment(int &size)
Get assignment and size.
Definition: hungarian.cpp:592
void free()
Free space alloacted by method.
Definition: hungarian.cpp:225
HungarianMethod()
Constructor.
Definition: hungarian.cpp:52
int ** array_to_matrix(int *m, int rows, int cols)
Convert an array to a matrix.
Definition: hungarian.cpp:96