代码
- 编译运行
mkdir build
cd build
cmake ..
make
./data_associate
- 头文件 — hungarian.h
#ifndef HUNGARIAN_H
#define HUNGARIAN_H
#include <iostream>
#include <vector>
class HungarianAlgorithm
{
public:
HungarianAlgorithm();
~HungarianAlgorithm();
std::vector<float> Solve(std::vector<std::vector<float>> &DistMatrix, std::vector<int> &Assignment);
private:
void assignmentoptimal(int *assignment, std::vector<float> &cost, float *distMatrix, int nOfRows, int nOfColumns);
void buildassignmentvector(int *assignment, bool *starMatrix, int nOfRows, int nOfColumns);
void computeassignmentcost(int *assignment, std::vector<float> &cost, float *distMatrix, int nOfRows);
void step2a(int *assignment, float *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
void step2b(int *assignment, float *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
void step3(int *assignment, float *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
void step4(int *assignment, float *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim, int row, int col);
void step5(int *assignment, float *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
};
#endif
- 源文件 — hungarian.cpp
#include <stdlib.h>
#include <cfloat> // for DBL_MAX
#include <cmath> // for fabs()
#include "hungarian.h"
HungarianAlgorithm::HungarianAlgorithm() {}
HungarianAlgorithm::~HungarianAlgorithm() {}
// 输入 关联矩阵
// 输出 匹配位置
// 返回 每一个匹配的cost
std::vector<float> HungarianAlgorithm::Solve(std::vector<std::vector<float>> &DistMatrix, std::vector<int> &Assignment)
{
unsigned int nRows = DistMatrix.size();
unsigned int nCols = DistMatrix[0].size();
int cost_size = std::max(nRows, nCols);
// 初始化cost值为0
std::vector<float> cost(cost_size, 0.0);
float *distMatrixIn = new float[nRows * nCols];
int *assignment = new int[nRows];
for (unsigned int i = 0; i < nRows; i++)
for (unsigned int j = 0; j < nCols; j++)
distMatrixIn[i + nRows * j] = DistMatrix[i][j];
assignmentoptimal(assignment, cost, distMatrixIn, nRows, nCols);
Assignment.clear();
for (unsigned int r = 0; r < nRows; r++)
Assignment.push_back(assignment[r]);
delete[] distMatrixIn;
delete[] assignment;
return cost;
}
void HungarianAlgorithm::assignmentoptimal(int *assignment, std::vector<float> &cost, float *distMatrixIn, int nOfRows, int nOfColumns)
{
float *distMatrix, *distMatrixTemp, *distMatrixEnd, *columnEnd, value, minValue;
bool *coveredColumns, *coveredRows, *starMatrix, *newStarMatrix, *primeMatrix;
int nOfElements, minDim, row, col;
for (row = 0; row < nOfRows; row++)
assignment[row] = -1;
nOfElements = nOfRows * nOfColumns;
distMatrix = (float *)malloc(nOfElements * sizeof(float));
distMatrixEnd = distMatrix + nOfElements;
for (row = 0; row < nOfElements; row++)
distMatrix[row] = distMatrixIn[row];
;
/* 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 (fabs(distMatrix[row + nOfRows * col]) < DBL_EPSILON)
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 (fabs(distMatrix[row + nOfRows * col]) < DBL_EPSILON)
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);
return;
}
void HungarianAlgorithm::buildassignmentvector(int *assignment, bool *starMatrix, int nOfRows, int nOfColumns)
{
int row, col;
for (row = 0; row < nOfRows; row++)
for (col = 0; col < nOfColumns; col++)
if (starMatrix[row + nOfRows * col])
{
#ifdef ONE_INDEXING
assignment[row] = col + 1; /* MATLAB-Indexing */
#else
assignment[row] = col;
#endif
break;
}
}
void HungarianAlgorithm::computeassignmentcost(int *assignment, std::vector<float> &cost, float *distMatrix, int nOfRows)
{
int row, col;
for (row = 0; row < nOfRows; row++)
{
col = assignment[row];
if (col >= 0)
{
cost[row] = distMatrix[row + nOfRows * col];
// std::cout << "cost_" << std::to_string(row) << ":" << distMatrix[row + nOfRows * col] << std::endl;
}
}
}
void HungarianAlgorithm::step2a(int *assignment, float *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
{
if (assignment == NULL || distMatrix == NULL || starMatrix == NULL || newStarMatrix == NULL || primeMatrix == NULL || coveredColumns == NULL || coveredRows == NULL)
{
return;
}
bool *starMatrixTemp, *columnEnd;
int col;
/* cover every column containing a starred zero */
for (col = 0; col < nOfColumns; col++)
{
starMatrixTemp = starMatrix + nOfRows * col;
columnEnd = starMatrixTemp + nOfRows;
while (starMatrixTemp < columnEnd)
{
if (*starMatrixTemp++)
{
coveredColumns[col] = true;
break;
}
}
}
/* move to step 3 */
step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
}
/********************************************************/
void HungarianAlgorithm::step2b(int *assignment, float *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
{
if (assignment == NULL || distMatrix == NULL || starMatrix == NULL || newStarMatrix == NULL || primeMatrix == NULL || coveredColumns == NULL || coveredRows == NULL)
{
return;
}
int col, nOfCoveredColumns;
/* count covered columns */
nOfCoveredColumns = 0;
for (col = 0; col < nOfColumns; col++)
if (coveredColumns[col])
nOfCoveredColumns++;
if (nOfCoveredColumns == minDim)
{
/* algorithm finished */
buildassignmentvector(assignment, starMatrix, nOfRows, nOfColumns);
}
else
{
/* move to step 3 */
step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
}
}
void HungarianAlgorithm::step3(int *assignment, float *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
{
if (assignment == NULL || distMatrix == NULL || starMatrix == NULL || newStarMatrix == NULL || primeMatrix == NULL || coveredColumns == NULL || coveredRows == NULL)
{
return;
}
bool zerosFound;
int row, col, starCol;
zerosFound = true;
while (zerosFound)
{
zerosFound = false;
for (col = 0; col < nOfColumns; col++)
if (!coveredColumns[col])
for (row = 0; row < nOfRows; row++)
if ((!coveredRows[row]) && (fabs(distMatrix[row + nOfRows * col]) < DBL_EPSILON))
{
/* prime zero */
primeMatrix[row + nOfRows * col] = true;
/* find starred zero in current row */
for (starCol = 0; starCol < nOfColumns; starCol++)
if (starMatrix[row + nOfRows * starCol])
break;
if (starCol == nOfColumns) /* no starred zero found */
{
/* move to step 4 */
step4(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim, row, col);
return;
}
else
{
coveredRows[row] = true;
coveredColumns[starCol] = false;
zerosFound = true;
break;
}
}
}
/* move to step 5 */
step5(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
}
void HungarianAlgorithm::step4(int *assignment, float *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim, int row, int col)
{
if (assignment == NULL || distMatrix == NULL || starMatrix == NULL || newStarMatrix == NULL || primeMatrix == NULL || coveredColumns == NULL || coveredRows == NULL)
{
return;
}
int n, starRow, starCol, primeRow, primeCol;
int nOfElements = nOfRows * nOfColumns;
/* generate temporary copy of starMatrix */
for (n = 0; n < nOfElements; n++)
newStarMatrix[n] = starMatrix[n];
/* star current zero */
newStarMatrix[row + nOfRows * col] = true;
/* find starred zero in current column */
starCol = col;
for (starRow = 0; starRow < nOfRows; starRow++)
if (starMatrix[starRow + nOfRows * starCol])
break;
while (starRow < nOfRows)
{
/* unstar the starred zero */
newStarMatrix[starRow + nOfRows * starCol] = false;
/* find primed zero in current row */
primeRow = starRow;
for (primeCol = 0; primeCol < nOfColumns; primeCol++)
if (primeMatrix[primeRow + nOfRows * primeCol])
break;
/* star the primed zero */
newStarMatrix[primeRow + nOfRows * primeCol] = true;
/* find starred zero in current column */
starCol = primeCol;
for (starRow = 0; starRow < nOfRows; starRow++)
if (starMatrix[starRow + nOfRows * starCol])
break;
}
/* use temporary copy as new starMatrix */
/* delete all primes, uncover all rows */
for (n = 0; n < nOfElements; n++)
{
primeMatrix[n] = false;
starMatrix[n] = newStarMatrix[n];
}
for (n = 0; n < nOfRows; n++)
coveredRows[n] = false;
/* move to step 2a */
step2a(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
}
/********************************************************/
void HungarianAlgorithm::step5(int *assignment, float *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
{
if (assignment == NULL || distMatrix == NULL || starMatrix == NULL || newStarMatrix == NULL || primeMatrix == NULL || coveredColumns == NULL || coveredRows == NULL)
{
return;
}
float h, value;
int row, col;
/* find smallest uncovered element h */
h = DBL_MAX;
for (row = 0; row < nOfRows; row++)
if (!coveredRows[row])
for (col = 0; col < nOfColumns; col++)
if (!coveredColumns[col])
{
value = distMatrix[row + nOfRows * col];
if (value < h)
h = value;
}
/* add h to each covered row */
for (row = 0; row < nOfRows; row++)
if (coveredRows[row])
for (col = 0; col < nOfColumns; col++)
distMatrix[row + nOfRows * col] += h;
/* subtract h from each uncovered column */
for (col = 0; col < nOfColumns; col++)
if (!coveredColumns[col])
for (row = 0; row < nOfRows; row++)
distMatrix[row + nOfRows * col] -= h;
/* move to step 3 */
step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
}
- 源文件 — main.cpp
#include "hungarian.h"
using namespace std;
int main()
{
HungarianAlgorithm HungAlgo;
vector<float> A = {1.9, 2.9, 3.9, 4.9};
vector<float> B = {21.1, 3.1, 10.1, 2.1};
vector<vector<float>> costMatrix(A.size(), vector<float>(B.size()));
// 关联矩阵
for (int i = 0; i < A.size(); i++) // current_target
{
for (int j = 0; j < B.size(); j++) // predicted_target
{
double dist = abs(A[i] - B[j]);
costMatrix[i][j] = dist;
cout << costMatrix[i][j] << ",";
}
cout << endl;
}
vector<int> assignment;
vector<float> cost = HungAlgo.Solve(costMatrix, assignment);
for (int i = 0; i < cost.size(); i++)
{
cout << "cost:" << cost[i] << endl;
}
for (int i = 0; i < assignment.size(); i++)
{
cout << "assignment:" << assignment[i] << endl;
}
return 0;
}
- CmakeLists.txt
cmake_minimum_required(VERSION 3.0)
project(data_associate)
add_definitions(-std=c++11)
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_BUILD_TYPE Release)
include_directories("include/")
add_executable(data_associate
src/hungarian.cpp
src/main.cpp
)
用法
具体调用见main.cpp,测试情况如下图