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