本文是《激光點雲障礙物檢測》系列的第三篇，前文已經介紹了 ROI 區域裁剪、地面點雲分割、體素濾波以及聚類提取。本篇將重點講解如何利用 匈牙利算法（Hungarian Algorithm） 在多幀點雲之間進行 障礙物匹配與跟蹤（Object Association & Tracking），實現穩定的障礙物ID跟蹤效果。
在連續幀點雲數據中，每一幀通過聚類算法得到若干個障礙物。
然而，單幀聚類結果是無序的，每一幀的障礙物 ID都可能變化：
|
幀號
|
檢測到的障礙物
|
ID分配（隨機）
|
|
t-1
|
A, B, C
|
0, 1, 2
|
|
t
|
A, B, C
|
1, 0, 2
|
若不進行匹配關聯，系統無法判斷“當前幀的第1個障礙物”是否與上一幀的第0個障礙物是同一個實體。
這就會導致軌跡跳變、ID重置、預測不穩定等問題。
為了解決這個問題，我們使用 匈牙利算法（ Kuhn-Munkres）在前後兩幀的障礙物之間建立最優匹配關係。
關聯算法的整體思路如下：
前一幀障礙物集合 (Frame t-1)
↓
當前幀障礙物集合 (Frame t)
↓
計算兩幀之間的連接矩陣（障礙物之間的歐式距離、障礙物框之間的相似度等）
↓
使用匈牙利算法尋找最優匹配
↓
得到一一對應關係（ID關聯結果）假設在前一幀檢測到 N 個障礙物，在當前幀檢測到 M 個障礙物。
我們通過計算障礙物間的歐式距離與形狀相似度來建立一個匹配對向量。
std::vector<int> pre_ids;
std::vector<int> cur_ids;
std::vector<int> matches;
// Associate Boxes that are similar in two frames
auto connection_pairs = associateBoxes(prev_boxes, *curr_boxes,
displacement_thresh, iou_thresh);
if (connection_pairs.empty()) return;connection_pairs表示：
表示匹配的 (boxA_id, boxB_id) 對，比如 [ [0,3], [1,2], [1,3] ]
其中在構造匹配時，是通過計算障礙物之間的歐式距離與框相似度來判定的
const float dis =
sqrt((a.position[0] - b.position[0]) * (a.position[0] - b.position[0]) +
(a.position[1] - b.position[1]) * (a.position[1] - b.position[1]) +
(a.position[2] - b.position[2]) * (a.position[2] - b.position[2]));
const float a_max_dim =
std::max(a.dimension[0], std::max(a.dimension[1], a.dimension[2]));
const float b_max_dim =
std::max(b.dimension[0], std::max(b.dimension[1], b.dimension[2]));
const float ctr_dis = dis / std::min(a_max_dim, b_max_dim); // 歸一化距離：相對中心位移
/* 計算包圍盒尺寸的相對差異，值越小表示尺寸越相似
IOU (Intersection Over Union) 通常用於衡量兩個邊界框的重疊程度，這裏用來衡量尺寸相似度。
公式：2 * |A - B| / (|A| + |B|)，值域為 [0, 2]，值越小表示尺寸越相似
*/
const float x_dim =
2 * (a.dimension[0] - b.dimension[0]) / (a.dimension[0] + b.dimension[0]);
const float y_dim =
2 * (a.dimension[1] - b.dimension[1]) / (a.dimension[1] + b.dimension[1]);
const float z_dim =
2 * (a.dimension[2] - b.dimension[2]) / (a.dimension[2] + b.dimension[2]);
if (ctr_dis <= displacement_thresh && x_dim <= iou_thresh &&
y_dim <= iou_thresh && z_dim <= iou_thresh) {
return true;
} else {
return false;
}核心代碼中，那為什麼計算相對中心位移要用兩個障礙物框鍾最大維度之間的最小值呢？
給出一個例子就明白了：
我們想判斷兩個包圍盒是不是“同一個障礙物”。假設有兩個盒子：Box A：長 4 米、Box B：長 0.4 米、中心距離 = 0.5 米。那麼這兩個盒子的位置差 0.5 米到底算“近”還是“遠”？這要看盒子自身的尺寸。
首先，如果距離不歸一化，那麼對於大盒子、小盒子、甚至微小噪聲，都會使用同一個“0.5 米閾值”，顯然是不合理的。比如：
- 兩個 4 米大的卡車中心相差 0.5 米 → 仍然是同一個車
- 兩個 0.2 米的小物體中心相差 0.5 米 → 肯定不是同一個東西
所以需要相對化：
那為什麼要去兩者中較小的盒子尺寸作為基準？有兩點考慮
- 保守判斷（防止過於寬鬆）
如果使用較大的盒子來歸一化（比如 std::max），那麼, 當一個盒子特別大，一個很小，即使中心差很大，相對距離仍然可能被“稀釋”得很小，會錯誤地認為兩個不同物體是同一個。
因此，讓較小物體的尺寸來決定“近不近”——只要它偏移得超過它自身大小，就説明不是同一個物體- 尺度合理化
使用最小的盒子尺寸可以保證：“兩個物體的中心距離必須在較小物體的尺度範圍內，才認為是同一個。”也就是説：你的小盒子如果已經“跑出自己身位了”，那就不能再當成同一個物體。
在實際應用中，我們通常先對代價矩陣進行“閾值篩選”，
只保留距離小於某個閾值的匹配對，得到二值化的連接矩陣。
核心代碼如下：
// 將連接對映射到左右集合
for (auto &pair : connection_pairs) {
if (std::find(left->begin(), left->end(), pair[0]) == left->end())
left->push_back(pair[0]);
if (std::find(right->begin(), right->end(), pair[1]) == right->end())
right->push_back(pair[1]);
}
std::vector<std::vector<int>> connection_matrix(
left->size(), std::vector<int>(right->size(), 0));
// 在矩陣中填入匹配標記
for (auto &pair : connection_pairs) {
int left_index = std::find(left->begin(), left->end(), pair[0]) - left->begin();
int right_index = std::find(right->begin(), right->end(), pair[1]) - right->begin();
connection_matrix[left_index][right_index] = 1;
}在這個矩陣中：
connection_matrix[i][j] = 1表示可匹配；connection_matrix[i][j] = 0表示不匹配。
⚙️ 五、匈牙利算法求解最優匹配
匈牙利算法的核心目標是：
在二分圖（前一幀障礙物集與當前幀障礙物集）中找到最大匹配，使總匹配代價最小。
算法偽代碼如下：
- 初始化右側節點（當前幀障礙物）的配對狀態；
- 遍歷左側每個障礙物（上一幀）；
- 嘗試為其找到一個匹配：
- 若當前右節點未被匹配，則直接配對；
- 若右節點已被佔用，則遞歸查找能否重新安排其匹配（DFS方式）。
代碼實現：
std::vector<bool> right_connected(connection_matrix[0].size(), false);
std::vector<int> right_pair(connection_matrix[0].size(), -1);
int count = 0;
for (int i = 0; i < connection_matrix.size(); ++i) {
if (hungarianFind(i, connection_matrix, &right_connected, &right_pair))
count++;
}
std::cout << "Found " << count << " matches between frames." << std::endl;匈牙利算法輸出的結果是一個匹配列表 right_pair，
其中 right_pair[j] = i 表示當前幀的第 j 個障礙物對應上一幀的第 i 個障礙物。
這樣就能實現障礙物的跨幀追蹤，使得每個障礙物都有穩定的 ID。
示意圖如下：
Frame t-1: Box_0 Box_1 Box_2
↓ ↓
Frame t: Box_1 Box_0 Box_2本文介紹瞭如何利用匈牙利算法實現點雲障礙物的幀間關聯。
到此，我們已經完成了整個激光點雲障礙物檢測的主要流程：
- ROI 區域裁剪 → 提取目標區域；
- 地面分割 → 去除地面點；
- 體素濾波 → 降採樣加速計算；
- 聚類提取 → 分離不同障礙物；
- 匈牙利算法 → 建立跨幀關聯。
這一整套流程為 動態障礙物跟蹤、路徑規劃 和 自主避障 提供了關鍵基礎。
在此基礎上，後續使用 卡爾曼濾波器（Kalman Filter） 或 多目標跟蹤（Multi-Object Tracking, MOT） 模型進行障礙物運動預測；後續做好後會繼續更新。