/******************************************************************** ******************************************************************** ** ** libhungarian by Cyrill Stachniss, 2004 ** ** ** Solving the Minimum Assignment Problem using the ** Hungarian Method. ** ** ** This file may be freely copied and distributed! ** ** ** Parts of the used code was originally provided by the ** "Stanford GraphGase", but I made changes to this code. ** As asked by the copyright node of the "Stanford GraphGase", ** I hereby proclaim that this file are *NOT* part of the ** "Stanford GraphGase" distrubition! ** ** This file is distributed in the hope that it will be useful, ** but WITHOUT ANY WARRANTY; without even the implied ** warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR ** PURPOSE. ** ******************************************************************** ********************************************************************/ #include #include #include "hungarian.h" #define INF (1.0/0.0) #define verbose (0) #define hungarian_test_alloc(X) do {if ((void *)(X) == NULL) fprintf(stderr, "Out of memory in %s, (%s, line %d).\n", __FUNCTION__, __FILE__, __LINE__); } while (0) double hungarian(double* data, int rows, int cols, size_t *from, size_t *to) { size_t i, j, k, n; hungarian_problem_t p; int matrix_size = hungarian_init(&p, data , rows, cols, HUNGARIAN_MODE_MINIMIZE_COST) ; double cost = hungarian_solve(&p); n = (rows < cols) ? rows : cols; k = 0; if (from != NULL && to != NULL) { for(i=0;inum_rows = rows; p->num_cols = cols; p->cost = (double**)calloc(rows,sizeof(double*)); hungarian_test_alloc(p->cost); p->assignment = (int**)calloc(rows,sizeof(int*)); hungarian_test_alloc(p->assignment); for(i=0; inum_rows; i++) { p->cost[i] = (double*)calloc(cols,sizeof(double)); hungarian_test_alloc(p->cost[i]); p->assignment[i] = (int*)calloc(cols,sizeof(int)); hungarian_test_alloc(p->assignment[i]); for(j=0; jnum_cols; j++) { p->cost[i][j] = (i < org_rows && j < org_cols) ? cost_matrix[i*org_cols+j] : 0; p->assignment[i][j] = 0; if (max_cost < p->cost[i][j]) max_cost = p->cost[i][j]; } } if (mode == HUNGARIAN_MODE_MAXIMIZE_UTIL) { for(i=0; inum_rows; i++) { for(j=0; jnum_cols; j++) { p->cost[i][j] = max_cost - p->cost[i][j]; } } } else if (mode == HUNGARIAN_MODE_MINIMIZE_COST) { // nothing to do } else fprintf(stderr,"%s: unknown mode. Mode was set to HUNGARIAN_MODE_MINIMIZE_COST !\n", __FUNCTION__); return rows; } void hungarian_free(hungarian_problem_t* p) { int i; for(i=0; inum_rows; i++) { free(p->cost[i]); free(p->assignment[i]); } free(p->cost); free(p->assignment); p->cost = NULL; p->assignment = NULL; } double hungarian_solve(hungarian_problem_t* p) { int i, j, m, n, k, l, t, q, unmatched; double s, cost; int* col_mate; int* row_mate; int* parent_row; int* unchosen_row; double* row_dec; double* col_inc; double* slack; int* slack_row; cost=0; m =p->num_rows; n =p->num_cols; col_mate = (int*)calloc(p->num_rows,sizeof(int)); hungarian_test_alloc(col_mate); unchosen_row = (int*)calloc(p->num_rows,sizeof(int)); hungarian_test_alloc(unchosen_row); row_dec = (double*)calloc(p->num_rows,sizeof(double)); hungarian_test_alloc(row_dec); slack_row = (int*)calloc(p->num_rows,sizeof(int)); hungarian_test_alloc(slack_row); row_mate = (int*)calloc(p->num_cols,sizeof(int)); hungarian_test_alloc(row_mate); parent_row = (int*)calloc(p->num_cols,sizeof(int)); hungarian_test_alloc(parent_row); col_inc = (double*)calloc(p->num_cols,sizeof(double)); hungarian_test_alloc(col_inc); slack = (double*)calloc(p->num_cols,sizeof(double)); hungarian_test_alloc(slack); for (i=0;inum_rows;i++) { col_mate[i]=0; unchosen_row[i]=0; row_dec[i]=0; slack_row[i]=0; } for (j=0;jnum_cols;j++) { row_mate[j]=0; parent_row[j] = 0; col_inc[j]=0; slack[j]=0; } for (i=0;inum_rows;++i) for (j=0;jnum_cols;++j) p->assignment[i][j]=HUNGARIAN_NOT_ASSIGNED; // Begin subtract column minima in order to start with lots of zeroes 12 if (verbose) fprintf(stderr, "Using heuristic\n"); for (l=0;lcost[0][l]; for (k=1;kcost[k][l]cost[k][l]; cost+=s; if (s!=0) for (k=0;kcost[k][l]-=s; } // End subtract column minima in order to start with lots of zeroes 12 // Begin initial state 16 t=0; for (l=0;lcost[k][0]; for (l=1;lcost[k][l]cost[k][l]; row_dec[k]=s; for (l=0;lcost[k][l] && row_mate[l]<0) { col_mate[k]=l; row_mate[l]=k; if (verbose) fprintf(stderr, "matching col %d==row %d\n",l,k); goto row_done; } col_mate[k]= -1; if (verbose) fprintf(stderr, "node %d: unmatched row %d\n",t,k); unchosen_row[t++]=k; row_done: ; } // End initial state 16 // Begin Hungarian algorithm 18 if (t==0) goto done; unmatched=t; while (1) { if (verbose) fprintf(stderr, "Matched %d rows.\n",m-t); q=0; while (1) { while (qcost[k][l]-s+col_inc[l]; if (delcost[k][l]cost[k][l]!=row_dec[k]-col_inc[l]) exit(0); } k=0; for (l=0;lm) exit(0); // End doublecheck the solution 23 // End Hungarian algorithm 18 // */ for (i=0;iassignment[i][col_mate[i]]=HUNGARIAN_ASSIGNED; //TRACE("%d - %d\n", i, col_mate[i]); } for (k=0;kcost[k][l]-row_dec[k]+col_inc[l]); p->cost[k][l]=p->cost[k][l]-row_dec[k]+col_inc[l]; } //TRACE("\n"); } for (i=0;i