void assignmentoptimal(int *assignment, double *cost, double *distMatrixIn, int nOfRows, int nOfColumns) { double *distMatrix, *distMatrixTemp, *distMatrixEnd, *columnEnd, value, minValue; bool *coveredColumns, *coveredRows, *starMatrix, *newStarMatrix, *primeMatrix; int nOfElements, minDim, row, col; #ifdef CHECK_FOR_INF bool infiniteValueFound; double maxFiniteValue, infValue; #endif /* initialization */ *cost = 0; for(row=0; row<nOfRows; row++) #ifdef ONE_INDEXING assignment[row] = 0; #else assignment[row] = -1; #endif /* generate working copy of distance Matrix */ /* check if all matrix elements are positive */ nOfElements = nOfRows * nOfColumns; distMatrix = (double *)mxMalloc(nOfElements * sizeof(double)); distMatrixEnd = distMatrix + nOfElements; for(row=0; row<nOfElements; row++) { value = distMatrixIn[row]; // if(mxIsFinite(value) && (value < 0)) if(myIsFinite(value) && (value < 0)) mexErrMsgTxt("All matrix elements have to be non-negative."); distMatrix[row] = value; } #ifdef CHECK_FOR_INF /* check for infinite values */ maxFiniteValue = -1; infiniteValueFound = false; distMatrixTemp = distMatrix; while(distMatrixTemp < distMatrixEnd) { value = *distMatrixTemp++; // if(mxIsFinite(value)) if(myIsFinite(value)) { if(value > maxFiniteValue) maxFiniteValue = value; } else infiniteValueFound = true; } if(infiniteValueFound) { if(maxFiniteValue == -1) /* all elements are infinite */ return; /* set all infinite elements to big finite value */ if(maxFiniteValue > 0) infValue = 10 * maxFiniteValue * nOfElements; else infValue = 10; distMatrixTemp = distMatrix; while(distMatrixTemp < distMatrixEnd) // if(mxIsInf(*distMatrixTemp++)) if(myIsFinite(*distMatrixTemp++)) *(distMatrixTemp-1) = infValue; } #endif /* memory allocation */ coveredColumns = (bool *)mxCalloc(nOfColumns, sizeof(bool)); coveredRows = (bool *)mxCalloc(nOfRows, sizeof(bool)); starMatrix = (bool *)mxCalloc(nOfElements, sizeof(bool)); primeMatrix = (bool *)mxCalloc(nOfElements, sizeof(bool)); newStarMatrix = (bool *)mxCalloc(nOfElements, sizeof(bool)); /* used in step4 */ /* preliminary steps */ if(nOfRows <= nOfColumns) { minDim = nOfRows; for(row=0; row<nOfRows; row++) { /* find the smallest element in the row */ distMatrixTemp = distMatrix + row; minValue = *distMatrixTemp; distMatrixTemp += nOfRows; while(distMatrixTemp < distMatrixEnd) { value = *distMatrixTemp; if(value < minValue) minValue = value; distMatrixTemp += nOfRows; } /* subtract the smallest element from each element of the row */ distMatrixTemp = distMatrix + row; while(distMatrixTemp < distMatrixEnd) { *distMatrixTemp -= minValue; distMatrixTemp += nOfRows; } } /* Steps 1 and 2a */ for(row=0; row<nOfRows; row++) for(col=0; col<nOfColumns; col++) if(distMatrix[row + nOfRows*col] == 0) if(!coveredColumns[col]) { starMatrix[row + nOfRows*col] = true; coveredColumns[col] = true; break; } } else /* if(nOfRows > nOfColumns) */ { minDim = nOfColumns; for(col=0; col<nOfColumns; col++) { /* find the smallest element in the column */ distMatrixTemp = distMatrix + nOfRows*col; columnEnd = distMatrixTemp + nOfRows; minValue = *distMatrixTemp++; while(distMatrixTemp < columnEnd) { value = *distMatrixTemp++; if(value < minValue) minValue = value; } /* subtract the smallest element from each element of the column */ distMatrixTemp = distMatrix + nOfRows*col; while(distMatrixTemp < columnEnd) *distMatrixTemp++ -= minValue; } /* Steps 1 and 2a */ for(col=0; col<nOfColumns; col++) for(row=0; row<nOfRows; row++) if(distMatrix[row + nOfRows*col] == 0) if(!coveredRows[row]) { starMatrix[row + nOfRows*col] = true; coveredColumns[col] = true; coveredRows[row] = true; break; } for(row=0; row<nOfRows; row++) coveredRows[row] = false; } /* move to step 2b */ step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim); /* compute cost and remove invalid assignments */ computeassignmentcost(assignment, cost, distMatrixIn, nOfRows); /* free allocated memory */ mxFree(distMatrix); mxFree(coveredColumns); mxFree(coveredRows); mxFree(starMatrix); mxFree(primeMatrix); mxFree(newStarMatrix); return; }
SHARED_EXPORT void assignmentoptimal(double *assignment, double *cost, double *distMatrixIn, int nOfRows, int nOfColumns) { double *distMatrix, *distMatrixTemp, *distMatrixEnd, *columnEnd, value, minValue; char *coveredColumns, *coveredRows, *starMatrix, *newStarMatrix, *primeMatrix; int nOfElements, minDim, row, col; #ifdef CHECK_FOR_INF char infiniteValueFound; double maxFiniteValue, infValue; #endif /* initialization */ *cost = 0; for(row=0; row<nOfRows; row++) #ifdef ONE_INDEXING assignment[row] = 0.0; #else assignment[row] = -1.0; #endif /* generate working copy of distance Matrix */ /* check if all matrix elements are positive */ nOfElements = nOfRows * nOfColumns; distMatrix = (double *)malloc(nOfElements * sizeof(double)); distMatrixEnd = distMatrix + nOfElements; for(row=0; row<nOfElements; row++) { value = distMatrixIn[row]; if(isfinite(value) && (value < 0)) fprintf(stderr, "All matrix elements have to be non-negative.\n"); #ifdef COLUMN_ORDER /* translate an input column-order matrix into row-order form */ distMatrix[ ((int) row/nOfColumns) + nOfRows * (row%nOfColumns) ] = value; #else distMatrix[row] = value; #endif } #ifdef CHECK_FOR_INF /* check for infinite values */ maxFiniteValue = -1; infiniteValueFound = 0; distMatrixTemp = distMatrix; while(distMatrixTemp < distMatrixEnd) { value = *distMatrixTemp++; if(isfinite(value)) { if(value > maxFiniteValue) maxFiniteValue = value; } else infiniteValueFound = 1; } if(infiniteValueFound) { if(maxFiniteValue == -1) /* all elements are infinite */ return; /* set all infinite elements to big finite value */ if(maxFiniteValue > 0) infValue = 10 * maxFiniteValue * nOfElements; else infValue = 10; distMatrixTemp = distMatrix; while(distMatrixTemp < distMatrixEnd) if(isinf(*distMatrixTemp++)) *(distMatrixTemp-1) = infValue; } #endif /* memory allocation */ coveredColumns = (char *)malloc(nOfColumns* sizeof(char)); memset( coveredColumns, 0, nOfColumns * sizeof(char)); coveredRows = (char *)malloc(nOfRows * sizeof(char)); memset( coveredRows , 0, nOfRows * sizeof(char)); starMatrix = (char *)malloc(nOfElements* sizeof(char)); memset( starMatrix , 0, nOfElements * sizeof(char)); primeMatrix = (char *)malloc(nOfElements* sizeof(char)); memset( primeMatrix , 0, nOfElements * sizeof(char)); newStarMatrix = (char *)malloc(nOfElements* sizeof(char)); /* used in step4 */ memset( newStarMatrix , 0, nOfElements * sizeof(char)); /* preliminary steps */ if(nOfRows <= nOfColumns) { minDim = nOfRows; for(row=0; row<nOfRows; row++) { /* find the smallest element in the row */ distMatrixTemp = distMatrix + row; minValue = *distMatrixTemp; distMatrixTemp += nOfRows; while(distMatrixTemp < distMatrixEnd) { value = *distMatrixTemp; if(value < minValue) minValue = value; distMatrixTemp += nOfRows; } /* subtract the smallest element from each element of the row */ distMatrixTemp = distMatrix + row; while(distMatrixTemp < distMatrixEnd) { *distMatrixTemp -= minValue; distMatrixTemp += nOfRows; } } /* Steps 1 and 2a */ for(row=0; row<nOfRows; row++) for(col=0; col<nOfColumns; col++) if(distMatrix[row + nOfRows*col] == 0) if(!coveredColumns[col]) { starMatrix[row + nOfRows*col] = 1; coveredColumns[col] = 1; break; } } else /* if(nOfRows > nOfColumns) */ { minDim = nOfColumns; for(col=0; col<nOfColumns; col++) { /* find the smallest element in the column */ distMatrixTemp = distMatrix + nOfRows*col; columnEnd = distMatrixTemp + nOfRows; minValue = *distMatrixTemp++; while(distMatrixTemp < columnEnd) { value = *distMatrixTemp++; if(value < minValue) minValue = value; } /* subtract the smallest element from each element of the column */ distMatrixTemp = distMatrix + nOfRows*col; while(distMatrixTemp < columnEnd) *distMatrixTemp++ -= minValue; } /* Steps 1 and 2a */ for(col=0; col<nOfColumns; col++) for(row=0; row<nOfRows; row++) if(distMatrix[row + nOfRows*col] == 0) if(!coveredRows[row]) { starMatrix[row + nOfRows*col] = 1; coveredColumns[col] = 1; coveredRows[row] = 1; break; } for(row=0; row<nOfRows; row++) coveredRows[row] = 0; } /* move to step 2b */ step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim); /* compute cost and remove invalid assignments */ computeassignmentcost(assignment, cost, distMatrixIn, nOfRows); /* free allocated memory */ free(distMatrix); free(coveredColumns); free(coveredRows); free(starMatrix); free(primeMatrix); free(newStarMatrix); return; }
// -------------------------------------------------------------------------- // Function Name : assignmentoptimal // Auther : chen chen // Data : 2016-02-15 // Computes the optimal assignment (minimum overall costs) using Munkres algorithm. // -------------------------------------------------------------------------- void AssignmentProblemSolver::assignmentoptimal(int *assignment, double *cost, double *distMatrixIn, int nOfRows, int nOfColumns) { double *distMatrix; double *distMatrixTemp; double *distMatrixEnd; double *columnEnd; double value; double minValue; bool *coveredColumns; bool *coveredRows; bool *starMatrix; bool *newStarMatrix; bool *primeMatrix; int nOfElements; int minDim; int row; int col; // Init *cost = 0; for(row = 0; row < nOfRows; row++) { assignment[row] = -1.0; } // Total elements number nOfElements = nOfRows * nOfColumns; // Memory allocation distMatrix = (double *)malloc(nOfElements * sizeof(double)); // Pointer to last element distMatrixEnd = distMatrix + nOfElements; for(row = 0; row < nOfElements; row++) { value = distMatrixIn[row]; distMatrix[row] = value; } // Memory allocation coveredColumns = (bool *)calloc(nOfColumns, sizeof(bool)); coveredRows = (bool *)calloc(nOfRows, sizeof(bool)); starMatrix = (bool *)calloc(nOfElements, sizeof(bool)); primeMatrix = (bool *)calloc(nOfElements, sizeof(bool)); newStarMatrix = (bool *)calloc(nOfElements, sizeof(bool)); /* used in step4 */ /* preliminary steps */ if(nOfRows <= nOfColumns) { minDim = nOfRows; for(row=0; row<nOfRows; row++) { /* find the smallest element in the row */ distMatrixTemp = distMatrix + row; minValue = *distMatrixTemp; distMatrixTemp += nOfRows; while(distMatrixTemp < distMatrixEnd) { value = *distMatrixTemp; if(value < minValue) { minValue = value; } distMatrixTemp += nOfRows; } /* subtract the smallest element from each element of the row */ distMatrixTemp = distMatrix + row; while(distMatrixTemp < distMatrixEnd) { *distMatrixTemp -= minValue; distMatrixTemp += nOfRows; } } /* Steps 1 and 2a */ for(row = 0; row < nOfRows; row++) { for(col = 0; col < nOfColumns; col++) { if(distMatrix[row + nOfRows*col] == 0) { if(!coveredColumns[col]) { starMatrix[row + nOfRows*col] = true; coveredColumns[col] = true; break; } } } } } else /* if(nOfRows > nOfColumns) */ { minDim = nOfColumns; for(col = 0; col < nOfColumns; col++) { /* find the smallest element in the column */ distMatrixTemp = distMatrix + nOfRows*col; columnEnd = distMatrixTemp + nOfRows; minValue = *distMatrixTemp++; while(distMatrixTemp < columnEnd) { value = *distMatrixTemp++; if(value < minValue) { minValue = value; } } /* subtract the smallest element from each element of the column */ distMatrixTemp = distMatrix + nOfRows*col; while(distMatrixTemp < columnEnd) { *distMatrixTemp++ -= minValue; } } /* Steps 1 and 2a */ for(col = 0; col < nOfColumns; col++) { for(row = 0; row < nOfRows; row++) { if(distMatrix[row + nOfRows*col] == 0) { if(!coveredRows[row]) { starMatrix[row + nOfRows*col] = true; coveredColumns[col] = true; coveredRows[row] = true; break; } } } } for(row = 0; row < nOfRows; row++) { coveredRows[row] = false; } } /* move to step 2b */ step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim); /* compute cost and remove invalid assignments */ computeassignmentcost(assignment, cost, distMatrixIn, nOfRows); /* free allocated memory */ free(distMatrix); free(coveredColumns); free(coveredRows); free(starMatrix); free(primeMatrix); free(newStarMatrix); }