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
35namespace 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 */
72void
73HungarianMethod::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 */
96int **
97HungarianMethod::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. */
111void
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. */
121void
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. */
132void
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 */
146int
147HungarianMethod::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. */
222void
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 */
247void
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 }
447done:
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 */
532int
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 */
546int
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 */
560bool
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 */
570int *
572{
573 size = p->num_rows;
574 return col_mates_;
575}
576
577} // end of namespace fawkes
void print_cost_matrix()
Print the cost matrix.
Definition: hungarian.cpp:122
~HungarianMethod()
Destructor.
Definition: hungarian.cpp:61
void free()
Free space alloacted by method.
Definition: hungarian.cpp:223
int get_row_assignment(const int &row)
Get row assignment.
Definition: hungarian.cpp:547
void print_assignment()
Print the assignment matrix.
Definition: hungarian.cpp:112
void print_matrix(int **C, int rows, int cols)
Print matrix to stdout.
Definition: hungarian.cpp:73
void print_status()
Print the current status.
Definition: hungarian.cpp:133
hungarian_problem_t * p
our problem instance member.
Definition: hungarian.h:72
int get_column_assignment(const int &col)
Get column assignment.
Definition: hungarian.cpp:533
HungarianMethod()
Constructor.
Definition: hungarian.cpp:52
int * get_assignment(int &size)
Get assignment and size.
Definition: hungarian.cpp:571
int init(int **cost_matrix, int rows, int cols, int mode)
Initialize hungarian method.
Definition: hungarian.cpp:147
int ** array_to_matrix(int *m, int rows, int cols)
Convert an array to a matrix.
Definition: hungarian.cpp:97
bool is_available()
Check if data is available.
Definition: hungarian.cpp:561
void solve()
Solve the assignment problem.
Definition: hungarian.cpp:248
Fawkes library namespace.