23 vector<vector<double>> &DistMatrix,
24 vector<int> &Assignment)
26 unsigned int nRows = DistMatrix.size();
27 unsigned int nCols = DistMatrix[0].size();
29 double *distMatrixIn =
new double[nRows * nCols];
30 int *assignment =
new int[nRows];
37 for (
unsigned int i = 0; i < nRows; i++)
38 for (
unsigned int j = 0; j < nCols; j++)
39 distMatrixIn[i + nRows * j] = DistMatrix[i][j];
42 assignmentoptimal(assignment, &cost, distMatrixIn, nRows, nCols);
45 for (
unsigned int r = 0; r < nRows; r++)
46 Assignment.push_back(assignment[r]);
48 delete[] distMatrixIn;
56 void HungarianAlgorithm::assignmentoptimal(
63 double *distMatrix, *distMatrixTemp, *distMatrixEnd, *columnEnd, value, minValue;
64 bool *coveredColumns, *coveredRows, *starMatrix, *newStarMatrix, *primeMatrix;
65 int nOfElements, minDim, row, col;
69 for (row = 0; row < nOfRows; row++)
74 nOfElements = nOfRows * nOfColumns;
75 distMatrix = (
double *)malloc(nOfElements *
sizeof(
double));
76 distMatrixEnd = distMatrix + nOfElements;
78 for (row = 0; row < nOfElements; row++)
80 value = distMatrixIn[row];
82 cerr <<
"All matrix elements have to be non-negative." << endl;
83 distMatrix[row] = value;
87 coveredColumns = (
bool *)calloc(nOfColumns,
sizeof(
bool));
88 coveredRows = (
bool *)calloc(nOfRows,
sizeof(
bool));
89 starMatrix = (
bool *)calloc(nOfElements,
sizeof(
bool));
90 primeMatrix = (
bool *)calloc(nOfElements,
sizeof(
bool));
91 newStarMatrix = (
bool *)calloc(nOfElements,
sizeof(
bool));
94 if (nOfRows <= nOfColumns)
98 for (row = 0; row < nOfRows; row++)
101 distMatrixTemp = distMatrix + row;
102 minValue = *distMatrixTemp;
103 distMatrixTemp += nOfRows;
104 while (distMatrixTemp < distMatrixEnd)
106 value = *distMatrixTemp;
107 if (value < minValue)
109 distMatrixTemp += nOfRows;
113 distMatrixTemp = distMatrix + row;
114 while (distMatrixTemp < distMatrixEnd)
116 *distMatrixTemp -= minValue;
117 distMatrixTemp += nOfRows;
122 for (row = 0; row < nOfRows; row++)
123 for (col = 0; col < nOfColumns; col++)
124 if (fabs(distMatrix[row + nOfRows * col]) < DBL_EPSILON)
125 if (!coveredColumns[col])
127 starMatrix[row + nOfRows * col] =
true;
128 coveredColumns[col] =
true;
136 for (col = 0; col < nOfColumns; col++)
139 distMatrixTemp = distMatrix + nOfRows * col;
140 columnEnd = distMatrixTemp + nOfRows;
142 minValue = *distMatrixTemp++;
143 while (distMatrixTemp < columnEnd)
145 value = *distMatrixTemp++;
146 if (value < minValue)
151 distMatrixTemp = distMatrix + nOfRows * col;
152 while (distMatrixTemp < columnEnd)
153 *distMatrixTemp++ -= minValue;
157 for (col = 0; col < nOfColumns; col++)
158 for (row = 0; row < nOfRows; row++)
159 if (fabs(distMatrix[row + nOfRows * col]) < DBL_EPSILON)
160 if (!coveredRows[row])
162 starMatrix[row + nOfRows * col] =
true;
163 coveredColumns[col] =
true;
164 coveredRows[row] =
true;
167 for (row = 0; row < nOfRows; row++)
168 coveredRows[row] =
false;
172 step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
175 computeassignmentcost(assignment, cost, distMatrixIn, nOfRows);
179 free(coveredColumns);
189 void HungarianAlgorithm::buildassignmentvector(
195 for (
int row = 0; row < nOfRows; row++)
196 for (
int col = 0; col < nOfColumns; col++)
197 if (starMatrix[row + nOfRows * col])
200 assignment[row] = col + 1;
202 assignment[row] = col;
209 void HungarianAlgorithm::computeassignmentcost(
217 for (row = 0; row < nOfRows; row++)
219 col = assignment[row];
221 *cost += distMatrix[row + nOfRows * col];
226 void HungarianAlgorithm::step2a(
232 bool *coveredColumns,
238 bool *starMatrixTemp, *columnEnd;
242 for (col = 0; col < nOfColumns; col++)
244 starMatrixTemp = starMatrix + nOfRows * col;
245 columnEnd = starMatrixTemp + nOfRows;
246 while (starMatrixTemp < columnEnd)
248 if (*starMatrixTemp++)
250 coveredColumns[col] =
true;
257 step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
261 void HungarianAlgorithm::step2b(
267 bool *coveredColumns,
273 int col, nOfCoveredColumns;
276 nOfCoveredColumns = 0;
277 for (col = 0; col < nOfColumns; col++)
278 if (coveredColumns[col])
281 if (nOfCoveredColumns == minDim)
284 buildassignmentvector(assignment, starMatrix, nOfRows, nOfColumns);
289 step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
294 void HungarianAlgorithm::step3(
300 bool *coveredColumns,
307 int row, col, starCol;
313 for (col = 0; col < nOfColumns; col++)
314 if (!coveredColumns[col])
315 for (row = 0; row < nOfRows; row++)
316 if ((!coveredRows[row]) && (fabs(distMatrix[row + nOfRows * col]) < DBL_EPSILON))
319 primeMatrix[row + nOfRows * col] =
true;
322 for (starCol = 0; starCol < nOfColumns; starCol++)
323 if (starMatrix[row + nOfRows * starCol])
326 if (starCol == nOfColumns)
329 step4(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim, row, col);
334 coveredRows[row] =
true;
335 coveredColumns[starCol] =
false;
343 step5(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
347 void HungarianAlgorithm::step4(
353 bool *coveredColumns,
361 int n, starRow, starCol, primeRow, primeCol;
362 int nOfElements = nOfRows * nOfColumns;
365 for (n = 0; n < nOfElements; n++)
366 newStarMatrix[n] = starMatrix[n];
369 newStarMatrix[row + nOfRows * col] =
true;
373 for (starRow = 0; starRow < nOfRows; starRow++)
374 if (starMatrix[starRow + nOfRows * starCol])
377 while (starRow < nOfRows)
380 newStarMatrix[starRow + nOfRows * starCol] =
false;
384 for (primeCol = 0; primeCol < nOfColumns; primeCol++)
385 if (primeMatrix[primeRow + nOfRows * primeCol])
389 newStarMatrix[primeRow + nOfRows * primeCol] =
true;
393 for (starRow = 0; starRow < nOfRows; starRow++)
394 if (starMatrix[starRow + nOfRows * starCol])
400 for (n = 0; n < nOfElements; n++)
402 primeMatrix[n] =
false;
403 starMatrix[n] = newStarMatrix[n];
405 for (n = 0; n < nOfRows; n++)
406 coveredRows[n] =
false;
409 step2a(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
413 void HungarianAlgorithm::step5(
419 bool *coveredColumns,
430 for (row = 0; row < nOfRows; row++)
431 if (!coveredRows[row])
432 for (col = 0; col < nOfColumns; col++)
433 if (!coveredColumns[col])
435 value = distMatrix[row + nOfRows * col];
441 for (row = 0; row < nOfRows; row++)
442 if (coveredRows[row])
443 for (col = 0; col < nOfColumns; col++)
444 distMatrix[row + nOfRows * col] += h;
447 for (col = 0; col < nOfColumns; col++)
448 if (!coveredColumns[col])
449 for (row = 0; row < nOfRows; row++)
450 distMatrix[row + nOfRows * col] -= h;
453 step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
double Solve(std::vector< std::vector< double >> &DistMatrix, std::vector< int > &Assignment)