多目标跟踪之匈牙利算法
0 前言
目标跟踪(Object Tracking)是自动驾驶中非常常见的任务,根据跟踪目标数量的不同,目标跟踪可分为:
- 单目标跟踪(Single Object Tracking,SOT)
- 多目标跟踪(Multi-Objects Tracking,MOT)
目标跟踪所要做的是根据传感器量测序列确定真实目标的数量以及每个目标的对应状态(位置、速度、航向等),具体实现可大体分为三部分:
- 状态估计
- 数据关联
- 航迹管理
典型的多目标跟踪系统框架如下所示 [1]:
多目标跟踪系统框架
其中,数据关联(Data Association)是目标跟踪中最具挑战的任务之一。对于单目标跟踪而言,数据关联要解决的是传感器量测能否与跟踪目标关联上;对于多目标跟踪而言,数据关联要解决的是哪个传感器量测能与哪个跟踪目标关联上,意即,跟踪目标与传感器量测的一一对应关系。目前,数据关联已发展出多种理论与算法,包括但不限于 [2]:
- 最近邻(Nearest Neighbor,NN)
- 概率数据关联(Probability Data Association,PDA)
- 联合概率数据关联(Joint Probability Data Association,JPDA)
- 多假设跟踪(Multiple Hypothesis Tracking,MHT)
- 随机有限集(Random Finite Set,RFS)
- 匈牙利算法(Hungarian Algorithm)
多目标跟踪数据关联问题可以转化为有权二分图最小权匹配问题,本文将从图论中的一些基本概念出发,对匈牙利算法的发展脉络、算法流程及其 C++ 代码实现进行详细阐述。
1 基本概念
匈牙利算法本质是图算法,所以在对其进行展开前,我们首先需要认识一些图论中重要的基本概念。
1.1 图
图(Graph,G),是由顶点集合(Vertices,V)和边集合(Edges,E)组成的二元组,表征了不同顶点间的拓扑连接关系,记作:
G=(V,E) \\
根据图中的边是否具有单向性可将图分为有向图(Directed Graph)和无向图(Undirected Graph),根据图中的边是否具有不同的权重可将图分为有权图(Weighted Graph)和无权图(Unweighted Graph)。本文中将讨论的图都是无向有权图(Undirected Weighted Graph),一个典型的无向有权图如下所示 [3]:
典型的无向有权图
1.2 二分图
1.2.1 二分图的概念
二分图(Bipartite Graph)是一种特殊的图,又称二部图。二分图的顶点集可以被划分为两个互不相交的独立子集 U 和 V [4]:
G=(U,V,E) \\
二分图边集合 E 中的每一条边分别连接了顶点子集 U 和 V 中的一个顶点,但 U 和 V 内部的顶点互不相连。一个典型的二分图如下所示 [5]:
典型的二分图
1.2.2 二分图的判定
判定一个图是否是二分图等价于判定该图是否是二色图 [6],即图中所有顶点是否能被染色成两类颜色(需要保证相连顶点不能同色),我们可以通过广度优先搜索(Breadth First Search,BFS)实现 [7]。下面我们给出具体的 C++ 代码示例,示例中使用邻接表来存储图,时间复杂度 \mathcal{O}(V+E),可以使用菜鸟工具 C++ 在线工具运行并验证程序:
#include <iostream>
#include <vector>
#include <queue>
#include <utility>/*** @brief 用于判断输入的图是否是二分图** @param V 图的顶点数量* @param adj 图的邻接表表示* @return bool 图是否是二分图:true - 是,false - 不是*/
bool IsBipartite(int V, std::vector<int> adj[])
{// 存储所有顶点颜色的 vector 容器,初值 -1 表示未染色,0 表示红色,1 表示蓝色std::vector<int> col(V, -1);// 用于 BFS 过程的 FIFO 队列,元素类型是顶点索引及其颜色组成的二元组std::queue<std::pair<int, int>> q;// 遍历所有顶点for (int i = 0; i < V; i++){// 顶点 i 尚未染色if (col[i] == -1){// 将顶点 i 染成红色 0,并将其压入 BFS FIFO 队列col[i] = 0;q.push({ i, 0 });// 处理 BFS FIFO 队列中的各顶点while (!q.empty()){auto p = q.front();q.pop();// 当前顶点int v = p.first;// 当前顶点的颜色int c = p.second;// 遍历当前顶点的所有相连顶点for (int j : adj[v]){// 若相连顶点 j 与当前顶点颜色相同,则输入的图不是二分图if (col[j] == c) return false;// 若相连顶点 j 尚未染色if (col[j] == -1){// 将相连顶点 j 染成与当前顶点颜色相反的颜色,并将其压入 BFS// FIFO 队列col[j] = 1 - c;q.push({ j, col[j] });}}}}}return true;
}int main()
{int V, E;V = 4 , E = 8;// 使用邻接表存储图std::vector<int> adj[V];adj[0] = {1, 3};adj[1] = {0, 2};adj[2] = {1, 3};adj[3] = {0, 2};IsBipartite(V, adj) ? std::cout << "输入的图是二分图" << std::endl: std::cout << "输入的图不是二分图" << std::endl;return 0;
}
二分图的判定与本文中要讲解的匈牙利算法并不直接相关,这里只作为二分图概念的扩展内容。
1.3 匹配
给定图 G=(V,E),它的一个匹配(Matching)M 表示包含于边集合 E 的一个子集,即 M 由 E 中的若干条边组成:
M \subseteq E \\
匹配中的任意两条边之间没有公共顶点。以上面的二分图为例,下图中的两条红色边构成了它的一个匹配 [8]:
二分图的一个匹配
匹配 M 中的边被称为匹配边,上图中的两条红色边都是匹配边;边集合 E 中不属于匹配 M 的边被称为未匹配边,上图中的灰色边都是未匹配边。
匹配边的端点被称为匹配点,上图中顶点 u_4、u_5、v_1、v_3 都是匹配点;顶点集合 U 和 V 中不是匹配边端点的其它顶点被称为未匹配点,上图中顶点 u_1、u_2、u_3、v_2、v_4 都是未匹配点。
1.4 最大匹配
最大匹配(Maximum-Cardinality Matching)表示图的所有匹配中边数最多的那些匹配。最大匹配不唯一,仍以上面的二分图为例,它的最大匹配的边数为 4,下面的两个匹配都是它的最大匹配 [8]:
图的最大匹配不唯一
1.5 最大权匹配和最小权匹配
最大权匹配(Maximum-Weight Matching)表示的是有权图的所有匹配中边的权重之和最大的那些匹配,最小权匹配(Minimum-Weight Matching)表示的是有权图的所有匹配中边的权重之和最小的那些匹配。
仍以上面的二分图为例,我们为它所有的边赋上权重,将其转化为有权二分图 [9]:
有权二分图
对于上面的有权二分图,我们将任意边上的权重记作 w,任意匹配记作 M,匹配 M 包含的所有匹配边的权重和记作函数 f(M),则有权二分图的最大权匹配(Maximum-Weight Bipartite Matching)问题和有权二分图的最小权匹配(Minimum-Weight Bipartite Matching)问题可以用如下数学语言进行描述:
\begin{cases} f(M)=\displaystyle\sum_{(u,v) \in M}{w_{u,v}} \\ \max / \min{f(M)} \end{cases} \\
多目标跟踪数据关联问题可以转化为有权二分图最小权匹配问题,跟踪过程中的航迹序列和量测序列可以分别看作是上面这个有权二分图中的顶点集合 U 和 V,边的权重可以看作是航迹和量测间通过某种方式计算得到的匹配距离(例如欧式距离),这个匹配距离我们称之为代价(Cost),所有的匹配距离构成了代价矩阵(Cost Matrix),我们需要做的是,找到航迹和量测间的匹配关系使得总的匹配距离最小(代价最低)。
1.6 交替路
给定图 G=(V,E) 和它的一个匹配 M,交替路(Alternating Path)描述的是图中的这样一条路径:从图中的某个未匹配点出发,交替经过未匹配边和匹配边形成的路径。下面的二分图中,路径 u_3 \color{gray}{\rightarrow} v_1 \color{red}{\rightarrow} u_1 \color{gray}{\rightarrow} v_3 \color{red}{\rightarrow} u_2 就是一条交替路:
二分图中的一条交替路
1.7 增广路
1.7.1 增广路的概念
增广路(Augmenting Path)是一条特殊的交替路,增广路从图中的某个未匹配点起始,交替经过未匹配边和匹配边,并终止于不同于起始点的另一个未匹配点。下面的二分图中,路径 u_1 \color{gray}{\rightarrow} v_1 \color{red}{\rightarrow} u_5 \color{gray}{\rightarrow} v_4 就是一条增广路:
二分图中的一条增广路
1.7.2 增广路的性质
给定图 G=(V,E) 和它的一个匹配 M 以及增广路 P,并将 P 上所有的边记作集合 E_P,则有如下三个非常重要的性质:
- Berge 定理:对于给定的图 G 和它的一个匹配 M,M 是 G 的最大匹配的充要条件是 G 中不存在匹配 M 的增广路 [10];
- E_P 中边的数量一定为奇数,且增广路 P 的第奇数条边不属于匹配 M,第偶数条边属于匹配 M,这意味着,E_P 中的未匹配边数量一定比匹配边数量多 1;
- 通过性质 1 不难发现,通过将 E_P 中的未匹配边取反变成匹配边,匹配边取反变成未匹配边,就可以多出 1 条匹配边,取反得到的新匹配边和 M 中不属于 E_P 的剩余边可以构成一个更大的匹配 M’。在集合论中,M’ 被称作 M 和 E_P 的对称差(Symmetric Difference)[11],记作 M\ominus E_P:
M\ominus E_P=(M\cup E_P) - (M\cap E_P) \\
结合性质 1 和性质 3 我们不难发现,对于一个给定的二分图 G=(U,V,E) 和初始为空的匹配 M,我们只要反复搜索增广路就能逐渐扩展匹配的大小,最终当我们找不到增广路时就得到了一个最大匹配,下图中的示例很直观地展示了这个过程:
反复搜索增广路寻找二分图最大匹配
最后得到的 M=\{e_{1,3},e_{3,1},e_{4,2},e_{5,4}\} 就是输入二分图 G 的一个最大匹配。很多资料中使用“婚配问题”为例来讲解上述过程 [12],这里我们不再赘述,一句话总结:有机会上,没机会创造机会也要上。
2 匈牙利算法的发展脉络
匈牙利算法是全局最近邻(Global Nearest Neighbor,GNN)数据关联思想的一种具体实现,其最早用于求解经济学领域中的任务分配问题,后来发展成为图论领域中求解有权二分图最小权匹配问题的一般性算法 [13] [14] [15]:
- 1955 年,Harold Kuhn 在匈牙利数学家 Dénes Kőnig 和 Jenő Egerváry 的研究基础(系数矩阵中独立 0 元素的最多个数等于能覆盖所有 0 元素的最少直线数)上提出了匈牙利方法(Hungarian Method)以求解任务分配问题 [16]。
- 1956 年,Harold Kuhn 通过图论重新建模了使用匈牙利方法求解任务分配问题的过程,并将任务分配问题转化为有权二分图最小权匹配问题,同时比较了匈牙利方法的几个变种 [17]。
- 1956 年,Merrill M. Flood 给出了匈牙利方法的算法实现步骤(划线法:划最少的线覆盖所有的 0),这是原始的匈牙利算法,网络上很多讲解匈牙利算法的资料中都使用了这里的步骤 [18]。
- 1957 年,James Munkres 回顾并改进了匈牙利方法,针对划线法首次引入“标星 0(starred zeros)”和“标撇 0(primed zeros)”的概念,自此该算法常被称为 Kuhn–Munkres 算法、KM 算法或 Munkres 分配算法 [19]。Kuhn–Munkres 算法只适用于分配矩阵(关联矩阵)为方阵的情形(即 |U|=|V|),最坏时间复杂度 \mathcal{O}(n^4)。
- 1957 年,法国数学家 Claude Berge 证明,对于给定的图 G 和它的一个匹配 M,M 是最大匹配的充要条件是图中不存在匹配 M 的增广路——Berge 定理 [10],该定理奠定了后来各种通过搜索增广路求解图最大匹配问题的基础。
- 1965 年,组合优化大师 Jack Edmonds [20] 提出“带花树开花”算法(Blossom Algorithm)[21],用于求解无权一般图最大匹配问题 [22] 和有权一般图最大权匹配问题 [23],算法基于 Berge 定理通过反复搜索增广路来逐渐扩展部分匹配,直至找不到增广路。
- 1971 年,François Bourgeois 等人将 Kuhn–Munkres 算法扩展到分配矩阵为非方阵的情形 [24]。
- 1973 年,John Hopcroft 和 Richard Karp 提出 Hopcroft–Karp 算法[25],用于求解无权二分图最大匹配问题,类似 Blossom 算法,Hopcroft–Karp 算法同样是通过反复搜索增广路的方式来逐渐扩展部分匹配,不同的是该算法每次迭代可以得到多条增广路,因而算法效率更高 [26]。
- 1978 年,JIN KUE WONG 将 Kuhn–Munkres 算法的最坏时间复杂度优化到 \mathcal{O}(n^3) [27]。
通过网络搜索匈牙利算法相关资料时,我们经常会看到一些有误导性的说法,例如“匈牙利算法用于求解无权二分图的最大匹配问题,1965 年由匈牙利数学家 Edmonds 提出,故得名匈牙利算法;KM 算法用于求解有权二分图的最小权匹配问题” [28],类似说法很不严谨且容易让人产生混淆。根据匈牙利算法的发展历程,我们给出下面的三个事实对这些说法予以反驳:
- 现在常说的以及文献中常提到的匈牙利算法和 Kuhn-Munkres 算法指的是同一个东西,求解的都是有权二分图最小权匹配问题;
- 匈牙利算法起源于 1955 年 Kuhn 提出的匈牙利方法(Hungarian Method),1956 年 Merrill M. Flood 给出了匈牙利方法的算法实现步骤,1957 年 Munkres 针对该方法做了改进,后来大家习惯叫匈牙利算法或 Kuhn-Munkres 算法;
- Edmonds 1965 年提出的是用于求解一般图最大匹配和最大权匹配的“带花树开花”算法(Blossoms Algorithm),算法基于 Berge 定理通过反复搜索增广路的方法来逐渐扩展部分匹配,后来这个方法被引入了我们现在经常看到的各种版本的匈牙利算法代码实现。
自始至终,反复搜索增广路扩展部分匹配都是一种基于 Berge 定理的用于求解图最大匹配问题的方法,而非匈牙利算法本身。
除匈牙利算法外,Jonker-Volgenant 算法 [29]、Auction 算法 [30] 等也可用于求解分配问题,这三个算法也是 Matlab GNN 跟踪器 trackerGNN [31] 可选的分配算法,这里我们不做展开。
3 原始匈牙利算法
3.1 算法流程
上文中我们已经提到,匈牙利算法的原始流程由 Merrill M. Flood 于 1956 年给出。假设代价矩阵为 n\times n 的方阵(航迹、量测数量相同),则原始流程步骤如下图所示:
匈牙利算法原始流程
3.2 操作示例
假设我们有下面的 4\times 4 航迹量测代价矩阵,矩阵中的元素表示航迹量测间的匹配距离(实际项目中匹配距离不会这么夸张,这里只为演示):
航迹量测代价矩阵示例
我们将上述的匈牙利算法原始流程步骤应用到该代价矩阵上,则求解其最小权匹配的流程如下图所示:
匈牙利算法原始流程示例
最后需要划 4 条线才能覆盖住矩阵中所有的 0 元素,迭代终止,根据矩阵中 0 元素的位置我们很容易得到航迹量测最终的匹配关系:track1→object3,track2→object2,track3→object1,track4→object4。这个匹配满足航迹量测构成的二分图上的匹配边总权重最小,即总的匹配距离最小,代价最低。
二分图最大权匹配问题可以与最小权匹配问题相互转化。假设上述代价矩阵中的各元素表示航迹量测间的相似度得分,我们想要找到航迹量测的某个匹配关系使得总的相似度得分最高,这显然是一个二分图最大权匹配问题。首先,我们找到原代价矩阵中的最大代价,并将原代价矩阵中每个元素替换为最大代价与对应元素的差值以得到新的代价矩阵,这样代价矩阵中元素间的相对大小关系就发生了反转:
将原代价矩阵进行转化以求解二分图最大权匹配问题
然后,在新的代价矩阵上执行前文中的二分图最小权匹配求解流程即可,得到的匹配结果即为原代价矩阵的最大权匹配。
4 改进的 KM 算法
4.1 算法流程
上述匈牙利算法原始流程虽然直观易懂,但在代码实现上并不容易,其中的一个关键难点是:如何用尽量少的线覆盖代价矩阵中所有的 0?
前文中已经提到,1957 年 James Munkres 引入了“标星 0(starred zeros)”和“标撇 0(primed zeros)”的概念以改进匈牙利算法原始流程中的划线法,在算法执行过程中会选择性地对代价矩阵中产生的 0 元素标记星号(*)或标记撇号(’)来辅助搜索增广路,标星 0 表示增广路中的匹配边,标撇 0 表示增广路中的未匹配边。后来我们习惯将 Munkres 提出的方法称为 Kuhn–Munkres 算法、KM 算法或 Munkres 分配算法。
KM 算法本身只适用于代价矩阵为方阵的情形,本文将介绍一种 KM 算法的改进版本 [32],这里我们姑且称之为 MKM(Modified Kuhn–Munkres)算法。MKM 算法已被大量运用于工业实践,例如 Matlab 中的 Hungarian Algorithm for Linear Assignment Problems [33] 、Python 中的 munkres 包 [34] 以及百度 Apollo 自动驾驶框架中的 Hungarian Optimizer [35] [36]。MKM 通过“膨胀补 0”的方式来处理代价矩阵为非方阵的情形,例如,如果代价矩阵维度为 4\times 3,则 MKM 会为代价矩阵额外补充一个 0 元素列使之成为方阵。Apollo Hungarian Optimizer 中应用的 MKM 算法流程如下图所示:
改进的 KM 算法流程
可见,MKM 中也使用了反复搜索增广路的方式来逐步扩展部分匹配。此外,MKM 的步骤 6 与匈牙利算法原始流程中的步骤 4 是等价的。
4.2 操作示例
为演示 MKM 的操作流程,假设我们的航迹量测代价矩阵是个 4\times 3 的非方阵(4 个航迹 3 个量测,意味着传感器出现了漏检):
航迹量测代价矩阵为非方阵
我们将上述的 MKM 算法流程步骤应用到该代价矩阵上,则求解其最小权匹配的流程如下图所示:
最终根据矩阵中标星 0 的位置我们就可以得到航迹量测的匹配关系:track2→object2,track3→object3,track4→object1,至于 track1,没有找到与它匹配的量测。这个匹配满足航迹量测构成的二分图上的匹配边总权重最小,即总的匹配距离最小,代价最低。
对于代价矩阵为非方阵的二分图最大权匹配问题,我们同样可以像前文所述的那样首先对其进行转化,然后再实施 MKM 算法流程求解最小权匹配问题即可。
4.3 代码实现
这里我们抽取了 Apollo Hungarian Optimizer 的关键代码并整理成了一个 C++ demo 示例,主要的算法代码是一个名为 HungarianOptimizer
的模板类,该模板类具备如下主要特性:
- 适配代价矩阵为非方阵的情形
- 可求解最小权匹配问题
- 可求解最大权匹配问题
- 可输出部分匹配
可以点击这里查看 GitHub 项目主页,编译工程后运行可执行程序可以得到下面的终端打印结果:
The assignments are:(1, 1)(2, 2)(3, 0)
这里的结果是代价矩阵行列索引(从 0 开始)间的匹配关系,将其映射成 track id 和 object id 后会发现与我们上文中的手动演算结果一致。
需要说明的是,Apollo 在优化给定的航迹量测代价矩阵前,会首先将其切割为多个连通子图,然后对每个连通子图分别实施 MKM 流程,以达到提升效率的目的。这里我们只关心 MKM 的核心算法流程,所以省略了这一步骤,直接对给定的代价矩阵进行最小权匹配求解。
程序入口:main
示例工程的主入口如下所示,
int main() {std::vector<std::vector<float>> association_mat = {{82.0f, 83.0f, 69.0f},{77.0f, 37.0f, 49.0f},{11.0f, 69.0f, 5.0f},{8.0f, 9.0f, 98.0f}};HungarianOptimizer<float> optimizer;std::vector<std::pair<size_t, size_t>> assignments;UpdateCosts(association_mat, optimizer.costs());// entry of hungarian optimizer minimum-weighted matchingoptimizer.Minimize(&assignments);PrintAssignments(assignments);return 0;
}
main
函数主要做了四件事:
- 设定代价矩阵
- 更新 HungarianOptimizer 内部代价
- 最小权匹配问题求解
- 打印航迹量测关联结果
MKM 最小权匹配入口:Minimize
Minimize
函数是 HungarianOptimizer
的最小权匹配问题求解入口:
/* Find an assignment which minimizes the overall costs.* Return an array of pairs of integers. Each pair (i, j) corresponds to* assigning agent i to task j. */
template <typename T>
void HungarianOptimizer<T>::Minimize(std::vector<std::pair<size_t, size_t>>* assignments) {OptimizationInit();DoMunkres();FindAssignments(assignments);OptimizationClear();
}
Minimize
函数主要做了四件事:
- 算法初始化
OptimizationInit
:对应我们上文中提到的 MKM 初始化步 - 运行算法主流程
DoMunkres
:对应我们上文中提到的 MKM 步骤 1 到步骤 6 - 生成匹配结果
FindAssignments
- 清除初始化标志
OptimizationClear
需要说明的是,HungarianOptimizer
同样可以用于求解最大权匹配问题,对原代价矩阵直接调用其 Maximize
函数即可。
下面我们对 OptimizationInit
和 DoMunkres
的代码实现进行简要讲解并给出必要的中文注释。
4.3.1 算法初始化
OptimizationInit
函数对应了我们上文中提到的 MKM 初始化步,其主要完成了代价矩阵“膨胀补 0”、代价矩阵标记初始化、增广路初始化等工作。
template <typename T>
void HungarianOptimizer<T>::OptimizationInit() {// 已完成初始化,返回if (optimization_initialized_) {return;}width_ = costs_.width();if (width_ > 0) {height_ = costs_.height();} else {height_ = 0;}matrix_size_ = std::max(height_, width_);max_cost_ = 0;/* generate the expanded cost matrix by adding extra 0s in order to make a* square matrix. Meanwhile, find the max cost in the matrix. It may be used* later, if we want to maximizing rather than minimizing the overall costs.*/// 代价矩阵膨胀补 0,确保代价矩阵为方阵costs_.Resize(matrix_size_, matrix_size_);for (size_t row = 0; row < matrix_size_; ++row) {for (size_t col = 0; col < matrix_size_; ++col) {if ((row >= height_) || (col >= width_)) {costs_(row, col) = 0;} else {max_cost_ = std::max(max_cost_, costs_(row, col));}}}/* initially, none of the cells of the matrix are marked. */// 代价矩阵标记初始化marks_.Resize(matrix_size_, matrix_size_);for (size_t row = 0; row < matrix_size_; ++row) {for (size_t col = 0; col < matrix_size_; ++col) {marks_(row, col) = Mark::NONE;}}stars_in_col_.assign(matrix_size_, 0);rows_covered_.assign(matrix_size_, false);cols_covered_.assign(matrix_size_, false);// 增广路初始化assignments_.resize(matrix_size_ * 2);// 初始化化标志置 trueoptimization_initialized_ = true;
}
4.3.2 算法主流程
主流程入口:DoMunkres
DoMunkres
函数对应了我们上文中提到的 MKM 步骤 1 到步骤 6,步骤循环迭代,求解成功或达到指定迭代次数阈值结束循环。同时,若达到指定迭代次数阈值,则取消最终的代价矩阵中存在不止一个标星 0 的行里的所有星号标记,以实现部分匹配输出。
/* Run the Munkres algorithm */
template <typename T>
void HungarianOptimizer<T>::DoMunkres() {int max_num_iter = 1000;int num_iter = 0;// fn_state_ 代表算法当前状态,即执行到了哪一步fn_state_ = std::bind(&HungarianOptimizer::ReduceRows, this);// 从步骤 1 开始循环迭代,直至求解成功或达到指定迭代次数阈值while (fn_state_ != nullptr && num_iter < max_num_iter) {fn_state_();++num_iter;}/* 若达到指定迭代次数阈值,则取消最终的代价矩阵中存在不止一个标星 0 的行里的所有星号* 标记,实现部分匹配输出 */if (num_iter >= max_num_iter) {CheckStar();}
步骤 1:ReduceRows
代价矩阵的每一行元素减去该行元素中的最小值,然后执行步骤 2。
/* Step 1:* For each row of the matrix, find the smallest element and substract it* from every element in its row. Then, go to Step 2. */
template <typename T>
void HungarianOptimizer<T>::ReduceRows() {for (size_t row = 0; row < matrix_size_; ++row) {T min_cost = costs_(row, 0);for (size_t col = 1; col < matrix_size_; ++col) {min_cost = std::min(min_cost, costs_(row, col));}for (size_t col = 0; col < matrix_size_; ++col) {costs_(row, col) -= min_cost;}}// 执行步骤 2fn_state_ = std::bind(&HungarianOptimizer::StarZeroes, this);
}
步骤 2:StarZeroes
对代价矩阵中所在行和列中都没有标星 0 的 0 元素标记星号 *,然后执行步骤 3。
/* Step 2:* Find a zero Z in the matrix. If there is no starred zero in its row* or column, star Z. Repeat for every element in the matrix. Then, go to* Step3. */
template <typename T>
void HungarianOptimizer<T>::StarZeroes() {/* since no rows or columns are covered on entry to this step, we use the* covers as a quick way of making which rows & columns have stars in them */for (size_t row = 0; row < matrix_size_; ++row) {if (RowCovered(row)) {continue;}for (size_t col = 0; col < matrix_size_; ++col) {if (ColCovered(col)) {continue;}if (costs_(row, col) == 0) {Star(row, col);CoverRow(row);CoverCol(col);break;}}}ClearCovers();// 执行步骤 3fn_state_ = std::bind(&HungarianOptimizer::CoverStarredZeroes, this);
}
步骤 3:CoverStarredZeroes
覆盖代价矩阵中所有含有标星 0 的列,若所有的列都已被覆盖则结束迭代,否则执行步骤 4。
/* Step 3:* Cover each column containing a starred zero. If all columns are covered,* the starred zeros describe a complete set of unique assignments. In this* case, terminate the algorithm. Otherwise, go to Step 4. */
template <typename T>
void HungarianOptimizer<T>::CoverStarredZeroes() {size_t num_covered = 0;// 统计被覆盖的列的数量for (size_t col = 0; col < matrix_size_; ++col) {if (ColContainsStar(col)) {CoverCol(col);num_covered++;}}// 若所有的列都已被覆盖则结束迭代if (num_covered >= matrix_size_) {fn_state_ = nullptr;return;}// 执行步骤 4fn_state_ = std::bind(&HungarianOptimizer::PrimeZeroes, this);
}
步骤 4:PrimeZeroes
查找当前代价矩阵中是否存在未被覆盖的 0 元素:
存在:对未被覆盖的 0 元素标记撇号 ’,并判断标撇 0 所在的行是否含有标星 0
含有:覆盖标撇 0 所在的行,取消覆盖标撇 0 所在行里的标星 0 所在的列,继续查找当前代价矩阵中是否存在未被覆盖的 0 元素
不含有:将该标撇 0 置为增广路的首条边,执行步骤 5
不存在:执行步骤 6
/* Step 4:* Find a noncovered zero and prime it. If there is no starred zero in the* row containing this primed zero, go to Step 5. Otherwise, cover this row* and uncover the column containing the starred zero. Continue in this manner* until there are no uncovered zeros left, then go to Step 6. */
template <typename T>
void HungarianOptimizer<T>::PrimeZeroes() {// this loop is guaranteed to terminate in at most matrix_size_ iterations,// as FindZero() returns a location only if there is at least one uncovered// zero in the matrix. Each iteration, either one row is covered or the// loop terminates. Since there are matrix_size_ rows, after that many// iterations there are no uncovered cells and hence no uncovered zeroes,// so the loop terminates.for (;;) {size_t zero_row = 0;size_t zero_col = 0;// 若当前的代价矩阵不存在未被覆盖的 0if (!FindZero(&zero_row, &zero_col)) {// No uncovered zeroes.// 执行步骤 6fn_state_ = std::bind(&HungarianOptimizer::AugmentPath, this);return;}// 对未被覆盖的 0 标记撇号Prime(zero_row, zero_col);int star_col = FindStarInRow(zero_row);// 若标撇 0 所在的行含有标星 0if (star_col != kHungarianOptimizerColNotFound) {// 覆盖标撇 0 所在的行CoverRow(zero_row);// 取消覆盖标撇 0 所在行里的标星 0 所在的列UncoverCol(star_col);} else { // 若标撇 0 所在的行不含有标星 0std::pair<size_t, size_t> first_assignment =std::make_pair(zero_row, zero_col);// 将该标撇 0 置为增广路的首条边assignments_[0] = first_assignment;// 执行步骤 5fn_state_ = std::bind(&HungarianOptimizer::MakeAugmentingPath, this);return;}}
}
步骤 5:MakeAugmentingPath
首先,构建增广路,从增广路首条边开始,若标撇 0 所在列存在标星 0 则将该标星 0 置为增广路当前边的下一条边,若标星 0 所在行存在标撇 0 则将该标撇 0 置为增广路当前边的下一条边,直至找不到新的标星 0;然后,执行增广操作,取消增广路中标星 0 的标记,并将增广路中的标撇 0 重新标记为标星 0;最后,取消代价矩阵中的所有行列覆盖,清除代价矩阵中所有标撇 0 的标记,返回步骤 3。
/* Step 5:* Construct a series of alternating primed and starred zeros as follows.* Let Z0 represent the uncovered primed zero found in Step 4. Let Z1 denote* the starred zero in the column of Z0 (if any). Let Z2 denote the primed* zero in the row of Z1 (there will always be one). Continue until the* series terminates at a primed zero that has no starred zero in its column.* unstar each starred zero of the series, star each primed zero of the* series, erase all primes and uncover every line in the matrix. Return to* Step 3. */
template <typename T>
void HungarianOptimizer<T>::MakeAugmentingPath() {bool done = false;size_t count = 0;/* note: this loop is guaranteed to terminate within matrix_size_ iterations* because:* 1) on entry to this step, there is at least 1 column with no starred zero* (otherwise we would have terminated the algorithm already.)* 2) each row containing a star also contains exactly one primed zero.* 4) each column contains at most one starred zero.** Since the path_ we construct visits primed and starred zeroes alternately,* and terminates if we reach a primed zero in a column with no star, our* path_ must either contain matrix_size_ or fewer stars (in which case the* loop iterates fewer than matrix_size_ times), or it contains more. In* that case, because (1) implies that there are fewer than matrix_size_* stars, we must have visited at least one star more than once. Consider* the first such star that we visit more than once; it must have been reached* immediately after visiting a prime in the same row. By (2), this prime* is unique and so must have also been visited more than once.* Therefore, that prime must be in the same column as a star that has been* visited more than once, contradicting the assumption that we chose the* first multiply visited star, or it must be in the same column as more* than one star, contradicting (3). Therefore, we never visit any star* more than once and the loop terminates within matrix_size_ iterations. */// 构造增广路while (!done) {/* first construct the alternating path... */// 标撇 0 所在的列查找标星 0int row = FindStarInCol(assignments_[count].second);// 标撇 0 所在的列含有标星 0if (row != kHungarianOptimizerRowNotFound) {// 增广路游标加 1count++;// 找到的标星 0 置为标撇 0 的下一条边assignments_[count].first = row;assignments_[count].second = assignments_[count - 1].second;} else { // 标撇 0 所在的列不含有标星 0,增广路构造终止done = true;}// 若增广路构造尚未终止if (!done) {// 标星 0 所在的行查找标撇 0int col = FindPrimeInRow(assignments_[count].first);// 增广路游标加 1count++;// 找到的标撇 0 置为标星 0 的下一条边assignments_[count].first = assignments_[count - 1].first;assignments_[count].second = col;}}/* then, modify it. */// 增广操作:遍历增广路中的每一个 0 元素(代表一条匹配边或未匹配边)for (size_t i = 0; i <= count; ++i) {size_t row = assignments_[i].first;size_t col = assignments_[i].second;// 若当前 0 标记了星号(增广路中的匹配边)if (IsStarred(row, col)) {// 取消当前标星 0 的星号标记(变成全局未匹配边)Unstar(row, col);} else { // 若当前 0 标记了撇号(增广路中的未匹配边)// 将当前标撇 0 重新标记为标星 0(变成匹配边)Star(row, col);}}// 取消代价矩阵中的所有行列覆盖ClearCovers();// 清除代价矩阵中所有标撇 0 的标记ClearPrimes();// 返回步骤 3fn_state_ = std::bind(&HungarianOptimizer::CoverStarredZeroes, this);
}
步骤 6:AugmentPath
查找未被覆盖元素中的最小值 minval,被覆盖行中的每一个元素加上 minval,未被覆盖列中的每一个元素减去 minval。
/* Step 6:* Add the smallest uncovered value in the matrix to every element of each* covered row, and subtract it from every element of each uncovered column.* Return to Step 4 without altering any stars, primes, or covered lines. */
template <typename T>
void HungarianOptimizer<T>::AugmentPath() {// 查找未被覆盖元素中的最小值 minvalT minval = FindSmallestUncovered();// 被覆盖行中的每一个元素加上 minvalfor (size_t row = 0; row < matrix_size_; ++row) {if (RowCovered(row)) {for (size_t c = 0; c < matrix_size_; ++c) {costs_(row, c) += minval;}}}// 未被覆盖列中的每一个元素减去 minvalfor (size_t col = 0; col < matrix_size_; ++col) {if (!ColCovered(col)) {for (size_t r = 0; r < matrix_size_; ++r) {costs_(r, col) -= minval;}}}// 返回步骤 4fn_state_ = std::bind(&HungarianOptimizer::PrimeZeroes, this);
}
4.3.3 生成匹配结果
完成最小权匹配问题求解后,需要遍历最终的代价矩阵中的所有元素,根据矩阵中标星 0 的行索引和列索引即可生成航迹量测匹配关系。
/* Convert the final costs matrix into a set of assignments of agents to tasks.* Return an array of pairs of integers, the same as the return values of* Minimize() and Maximize() */
template <typename T>
void HungarianOptimizer<T>::FindAssignments(std::vector<std::pair<size_t, size_t>>* assignments) {assignments->clear();for (size_t row = 0; row < height_; ++row) {for (size_t col = 0; col < width_; ++col) {if (IsStarred(row, col)) {assignments->push_back(std::make_pair(row, col));break;}}}
}
4.3.4 清除初始化标志
实际工程开发中,当前帧信息处理完成后,需要将初始化标志置 false
,以使得处理下一帧信息时重新执行初始化操作,因为不同帧间的代价矩阵一般来说是不同的。
template <typename T>
void HungarianOptimizer<T>::OptimizationClear() {optimization_initialized_ = false;
}
至此,Apollo Hungarian Optimizer 核心代码分析结束。
5 后记
首先,本文给出了图论中的一些基本概念并由此引出了二分图最大权匹配问题与最小权匹配问题;然后,通过大量原始参考文献厘清了匈牙利算法的发展脉络,并驳斥了一些具有误导性的说法;其次,给出了匈牙利算法的原始流程与操作示例;最后,给出了改进的 KM 算法流程以及一个非方阵代价矩阵的操作示例,并基于 Apollo Hungarian Optimizer 给出了具体的代码实现。
参考
- Introduction to Multiple Target Tracking
- Introduction to Assignment Methods in Tracking Systems
- 图的基本概念和数据结构 Graph Basics and Data Structures
- Bipartite Graph
- 二部图及其判定算法 Bipartite Graphs
- Graph coloring
- Check whether a given graph is Bipartite or not
- 无权二部图中的最大匹配 Maximum-Cardinality Bipartite Matching (MCBM)
- 有权二部图中的最大匹配 Maximum-Weight Bipartite Matching
- Berge’s theorem
- Symmetric difference
- 趣写算法系列之–匈牙利算法
- Hungarian algorithm
- Design and Analysis of Modern Tracking Systems
- Efficient Algorithms for Finding Maximum Matching in Graphs
- The Hungarian method for the assignment problem
- VARIANTS OF THE HUNGARIAN METHOD FOR ASSIGNMENT PROBLEMS
- THE TRAVELING-SALESMAN PROBLEM
- Algorithms for the Assignment and Transportation Problems
- Jack Edmonds
- Blossom Algorithm
- PATHS, TREES, AND FLOWERS
- Maximum Matching and a Polyhedron With 0,1-Vertices
- An Extension of the Munkres Algorithm for the Assignment Problem to Rectangular Matrices
- Hopcroft–Karp algorithm
- An n5/2 algorithm for maximum matchings in bipartite graphs
- A NEW IMPLEMENTATION OF AN ALGORITHM FOR THE OPTIMAL ASSIGNMENT PROBLEM: AN IMPROVED VERSION OF MUNKRES’ ALGORITHM
- 带你入门多目标跟踪(三)匈牙利算法 & KM 算法
- A Shortest Augmenting Path Algorithm for Dense and Sparse Linear Assignment Problems
- The Auction Algorithm for Assignment and Other Network Flow Problem
- Matlab GNN 跟踪器 trackerGNN
- Munkres’ Assignment Algorithm-Modified for Rectangular Matrices
- Hungarian Algorithm for Linear Assignment Problems
- munkres — Munkres implementation for Python
- Apollo HM 对象跟踪
- Apollo Hungarian Optimizer
- Inside the Hungarian Algorithm: A Self-Driving Car Example
- Apollo perception 源码阅读 | fusion 之匈牙利算法
- 二分图最大匹配问题与匈牙利算法的核心思想
- COS 423 Lecture 19 Graph Matching - cs.Princeton
- 二部图最大匹配——新数据结构 Augmenting graph 与 Hopcroft-Karp 算法的复杂度证明
- 匈牙利算法简介
- The Assignment Problem (Using Hungarian Algorithm)
- 指派问题:匈牙利算法
- 匈牙利算法 Hungarian Algorithm
- 目标跟踪初探(DeepSORT)
- The Hungarian algorithm: An example
多目标跟踪之匈牙利算法相关推荐
- 多目标跟踪卡尔曼滤波和匈牙利算法
多目标跟踪关联匹配算法(匈牙利算法和KM算法原理讲解和代码实现) 目录 多目标跟踪关联匹配算法 多目标跟踪关联匹配算法(匈牙利算法和KM算法原理讲解和代码实现) 0.多目标跟踪算法流程 1.卡尔曼滤波 ...
- 带你入门多目标跟踪(三)匈牙利算法KM算法
匈牙利算法(Hungarian Algorithm)与KM算法(Kuhn-Munkres Algorithm)是做多目标跟踪的小伙伴很容易在论文中见到的两种算法.他们都是用来解决多目标跟踪中的数据关联 ...
- 智慧交通day02-车流量检测实现07:匈牙利算法
匈牙利算法(Hungarian Algorithm)与KM算法(Kuhn-Munkres Algorithm)是用来解决多目标跟踪中的数据关联问题,匈牙利算法与KM算法都是为了求解二分图的最大匹配问题 ...
- 图解匈牙利算法(含python代码)
文章目录 分配问题 匈牙利算法 算法步骤 算法实现 python版本 C++版本 分配问题 分配问题/指派问题(Assignment Problem)作为线性规划问题的一个特例,在运筹学研究中占有重要 ...
- 车流量检测实现:多目标追踪、卡尔曼滤波器、匈牙利算法、SORT/DeepSORT、yoloV3、虚拟线圈法、交并比IOU计算
日萌社 人工智能AI:Keras PyTorch MXNet TensorFlow PaddlePaddle 深度学习实战(不定时更新) CNN:RCNN.SPPNet.Fast RCNN.Faste ...
- 匈牙利算法解决加权二分图问题
匈牙利方法是一种组合优化算法,它在多项式时间内解决了赋值问题,广泛应用于多目标跟踪的关联问题中. 图1:(a)二分图,(b)边权重矩阵,(c)边成本的替代表示形式 动机:分配问题 假设有 nnn 辆卡 ...
- 目标跟踪中的卡尔曼滤波和匈牙利算法解读。
先解读Sort算法:Simple online and realtime tracking 论文地址 https://arxiv.org/abs/1602.00763 代码地址 https://git ...
- 二分图匹配匈牙利算法DFS实现
1 /*==================================================*\ 2 | 二分图匹配(匈牙利算法DFS 实现) 3 | INIT: g[][]邻接矩阵; ...
- 解题报告:luogu P2423 [HEOI2012]朋友圈【最大团转最大点独立集(匈牙利算法+时间戳优化)】
图的最大团:"任意两点之间都有一条边相连"的子图被称为无向图的团,点数最多的团为图的最大团 朋友圈中任意两个点之间都有关系,既是图中的团. 答案就是图中的最大团. 我们如果把B国的 ...
最新文章
- undistortPoints()函数用法总结
- 【原创】【专栏】《Linux设备驱动程序》--- LDD3源码目录结构和源码分析经典链接
- QPushButton 点击信号分析
- Jdbc访问mysql查询聚合函数_JDBC连接参数设置对Oracle数据库的影响分析
- 一步一步带你训练自己的SSD检测算法
- 一个 IT 青年北漂四年的感悟
- C++ 构造函数、析构函数、拷贝构造函数
- maven local responsitory 手工新增jar
- Angularjs interceptor
- 在线文本比较工具(对比工具)
- linux qt编译器设置,Qt使用教程:添加编译器(一)
- 通信用特种光缆的选型
- 练习题︱豆瓣图书的推荐与搜索、简易版知识引擎构建(neo4j)
- 基于VB2008的winsocket控件网络编程
- 菜鸟的Springboot学习日历(一)
- 正则表达式三 不捕获文本 前瞻后顾 否定前瞻 否定后顾 贪婪匹配 懒惰匹配
- 拒绝低销量:2022最新YouTube引流亚马逊方法
- 最新版养猫小程序前端+后端搭建详细教程
- R软件-ggplot2 画火山图
- delta对冲策略_期权的Delta对冲策略对比分析
热门文章
- 火影新忍出击steam服务器维修,《火影忍者博人传:新忍出击》游戏打不开报错解决方法...
- 偷走sex.com的疑犯落网
- AI神器竟然能代替PS抠图了
- MIIX510(MIIX5)如何进入BIOS
- Nginx解决“no resolver defined to resolve xxx.xxx”
- go语言自动化编写word
- 如何优雅的在微信公众号中编辑代码
- php 判断是否微信访问,PHP判断是否微信访问的方法示例
- python unittest 极简自动化测试框架:一、使用discover处理多模块下的多条用例的方法
- C语言小练习——约瑟夫环问题