[morpheus.git] / pkg / src / hungarian.c

/********************************************************************
 ********************************************************************
 **
 ** libhungarian by Cyrill Stachniss, 2004
 ** Modified by Benjamin Auder to work on floating point number,
 ** and to fit into a R package (2017)
 **
 ** 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 <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

#define INF (1e32)
#define verbose (0)

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

int hungarian_imax(int a, int b) {
	return (a<b)?b:a;
}

/** 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. **/
int hungarian_init(hungarian_problem_t* p, double** cost_matrix, int rows, int cols,
	int mode)
{
	int i,j;
	double max_cost = 0.;
	int 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 = hungarian_imax(cols, rows);
	cols = rows;

	p->num_rows = rows;
	p->num_cols = cols;

	p->cost = (double**)calloc(rows,sizeof(double*));
	p->assignment = (int**)calloc(rows,sizeof(int*));

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

/** 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, t, q, unmatched;
	double cost, s;
	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));
	unchosen_row = (int*)calloc(p->num_rows,sizeof(int));
	row_dec	= (double*)calloc(p->num_rows,sizeof(double));
	slack_row	= (int*)calloc(p->num_rows,sizeof(int));

	row_mate = (int*)calloc(p->num_cols,sizeof(int));
	parent_row = (int*)calloc(p->num_cols,sizeof(int));
	col_inc = (double*)calloc(p->num_cols,sizeof(double));
	slack = (double*)calloc(p->num_cols,sizeof(double));

	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] > 0.)
						{
							double 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]>0. && 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]>0.)
				{
					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)
		{
			/*TRACE("%d ",p->cost[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<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);
}


/***************
 * Usage in R :
 **************/

// Auxiliary function for hungarianAlgorithm below
double** array_to_matrix(double* m, int rows, int cols)
{
  double** r = (double**)calloc(rows,sizeof(double*));
  for (int i=0; i<rows; i++)
  {
    r[i] = (double*)calloc(cols,sizeof(double));
    for (int j=0; j<cols; j++)
      r[i][j] = m[i*cols+j];
  }
  return r;
}

//TODO: re-code this algorithm in a more readable way, based on
//https://www.topcoder.com/community/data-science/data-science-tutorials/assignment-problem-and-hungarian-algorithm/
// Get the optimal assignment, by calling hungarian_solve above; "distances" in columns
void hungarianAlgorithm(double* distances, int* pn, int* assignment)
{
	int n = *pn;
	double** mat = array_to_matrix(distances, n, n);

  hungarian_problem_t p;
	hungarian_init(&p, mat, n, n, HUNGARIAN_MODE_MINIMIZE_COST);

	hungarian_solve(&p);

	// Copy the optimal assignment in a R-friendly structure
	for (int i=0; i < n; i++) {
		for (int j=0; j < n; j++) {
			if (p.assignment[i][j])
				assignment[i] = j+1; //add 1 since we work with R
		}
	}

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