| 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 |
|
|---|
| 22 | HungarianAlgorithm::HungarianAlgorithm() {}
|
|---|
| 23 | HungarianAlgorithm::~HungarianAlgorithm() {}
|
|---|
| 24 |
|
|---|
| 25 |
|
|---|
| 26 | //********************************************************//
|
|---|
| 27 | // A single function wrapper for solving assignment problem.
|
|---|
| 28 | //********************************************************//
|
|---|
| 29 | double 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 | //********************************************************//
|
|---|
| 43 | void 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 | /********************************************************/
|
|---|
| 173 | void 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 | /********************************************************/
|
|---|
| 191 | void 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 | /********************************************************/
|
|---|
| 204 | void 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 | /********************************************************/
|
|---|
| 228 | void 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 | /********************************************************/
|
|---|
| 252 | void 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 | /********************************************************/
|
|---|
| 295 | void 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 | /********************************************************/
|
|---|
| 349 | void 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 | }
|
|---|
