1  //Source: https://github.com/mcximing/hungarianalgorithmcpp/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 mexfunctions for use in MATLAB, found here:


9  // http://www.mathworks.com/matlabcentral/fileexchange/6543functionsfortherectangularassignmentproblem


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 nonnegative, 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; /* MATLABIndexing */


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  }

