[ppam-mpi.git] / code / postprocess / hungarian.c

/********************************************************************
 ********************************************************************
 **
 ** libhungarian by Cyrill Stachniss, 2004
 ** -- modified (very) slightly by Benjamin Auder, 2010
 ** -- (verbose and printing routines deleted, because used in a R package)
 **
 **
 ** 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" distribution!
 **
 ** 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 <stdio.h>
#include <stdlib.h>

#define HUNGARIAN_NOT_ASSIGNED 0 
#define HUNGARIAN_ASSIGNED 1

#define HUNGARIAN_MODE_MINIMIZE_COST   0
#define HUNGARIAN_MODE_MAXIMIZE_UTIL 1

typedef struct {
	int num_rows;
	int num_cols;
	int** cost;
	int** assignment;
} hungarian_problem_t;

#define INF (0x7FFFFFFF)
#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)

/** This method initialize the hungarian_problem structure and init 
 *  the  cost matrices (missing lines or columns are filled with 0).
 *  It returns the size of the quadratic(!) assignment matrix. **/
void hungarian_init(hungarian_problem_t* p, int* cost_matrix, int rows, int cols, int mode) {

    int i,j, org_cols, org_rows;
    int max_cost;
    max_cost = 0;
    
    org_cols = cols;
    org_rows = rows;
    
    // is the number of cols  not equal to number of rows ? 
    // if yes, expand with 0-cols / 0-cols
    rows = (cols < rows) ? rows : cols;
    cols = rows;
    
    p->num_rows = rows;
    p->num_cols = cols;
    
    p->cost = (int**)calloc(rows,sizeof(int*));
    hungarian_test_alloc(p->cost);
    p->assignment = (int**)calloc(rows,sizeof(int*));
    hungarian_test_alloc(p->assignment);

    for(i=0; i<p->num_rows; i++) {
        p->cost[i] = (int*)calloc(cols,sizeof(int));
        hungarian_test_alloc(p->cost[i]);
        p->assignment[i] = (int*)calloc(cols,sizeof(int));
        hungarian_test_alloc(p->assignment[i]);
        for(j=0; j<p->num_cols; j++) {
        p->cost[i][j] =  (i < org_rows && j < org_cols) ? cost_matrix[i*(p->num_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; i<p->num_rows; i++) {
            for(j=0; j<p->num_cols; j++) {
                p->cost[i][j] =  max_cost - p->cost[i][j];
            }
        }
    }
}

/** Free the memory allocated by init. **/
void hungarian_free(hungarian_problem_t* p) {
    int i;
    for(i=0; i<p->num_rows; i++) {
        free(p->cost[i]);
        free(p->assignment[i]);
    }
    free(p->cost);
    free(p->assignment);
    p->cost = NULL;
    p->assignment = NULL;
}

/** This method computes the optimal assignment. **/
void hungarian_solve(hungarian_problem_t* p) {

    int i, j, m, n, k, l, s, t, q, unmatched, cost;
    int* col_mate;
    int* row_mate;
    int* parent_row;
    int* unchosen_row;
    int* row_dec;
    int* col_inc;
    int* 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  = (int*)calloc(p->num_rows,sizeof(int));
    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 = (int*)calloc(p->num_cols,sizeof(int));
    hungarian_test_alloc(col_inc);
    slack = (int*)calloc(p->num_cols,sizeof(int));
    hungarian_test_alloc(slack);

    for (i=0;i<p->num_rows;i++) {
        col_mate[i]=0;
        unchosen_row[i]=0;
        row_dec[i]=0;
        slack_row[i]=0;
    }
    for (j=0;j<p->num_cols;j++) {
        row_mate[j]=0;
        parent_row[j] = 0;
        col_inc[j]=0;
        slack[j]=0;
    }

    for (i=0;i<p->num_rows;++i)
        for (j=0;j<p->num_cols;++j)
            p->assignment[i][j]=HUNGARIAN_NOT_ASSIGNED;

    // Begin subtract column minima in order to start with lots of zeroes 12
    for (l=0;l<n;l++) {
        s=p->cost[0][l];
        for (k=1;k<m;k++) 
        if (p->cost[k][l]<s)
        s=p->cost[k][l];
        cost+=s;
        if (s!=0)
        for (k=0;k<m;k++)
        p->cost[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;l<n;l++) {
        row_mate[l]= -1;
        parent_row[l]= -1;
        col_inc[l]=0;
        slack[l]=INF;
    }
    for (k=0;k<m;k++) {
        s=p->cost[k][0];
        for (l=1;l<n;l++)
        if (p->cost[k][l]<s)
        s=p->cost[k][l];
        row_dec[k]=s;
        for (l=0;l<n;l++)
        if (s==p->cost[k][l] && row_mate[l]<0) {
            col_mate[k]=l;
            row_mate[l]=k;
            goto row_done;
        }
        col_mate[k]= -1;
        unchosen_row[t++]=k;
        row_done:
        ;
    }
    // End initial state 16

    // Begin Hungarian algorithm 18
    if (t==0)
        goto done;
    unmatched=t;
    while (1) {
        q=0;
        while (1) {
            while (q<t) {
                // Begin explore node q of the forest 19
                {
                k=unchosen_row[q];
                s=row_dec[k];
                for (l=0;l<n;l++)
                    if (slack[l]) {
                        int del;
                        del=p->cost[k][l]-s+col_inc[l];
                        if (del<slack[l]) {
                            if (del==0) {
                                if (row_mate[l]<0)
                                    goto breakthru;
                                slack[l]=0;
                                parent_row[l]=k;
                                unchosen_row[t++]=row_mate[l];
                            }
                            else {
                                slack[l]=del;
                                slack_row[l]=k;
                            }
                        }
                    }
                }
                // End explore node q of the forest 19
                q++;
            }

            // Begin introduce a new zero into the matrix 21
            s=INF;
            for (l=0;l<n;l++)
                if (slack[l] && slack[l]<s)
                    s=slack[l];
            for (q=0;q<t;q++)
                row_dec[unchosen_row[q]]+=s;
            for (l=0;l<n;l++) {
                if (slack[l]) {
                    slack[l]-=s;
                    if (slack[l]==0) {
                        // Begin look at a new zero 22
                        k=slack_row[l];
                        if (row_mate[l]<0) {
                            for (j=l+1;j<n;j++)
                                if (slack[j]==0)
                                    col_inc[j]+=s;
                            goto breakthru;
                        }
                        else {
                            parent_row[l]=k;
                            unchosen_row[t++]=row_mate[l];
                        }
                        // End look at a new zero 22
                    }
                }
                else
                    col_inc[l]+=s;
                    // End introduce a new zero into the matrix 21
            }
        }
        breakthru:
        // Begin update the matching 20
        while (1) {
            j=col_mate[k];
            col_mate[k]=l;
            row_mate[l]=k;
            if (j<0)
            break;
            k=parent_row[j];
            l=j;
        }
        // End update the matching 20
        if (--unmatched==0)
            goto done;
        // Begin get ready for another stage 17
        t=0;
        for (l=0;l<n;l++) {
            parent_row[l]= -1;
            slack[l]=INF;
        }
        for (k=0;k<m;k++)
            if (col_mate[k]<0)
                unchosen_row[t++]=k;
        // End get ready for another stage 17
    }

    done:

    // Begin doublecheck the solution 23
    for (k=0;k<m;k++)
        for (l=0;l<n;l++)
        if (p->cost[k][l]<row_dec[k]-col_inc[l])
        exit(0);
    for (k=0;k<m;k++) {
        l=col_mate[k];
        if (l<0 || p->cost[k][l]!=row_dec[k]-col_inc[l])
        exit(0);
    }
    k=0;
    for (l=0;l<n;l++)
        if (col_inc[l])
            k++;
    if (k>m)
        exit(0);
    // End doublecheck the solution 23
    // End Hungarian algorithm 18

    for (i=0;i<m;++i) {
        p->assignment[i][col_mate[i]]=HUNGARIAN_ASSIGNED;
        /*TRACE("%d - %d\n", i, col_mate[i]);*/
    }
    for (k=0;k<m;++k) {
        for (l=0;l<n;++l) {
            p->cost[k][l]=p->cost[k][l]-row_dec[k]+col_inc[l];
        }
    }
    for (i=0;i<m;i++)
        cost+=row_dec[i];
    for (i=0;i<n;i++)
        cost-=col_inc[i];

    free(slack);
    free(col_inc);
    free(parent_row);
    free(row_mate);
    free(slack_row);
    free(row_dec);
    free(unchosen_row);
    free(col_mate);
}

/* get the optimal assignment, by calling hungarian_solve above */
void computeCoefSimil(int* P1, int* P2, int* maxInd, int* n, double* coefSimil) {

    /* first, determine weights by computing intersections
     * reads like: weights[i*(*n)+j] == gain when cluster i in P1
     * is assigned to cluster j in P2
     */
    int i,j,k,inter;
    int* utils = malloc((*maxInd) * (*maxInd) * sizeof(int));
    for (i=1; i <= *maxInd; i++) {
        for (j=1; j <= *maxInd; j++) {
            //try label i with label j
            inter=0;
            for (k=0; k < *n; k++) {
                if (P1[k]==i && P2[k]==j) inter++;
            }
            utils[(i-1)*(*maxInd)+(j-1)] = inter;
        }
    }

    //then solve problem :
    hungarian_problem_t p;
    hungarian_init(&p, utils, *maxInd, *maxInd, HUNGARIAN_MODE_MAXIMIZE_UTIL);
    hungarian_solve(&p);

    //and now get optimal assignment :
    *coefSimil = 0.0;
    for (i=0; i < *maxInd; i++) {
        for (j=0; j < *maxInd; j++) {
            if (p.assignment[i][j]) *coefSimil += utils[i*(*maxInd)+j];
        }
    }
    *coefSimil /= (double)(*n);
    hungarian_free(&p);
}
Commit	Line	Data
81923e5c BA	1	/********************************************************************
	2	********************************************************************
	3	**
	4	** libhungarian by Cyrill Stachniss, 2004
	5	** -- modified (very) slightly by Benjamin Auder, 2010
	6	** -- (verbose and printing routines deleted, because used in a R package)
	7	**
	8	**
	9	** Solving the Minimum Assignment Problem using the
	10	** Hungarian Method.
	11	**
	12	This file may be freely copied and distributed! **
	13	**
	14	** Parts of the used code was originally provided by the
	15	** "Stanford GraphGase", but I made changes to this code.
	16	** As asked by the copyright node of the "Stanford GraphGase",
	17	** I hereby proclaim that this file are NOT part of the
	18	** "Stanford GraphGase" distribution!
	19	**
	20	** This file is distributed in the hope that it will be useful,
	21	** but WITHOUT ANY WARRANTY; without even the implied
	22	** warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
	23	** PURPOSE.
	24	**
	25	********************************************************************
	26	********************************************************************/
	27
	28	#include <stdio.h>
	29	#include <stdlib.h>
	30
	31	#define HUNGARIAN_NOT_ASSIGNED 0
	32	#define HUNGARIAN_ASSIGNED 1
	33
	34	#define HUNGARIAN_MODE_MINIMIZE_COST 0
	35	#define HUNGARIAN_MODE_MAXIMIZE_UTIL 1
	36
	37	typedef struct {
	38	int num_rows;
	39	int num_cols;
	40	int** cost;
	41	int** assignment;
	42	} hungarian_problem_t;
	43
	44	#define INF (0x7FFFFFFF)
	45	#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)
	46
	47	/** This method initialize the hungarian_problem structure and init
	48	* the cost matrices (missing lines or columns are filled with 0).
	49	* It returns the size of the quadratic(!) assignment matrix. **/
	50	void hungarian_init(hungarian_problem_t* p, int* cost_matrix, int rows, int cols, int mode) {
	51
	52	int i,j, org_cols, org_rows;
	53	int max_cost;
	54	max_cost = 0;
	55
	56	org_cols = cols;
	57	org_rows = rows;
	58
	59	// is the number of cols not equal to number of rows ?
	60	// if yes, expand with 0-cols / 0-cols
	61	rows = (cols < rows) ? rows : cols;
	62	cols = rows;
	63
	64	p->num_rows = rows;
65	p->num_cols = cols;
66
67	p->cost = (int*)calloc(rows,sizeof(int));
68	hungarian_test_alloc(p->cost);
69	p->assignment = (int*)calloc(rows,sizeof(int));
70	hungarian_test_alloc(p->assignment);
71
72	for(i=0; i<p->num_rows; i++) {
73	p->cost[i] = (int*)calloc(cols,sizeof(int));
74	hungarian_test_alloc(p->cost[i]);
75	p->assignment[i] = (int*)calloc(cols,sizeof(int));
76	hungarian_test_alloc(p->assignment[i]);
77	for(j=0; j<p->num_cols; j++) {
78	p->cost[i][j] = (i < org_rows && j < org_cols) ? cost_matrix[i*(p->num_cols)+j] : 0;
79	p->assignment[i][j] = 0;
80
81	if (max_cost < p->cost[i][j])
82	max_cost = p->cost[i][j];
83	}
84	}
85
86	if (mode == HUNGARIAN_MODE_MAXIMIZE_UTIL) {
87	for(i=0; i<p->num_rows; i++) {
88	for(j=0; j<p->num_cols; j++) {
89	p->cost[i][j] = max_cost - p->cost[i][j];
90	}
91	}
92	}
93	}
94
95	/ Free the memory allocated by init. /
96	void hungarian_free(hungarian_problem_t* p) {
97	int i;
98	for(i=0; i<p->num_rows; i++) {
99	free(p->cost[i]);
100	free(p->assignment[i]);
101	}
102	free(p->cost);
103	free(p->assignment);
104	p->cost = NULL;
105	p->assignment = NULL;
106	}
107
108	/ This method computes the optimal assignment. /
109	void hungarian_solve(hungarian_problem_t* p) {
110
111	int i, j, m, n, k, l, s, t, q, unmatched, cost;
112	int* col_mate;
113	int* row_mate;
114	int* parent_row;
115	int* unchosen_row;
116	int* row_dec;
117	int* col_inc;
118	int* slack;
119	int* slack_row;
120
121	cost=0;
122	m =p->num_rows;
123	n =p->num_cols;
124
125	col_mate = (int*)calloc(p->num_rows,sizeof(int));
126	hungarian_test_alloc(col_mate);
127	unchosen_row = (int*)calloc(p->num_rows,sizeof(int));
128	hungarian_test_alloc(unchosen_row);
129	row_dec = (int*)calloc(p->num_rows,sizeof(int));
130	hungarian_test_alloc(row_dec);
131	slack_row = (int*)calloc(p->num_rows,sizeof(int));
132	hungarian_test_alloc(slack_row);
133
134	row_mate = (int*)calloc(p->num_cols,sizeof(int));
135	hungarian_test_alloc(row_mate);
136	parent_row = (int*)calloc(p->num_cols,sizeof(int));
137	hungarian_test_alloc(parent_row);
138	col_inc = (int*)calloc(p->num_cols,sizeof(int));
139	hungarian_test_alloc(col_inc);
140	slack = (int*)calloc(p->num_cols,sizeof(int));
141	hungarian_test_alloc(slack);
142
143	for (i=0;i<p->num_rows;i++) {
144	col_mate[i]=0;
145	unchosen_row[i]=0;
146	row_dec[i]=0;
147	slack_row[i]=0;
148	}
149	for (j=0;j<p->num_cols;j++) {
150	row_mate[j]=0;
151	parent_row[j] = 0;
152	col_inc[j]=0;
153	slack[j]=0;
154	}
155
156	for (i=0;i<p->num_rows;++i)
157	for (j=0;j<p->num_cols;++j)
158	p->assignment[i][j]=HUNGARIAN_NOT_ASSIGNED;
159
160	// Begin subtract column minima in order to start with lots of zeroes 12
161	for (l=0;l<n;l++) {
162	s=p->cost[0][l];
163	for (k=1;k<m;k++)
164	if (p->cost[k][l]<s)
165	s=p->cost[k][l];
166	cost+=s;
167	if (s!=0)
168	for (k=0;k<m;k++)
169	p->cost[k][l]-=s;
170	}
171	// End subtract column minima in order to start with lots of zeroes 12
172
173	// Begin initial state 16
174	t=0;
175	for (l=0;l<n;l++) {
176	row_mate[l]= -1;
177	parent_row[l]= -1;
178	col_inc[l]=0;
179	slack[l]=INF;
180	}
181	for (k=0;k<m;k++) {
182	s=p->cost[k][0];
183	for (l=1;l<n;l++)
184	if (p->cost[k][l]<s)
185	s=p->cost[k][l];
186	row_dec[k]=s;
187	for (l=0;l<n;l++)
188	if (s==p->cost[k][l] && row_mate[l]<0) {
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	q=0;
206	while (1) {
207	while (q<t) {
208	// Begin explore node q of the forest 19
209	{
210	k=unchosen_row[q];
211	s=row_dec[k];
212	for (l=0;l<n;l++)
213	if (slack[l]) {
214	int del;
215	del=p->cost[k][l]-s+col_inc[l];
216	if (del<slack[l]) {
217	if (del==0) {
218	if (row_mate[l]<0)
219	goto breakthru;
220	slack[l]=0;
221	parent_row[l]=k;
222	unchosen_row[t++]=row_mate[l];
223	}
224	else {
225	slack[l]=del;
226	slack_row[l]=k;
227	}
228	}
229	}
230	}
231	// End explore node q of the forest 19
232	q++;
233	}
234
235	// Begin introduce a new zero into the matrix 21
236	s=INF;
237	for (l=0;l<n;l++)
238	if (slack[l] && slack[l]<s)
239	s=slack[l];
240	for (q=0;q<t;q++)
241	row_dec[unchosen_row[q]]+=s;
242	for (l=0;l<n;l++) {
243	if (slack[l]) {
244	slack[l]-=s;
245	if (slack[l]==0) {
246	// Begin look at a new zero 22
247	k=slack_row[l];
248	if (row_mate[l]<0) {
249	for (j=l+1;j<n;j++)
250	if (slack[j]==0)
251	col_inc[j]+=s;
252	goto breakthru;
253	}
254	else {
255	parent_row[l]=k;
256	unchosen_row[t++]=row_mate[l];
257	}
258	// End look at a new zero 22
259	}
260	}
261	else
262	col_inc[l]+=s;
263	// End introduce a new zero into the matrix 21
264	}
265	}
266	breakthru:
267	// Begin update the matching 20
268	while (1) {
269	j=col_mate[k];
270	col_mate[k]=l;
271	row_mate[l]=k;
272	if (j<0)
273	break;
274	k=parent_row[j];
275	l=j;
276	}
277	// End update the matching 20
278	if (--unmatched==0)
279	goto done;
280	// Begin get ready for another stage 17
281	t=0;
282	for (l=0;l<n;l++) {
283	parent_row[l]= -1;
284	slack[l]=INF;
285	}
286	for (k=0;k<m;k++)
287	if (col_mate[k]<0)
288	unchosen_row[t++]=k;
289	// End get ready for another stage 17
290	}
291
292	done:
293
294	// Begin doublecheck the solution 23
295	for (k=0;k<m;k++)
296	for (l=0;l<n;l++)
297	if (p->cost[k][l]<row_dec[k]-col_inc[l])
298	exit(0);
299	for (k=0;k<m;k++) {
300	l=col_mate[k];
301	if (l<0 \|\| p->cost[k][l]!=row_dec[k]-col_inc[l])
302	exit(0);
303	}
304	k=0;
305	for (l=0;l<n;l++)
306	if (col_inc[l])
307	k++;
308	if (k>m)
309	exit(0);
310	// End doublecheck the solution 23
311	// End Hungarian algorithm 18
312
313	for (i=0;i<m;++i) {
314	p->assignment[i][col_mate[i]]=HUNGARIAN_ASSIGNED;
315	/TRACE("%d - %d\n", i, col_mate[i]);/
316	}
317	for (k=0;k<m;++k) {
318	for (l=0;l<n;++l) {
319	p->cost[k][l]=p->cost[k][l]-row_dec[k]+col_inc[l];
320	}
321	}
322	for (i=0;i<m;i++)
323	cost+=row_dec[i];
324	for (i=0;i<n;i++)
325	cost-=col_inc[i];
326
327	free(slack);
328	free(col_inc);
329	free(parent_row);
330	free(row_mate);
331	free(slack_row);
332	free(row_dec);
333	free(unchosen_row);
334	free(col_mate);
335	}
336
337	/* get the optimal assignment, by calling hungarian_solve above */
338	void computeCoefSimil(int* P1, int* P2, int* maxInd, int* n, double* coefSimil) {
339
340	/* first, determine weights by computing intersections
341	* reads like: weights[i(n)+j] == gain when cluster i in P1
342	* is assigned to cluster j in P2
343	*/
344	int i,j,k,inter;
345	int* utils = malloc((maxInd) (maxInd) sizeof(int));
346	for (i=1; i <= *maxInd; i++) {
347	for (j=1; j <= *maxInd; j++) {
348	//try label i with label j
349	inter=0;
350	for (k=0; k < *n; k++) {
351	if (P1[k]==i && P2[k]==j) inter++;
352	}
353	utils[(i-1)(maxInd)+(j-1)] = inter;
354	}
355	}
356
357	//then solve problem :
358	hungarian_problem_t p;
359	hungarian_init(&p, utils, maxInd, maxInd, HUNGARIAN_MODE_MAXIMIZE_UTIL);
360	hungarian_solve(&p);
361
362	//and now get optimal assignment :
363	*coefSimil = 0.0;
364	for (i=0; i < *maxInd; i++) {
365	for (j=0; j < *maxInd; j++) {
366	if (p.assignment[i][j]) coefSimil += utils[i(*maxInd)+j];
367	}
368	}
369	coefSimil /= (double)(n);
370	hungarian_free(&p);
371	}