【圖論】匹配問題：Hungarian匈牙利算法研究+代碼實例測試

參考博文：
[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 <queue>
#include <set>
#include <vector>
#include <iostream>
using namespace std;

static const int MAX = 4;
static const double maxC = 10001.f;

vector< std::vector<double> > array_to_matrix(double* m, int rows, int cols);
void hungarian_print_Costmatrix(const vector<vector<double> >& Cost);
void hungarian_print_assignment(vector<int> &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<vector<double>>& DistMatrix, vector<int>& 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<double> > Cost = array_to_matrix(r, 4, 4);

	hungarian_print_Costmatrix(Cost);

	vector<int> assignment;
	//Hungarian APS;
	//APS.Solve(Cost, assignment);
	hungarian(Cost, assignment);

	hungarian_print_assignment(assignment);


	return 0;
}


void hungarian(vector<vector<double>>& DistMatrix, vector<int>& 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<int> 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<int> 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<double> > array_to_matrix(double* m, int rows, int cols)
{
	int i, j;
	std::vector< std::vector<double> > r;
	r.resize(rows, std::vector<double>(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<vector<double> >& 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<int> &assignment)
{

	for (int i = 0; i < assignment.size(); i++)
	{
		cout << assignment[i] << "  ";
	}//0 1 2
	cout << endl;

}