参考博文:
[1]https://blog.csdn.net/u011837761/article/details/52058703(Hungarian Algorithm)
[2]https://blog.csdn.net/dark_scope/article/details/8880547(趣写算法系列之--匈牙利算法)
[3]https://blog.csdn.net/seattledream/article/details/8627800(二分图匹配算法Hungarian Algorithm)
参考书籍:
[1]图论及其应用 徐俊明 中国科学技术大学出版社
代码来源:
[1]https://github.com/ngthanhtin/Muiltiple-Object-Tracking
在研究基于kalman滤波的多物体目标跟踪的过程中,
同一帧的检测点集需要和轨迹集匹配(因为是多物体跟踪)
具体说开就有点跑题了,总之要用到hungarian匹配算法,解决assignment问题
算法步骤的理解可以参考博文[3]中的O(n^4) algorithm explanation,
Step 0)找出每一行中值最小的元素,然后把该行所有元素都减去这一最小值,找出每一列中值最小的元素,然后把该列所有元素都减去这一最小值
Step 1)利用0权重边寻找最大匹配
Step 2)寻找最小点覆盖集,调整边的权重
具体的内容可以看博文和书籍,这里就不做过多的解释了,直接贴个测试demo
这里博文[2]的例子,有点bug,因为是本着完整匹配原则,所以给乌索普2匹配了他不想要的凤姐3
#include
#include
#include
#include
using namespace std;
static const int MAX = 4;
static const double maxC = 10001.f;
vector< std::vector > array_to_matrix(double* m, int rows, int cols);
void hungarian_print_Costmatrix(const vector >& Cost);
void hungarian_print_assignment(vector &assignment);
double c[MAX][MAX];
double Fx[MAX];//Cost矩阵中每一行的最小值
double Fy[MAX];//Cost矩阵中每一列的最小值
int matchX[MAX];//初始化为-1|matchX[i]表示X中的第i个元素和Y[matchX[i]]产生配对关系
int matchY[MAX];//初始化为-1|matchY[i]表示Y中的第i个元素和X[matchY[i]]产生配对关系
int Trace[MAX];//初始化为-1
int m;//tracks.size();
int n;//detections.size();
int k;//k=max(m,n)
int start;
int finish;
double GetC(int i, int j);
void FindAugmentingPath();
void SubX_AddY();
void Enlarge();
void hungarian(vector>& DistMatrix, vector& Assignment);
int main()
{
//double r[3 * 3] =
//{
// 100, 300, 300,
// 300, 100, 300,
// 300, 300, 100
//};
double r[4 * 4] =
{
100, 100, 300, 300,
300, 100, 100, 300,
100, 100, 300, 300,
300, 300, 100, 300
};
vector< vector > Cost = array_to_matrix(r, 4, 4);
hungarian_print_Costmatrix(Cost);
vector assignment;
//Hungarian APS;
//APS.Solve(Cost, assignment);
hungarian(Cost, assignment);
hungarian_print_assignment(assignment);
return 0;
}
void hungarian(vector>& DistMatrix, vector& Assignment)
{
int i, j;
m = DistMatrix.size();
n = DistMatrix[0].size();
k = m > n ? m : n;//k=max(m,n)
//构建c矩阵,
for (i = 0; i < k; i++)
{
for (j = 0; j < k; j++)
{
if (i >= m || j >= n)
{
c[i][j] = maxC;//maxC = 10001.f|超过Cost矩阵范围的元素设为特别大
continue;
}
if (DistMatrix[i][j] == 0)
c[i][j] = maxC;//预测点与最新点的欧式距离为0的开销设为特别大
else
c[i][j] = DistMatrix[i][j];
}
}
for (i = 0; i < MAX; i++)
{
matchX[i] = -1;
matchY[i] = -1;
}
//寻找Cost矩阵每一行的最小值
for (i = 0; i < k; i++)
{
Fx[i] = maxC;
for (j = 0; j < k; j++)
{
if (c[i][j] < Fx[i])
{
Fx[i] = c[i][j];
}
}
}
//Cost矩阵的i行的每一个元素减去Fx[i],然后寻找每一列的最小值
for (j = 0; j < k; j++)
{
Fy[j] = maxC;
for (i = 0; i < k; i++)
{
if (c[i][j] - Fx[i] < Fy[j])
{
Fy[j] = c[i][j] - Fx[i];
}
}
}
for (int x = 0; x < k; x++)
{
start = x;
finish = -1;
do
{
FindAugmentingPath();//利用0权重边寻找最大匹配
if (finish == -1)
SubX_AddY();//寻找最小点覆盖集,调整边的权重
} while (finish == -1);
Enlarge();//在matchX、matchY中更新X和Y的配对关系
}
for (int x = 0; x < m; x++)
{
Assignment.push_back(matchX[x]);
}
for (int x = 0; x < m; x++)
{
for (int y = 0; y < n; y++)
{
DistMatrix[x][y] = c[x][y];
if (c[x][y] == maxC)
DistMatrix[x][y] = 0.f;
}
}
//计算损失函数值
//float cost = 0.f;
//for (int x = 0; x < m; x++)
//{
// int y = matchX[x];
// if (c[x][y] < maxC)
// {
// cost += c[x][y];
// }
//}
}
double GetC(int i, int j)
{
return c[i][j] - Fx[i] - Fy[j];
}
void FindAugmentingPath()
{
queue q;
int i, j;
for (i = 0; i < MAX; i++)
{
Trace[i] = -1;
}
///memset(Trace, -1, sizeof(Trace));
//BFS宽度优先搜索|Running the algorithm BFS to find the opening of the road
q.push(start);
do
{
i = q.front();
q.pop();
for (j = 0; j < k; j++)
{
if (Trace[j] == -1 && GetC(i, j) == 0.0f)
{
Trace[j] = i;
if (matchY[j] == -1)
{
finish = j;
return;
}
q.push(matchY[j]);
}
}
} while (!q.empty());
}
void SubX_AddY()
{
int i, j, t;
double Delta;
set VisitedX, VisitedY;
VisitedX.insert(start);
for (j = 0; j < k; j++)
{
if (Trace[j] != -1)
{
VisitedX.insert(matchY[j]);
VisitedY.insert(j);
}
}
Delta = maxC;
for (i = 0; i < k; i++)
{
if (VisitedX.find(i) != VisitedX.end())
{
for (j = 0; j < k; j++)
{
if ((VisitedY.find(j) == VisitedY.end()) && (GetC(i, j) < Delta))
Delta = GetC(i, j);
}
}
}
for (t = 0; t < k; t++)
{
if (VisitedX.find(t) != VisitedX.end())
Fx[t] = Fx[t] + Delta;
if (VisitedY.find(t) != VisitedY.end())
Fy[t] = Fy[t] - Delta;
}
}
void Enlarge()
{
int x, next;
do
{
x = Trace[finish];
next = matchX[x];
matchX[x] = finish;
matchY[finish] = x;
finish = next;
} while (finish != -1);
}
vector< std::vector > array_to_matrix(double* m, int rows, int cols)
{
int i, j;
std::vector< std::vector > r;
r.resize(rows, std::vector(cols, 0));
for (i = 0; i < rows; i++)
{
for (j = 0; j < cols; j++)
r[i][j] = m[i*cols + j];
}
return r;
}
void hungarian_print_Costmatrix(const vector >& Cost)
{
int i, j;
cout << endl;
for (i = 0; i < Cost.size(); i++)
{
cout << "[";
for (j = 0; j < Cost[0].size(); j++)
{
fprintf(stderr, "%-5.0f ", Cost[i][j]);
}
cout << "]" << endl;
}
cout << endl;
}
void hungarian_print_assignment(vector &assignment)
{
for (int i = 0; i < assignment.size(); i++)
{
cout << assignment[i] << " ";
}//0 1 2
cout << endl;
}