source: cpp/frams/model/similarity/hungarian/hungarian.cpp @ 870

Last change on this file since 870 was 869, checked in by Maciej Komosinski, 6 years ago

Added another, improved way of calculating dissimilarity of two creatures/models. Details and comparisons in https://doi.org/10.1007/978-3-030-16692-2_8

File size: 11.8 KB
Line 
1//Source: https://github.com/mcximing/hungarian-algorithm-cpp/blob/master/Hungarian.cpp
2//HungarianAlgorithm::Solve() was changed to take arrays instead of vectors as an input
3
4///////////////////////////////////////////////////////////////////////////////
5// Hungarian.cpp: Implementation file for Class HungarianAlgorithm.
6//
7// This is a C++ wrapper with slight modification of a hungarian algorithm implementation by Markus Buehren.
8// The original implementation is a few mex-functions for use in MATLAB, found here:
9// http://www.mathworks.com/matlabcentral/fileexchange/6543-functions-for-the-rectangular-assignment-problem
10//
11// Both this code and the orignal code are published under the BSD license.
12// by Cong Ma, 2016
13
14
15#include <stdlib.h>
16#include <cfloat> // for DBL_MAX
17#include <math.h>  // for fabs()
18#include "hungarian.h"
19#include <common/log.h>
20
21
22HungarianAlgorithm::HungarianAlgorithm() {}
23HungarianAlgorithm::~HungarianAlgorithm() {}
24
25
26//********************************************************//
27// A single function wrapper for solving assignment problem.
28//********************************************************//
29double HungarianAlgorithm::Solve(double *&distMatrixIn, int *&assignment, int nRows, int nCols)
30{
31        double cost = 0.0;
32
33        // call solving function
34        assignmentoptimal(assignment, &cost, distMatrixIn, nRows, nCols);
35
36        return cost;
37}
38
39
40//********************************************************//
41// Solve optimal solution for assignment problem using Munkres algorithm, also known as Hungarian Algorithm.
42//********************************************************//
43void HungarianAlgorithm::assignmentoptimal(int *assignment, double *cost, double *distMatrixIn, int nOfRows, int nOfColumns)
44{
45        double *distMatrix, *distMatrixTemp, *distMatrixEnd, *columnEnd, value, minValue;
46        bool *coveredColumns, *coveredRows, *starMatrix, *newStarMatrix, *primeMatrix;
47        int nOfElements, minDim, row, col;
48
49        /* initialization */
50        *cost = 0;
51        for (row = 0; row < nOfRows; row++)
52                assignment[row] = -1;
53
54        /* generate working copy of distance Matrix */
55        /* check if all matrix elements are positive */
56        nOfElements = nOfRows * nOfColumns;
57        distMatrix = (double *)malloc(nOfElements * sizeof(double));
58        distMatrixEnd = distMatrix + nOfElements;
59
60        for (row = 0; row < nOfElements; row++)
61        {
62                value = distMatrixIn[row];
63                if (value < 0)
64                        logPrintf("HungarianAlgorithm", "assignmentoptimal", LOG_ERROR, "All matrix elements have to be non-negative, not %g", value);
65                distMatrix[row] = value;
66        }
67
68
69        /* memory allocation */
70        coveredColumns = (bool *)calloc(nOfColumns, sizeof(bool));
71        coveredRows = (bool *)calloc(nOfRows, sizeof(bool));
72        starMatrix = (bool *)calloc(nOfElements, sizeof(bool));
73        primeMatrix = (bool *)calloc(nOfElements, sizeof(bool));
74        newStarMatrix = (bool *)calloc(nOfElements, sizeof(bool)); /* used in step4 */
75
76        /* preliminary steps */
77        if (nOfRows <= nOfColumns)
78        {
79                minDim = nOfRows;
80
81                for (row = 0; row < nOfRows; row++)
82                {
83                        /* find the smallest element in the row */
84                        distMatrixTemp = distMatrix + row;
85                        minValue = *distMatrixTemp;
86                        distMatrixTemp += nOfRows;
87                        while (distMatrixTemp < distMatrixEnd)
88                        {
89                                value = *distMatrixTemp;
90                                if (value < minValue)
91                                        minValue = value;
92                                distMatrixTemp += nOfRows;
93                        }
94
95                        /* subtract the smallest element from each element of the row */
96                        distMatrixTemp = distMatrix + row;
97                        while (distMatrixTemp < distMatrixEnd)
98                        {
99                                *distMatrixTemp -= minValue;
100                                distMatrixTemp += nOfRows;
101                        }
102                }
103
104                /* Steps 1 and 2a */
105                for (row = 0; row < nOfRows; row++)
106                        for (col = 0; col < nOfColumns; col++)
107                                if (fabs(distMatrix[row + nOfRows * col]) < DBL_EPSILON)
108                                        if (!coveredColumns[col])
109                                        {
110                                                starMatrix[row + nOfRows * col] = true;
111                                                coveredColumns[col] = true;
112                                                break;
113                                        }
114        }
115        else /* if(nOfRows > nOfColumns) */
116        {
117                minDim = nOfColumns;
118
119                for (col = 0; col < nOfColumns; col++)
120                {
121                        /* find the smallest element in the column */
122                        distMatrixTemp = distMatrix + nOfRows * col;
123                        columnEnd = distMatrixTemp + nOfRows;
124
125                        minValue = *distMatrixTemp++;
126                        while (distMatrixTemp < columnEnd)
127                        {
128                                value = *distMatrixTemp++;
129                                if (value < minValue)
130                                        minValue = value;
131                        }
132
133                        /* subtract the smallest element from each element of the column */
134                        distMatrixTemp = distMatrix + nOfRows * col;
135                        while (distMatrixTemp < columnEnd)
136                                *distMatrixTemp++ -= minValue;
137                }
138
139                /* Steps 1 and 2a */
140                for (col = 0; col < nOfColumns; col++)
141                        for (row = 0; row < nOfRows; row++)
142                                if (fabs(distMatrix[row + nOfRows * col]) < DBL_EPSILON)
143                                        if (!coveredRows[row])
144                                        {
145                                                starMatrix[row + nOfRows * col] = true;
146                                                coveredColumns[col] = true;
147                                                coveredRows[row] = true;
148                                                break;
149                                        }
150                for (row = 0; row < nOfRows; row++)
151                        coveredRows[row] = false;
152
153        }
154
155        /* move to step 2b */
156        step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
157
158        /* compute cost and remove invalid assignments */
159        computeassignmentcost(assignment, cost, distMatrixIn, nOfRows);
160
161        /* free allocated memory */
162        free(distMatrix);
163        free(coveredColumns);
164        free(coveredRows);
165        free(starMatrix);
166        free(primeMatrix);
167        free(newStarMatrix);
168
169        return;
170}
171
172/********************************************************/
173void HungarianAlgorithm::buildassignmentvector(int *assignment, bool *starMatrix, int nOfRows, int nOfColumns)
174{
175        int row, col;
176
177        for (row = 0; row < nOfRows; row++)
178                for (col = 0; col < nOfColumns; col++)
179                        if (starMatrix[row + nOfRows * col])
180                        {
181#ifdef ONE_INDEXING
182                                assignment[row] = col + 1; /* MATLAB-Indexing */
183#else
184                                assignment[row] = col;
185#endif
186                                break;
187                        }
188}
189
190/********************************************************/
191void HungarianAlgorithm::computeassignmentcost(int *assignment, double *cost, double *distMatrix, int nOfRows)
192{
193        int row, col;
194
195        for (row = 0; row < nOfRows; row++)
196        {
197                col = assignment[row];
198                if (col >= 0)
199                        *cost += distMatrix[row + nOfRows * col];
200        }
201}
202
203/********************************************************/
204void HungarianAlgorithm::step2a(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
205{
206        bool *starMatrixTemp, *columnEnd;
207        int col;
208
209        /* cover every column containing a starred zero */
210        for (col = 0; col < nOfColumns; col++)
211        {
212                starMatrixTemp = starMatrix + nOfRows * col;
213                columnEnd = starMatrixTemp + nOfRows;
214                while (starMatrixTemp < columnEnd) {
215                        if (*starMatrixTemp++)
216                        {
217                                coveredColumns[col] = true;
218                                break;
219                        }
220                }
221        }
222
223        /* move to step 3 */
224        step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
225}
226
227/********************************************************/
228void HungarianAlgorithm::step2b(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
229{
230        int col, nOfCoveredColumns;
231
232        /* count covered columns */
233        nOfCoveredColumns = 0;
234        for (col = 0; col < nOfColumns; col++)
235                if (coveredColumns[col])
236                        nOfCoveredColumns++;
237
238        if (nOfCoveredColumns == minDim)
239        {
240                /* algorithm finished */
241                buildassignmentvector(assignment, starMatrix, nOfRows, nOfColumns);
242        }
243        else
244        {
245                /* move to step 3 */
246                step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
247        }
248
249}
250
251/********************************************************/
252void HungarianAlgorithm::step3(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
253{
254        bool zerosFound;
255        int row, col, starCol;
256
257        zerosFound = true;
258        while (zerosFound)
259        {
260                zerosFound = false;
261                for (col = 0; col < nOfColumns; col++)
262                        if (!coveredColumns[col])
263                                for (row = 0; row < nOfRows; row++)
264                                        if ((!coveredRows[row]) && (fabs(distMatrix[row + nOfRows * col]) < DBL_EPSILON))
265                                        {
266                                                /* prime zero */
267                                                primeMatrix[row + nOfRows * col] = true;
268
269                                                /* find starred zero in current row */
270                                                for (starCol = 0; starCol < nOfColumns; starCol++)
271                                                        if (starMatrix[row + nOfRows * starCol])
272                                                                break;
273
274                                                if (starCol == nOfColumns) /* no starred zero found */
275                                                {
276                                                        /* move to step 4 */
277                                                        step4(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim, row, col);
278                                                        return;
279                                                }
280                                                else
281                                                {
282                                                        coveredRows[row] = true;
283                                                        coveredColumns[starCol] = false;
284                                                        zerosFound = true;
285                                                        break;
286                                                }
287                                        }
288        }
289
290        /* move to step 5 */
291        step5(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
292}
293
294/********************************************************/
295void HungarianAlgorithm::step4(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim, int row, int col)
296{
297        int n, starRow, starCol, primeRow, primeCol;
298        int nOfElements = nOfRows * nOfColumns;
299
300        /* generate temporary copy of starMatrix */
301        for (n = 0; n < nOfElements; n++)
302                newStarMatrix[n] = starMatrix[n];
303
304        /* star current zero */
305        newStarMatrix[row + nOfRows * col] = true;
306
307        /* find starred zero in current column */
308        starCol = col;
309        for (starRow = 0; starRow < nOfRows; starRow++)
310                if (starMatrix[starRow + nOfRows * starCol])
311                        break;
312
313        while (starRow < nOfRows)
314        {
315                /* unstar the starred zero */
316                newStarMatrix[starRow + nOfRows * starCol] = false;
317
318                /* find primed zero in current row */
319                primeRow = starRow;
320                for (primeCol = 0; primeCol < nOfColumns; primeCol++)
321                        if (primeMatrix[primeRow + nOfRows * primeCol])
322                                break;
323
324                /* star the primed zero */
325                newStarMatrix[primeRow + nOfRows * primeCol] = true;
326
327                /* find starred zero in current column */
328                starCol = primeCol;
329                for (starRow = 0; starRow < nOfRows; starRow++)
330                        if (starMatrix[starRow + nOfRows * starCol])
331                                break;
332        }
333
334        /* use temporary copy as new starMatrix */
335        /* delete all primes, uncover all rows */
336        for (n = 0; n < nOfElements; n++)
337        {
338                primeMatrix[n] = false;
339                starMatrix[n] = newStarMatrix[n];
340        }
341        for (n = 0; n < nOfRows; n++)
342                coveredRows[n] = false;
343
344        /* move to step 2a */
345        step2a(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
346}
347
348/********************************************************/
349void HungarianAlgorithm::step5(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
350{
351        double h, value;
352        int row, col;
353
354        /* find smallest uncovered element h */
355        h = DBL_MAX;
356        for (row = 0; row < nOfRows; row++)
357                if (!coveredRows[row])
358                        for (col = 0; col < nOfColumns; col++)
359                                if (!coveredColumns[col])
360                                {
361                                        value = distMatrix[row + nOfRows * col];
362                                        if (value < h)
363                                                h = value;
364                                }
365
366        /* add h to each covered row */
367        for (row = 0; row < nOfRows; row++)
368                if (coveredRows[row])
369                        for (col = 0; col < nOfColumns; col++)
370                                distMatrix[row + nOfRows * col] += h;
371
372        /* subtract h from each uncovered column */
373        for (col = 0; col < nOfColumns; col++)
374                if (!coveredColumns[col])
375                        for (row = 0; row < nOfRows; row++)
376                                distMatrix[row + nOfRows * col] -= h;
377
378        /* move to step 3 */
379        step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
380}
Note: See TracBrowser for help on using the repository browser.