基于3D激光点云的障碍物检测与跟踪---(3)基于匈牙利算法的障碍物跟踪

本文是《激光点云障碍物检测》系列的第三篇，前文已经介绍了 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 framesauto 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 米 → 肯定不是同一个东西

所以需要相对化：
$$dis=实际距离/物体自身尺寸$$

那为什么要去两者中较小的盒子尺寸作为基准？有两点考虑

1. 保守判断（防止过于宽松）
   如果使用较大的盒子来归一化（比如 std::max），那么, 当一个盒子特别大，一个很小，即使中心差很大，相对距离仍然可能被“稀释”得很小，会错误地认为两个不同物体是同一个。
   因此，让较小物体的尺寸来决定“近不近”——只要它偏移得超过它自身大小，就说明不是同一个物体
2. 尺度合理化
   使用最小的盒子尺寸可以保证：“两个物体的中心距离必须在较小物体的尺度范围内，才认为是同一个。”也就是说：你的小盒子如果已经“跑出自己身位了”，那就不能再当成同一个物体。

---

🧮 四、构造连接矩阵（Connection Matrix）

在实际应用中，我们通常先对代价矩阵进行“阈值筛选”，
只保留距离小于某个阈值的匹配对，得到二值化的连接矩阵。

核心代码如下：

  // 将连接对映射到左右集合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 表示不匹配。

---

⚙️ 五、匈牙利算法求解最优匹配

匈牙利算法的核心目标是：

在二分图（前一帧障碍物集与当前帧障碍物集）中找到最大匹配，使总匹配代价最小。

算法伪代码如下：

1. 初始化右侧节点（当前帧障碍物）的配对状态；

2. 遍历左侧每个障碍物（上一帧）；

3. 尝试为其找到一个匹配：

   • 若当前右节点未被匹配，则直接配对；
   • 若右节点已被占用，则递归查找能否重新安排其匹配（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

---

🧱 七、总结

本文介绍了如何利用匈牙利算法实现点云障碍物的帧间关联。
到此，我们已经完成了整个激光点云障碍物检测的主要流程：

1. ROI 区域裁剪 → 提取目标区域；
2. 地面分割 → 去除地面点；
3. 体素滤波 → 降采样加速计算；
4. 聚类提取 → 分离不同障碍物；
5. 匈牙利算法 → 建立跨帧关联。

这一整套流程为 动态障碍物跟踪、路径规划 和 自主避障 提供了关键基础。

---

🧭 后续

在此基础上，后续使用 卡尔曼滤波器（Kalman Filter） 或 多目标跟踪（Multi-Object Tracking, MOT） 模型进行障碍物运动预测；后续做好后会继续更新。

---

📘 完整系列推荐阅读：

1. 基于3D激光点云的障碍物检测与跟踪—(1)体素下采样、ROI 区域裁剪与地面点云分割
2. 基于3D激光点云的障碍物检测与跟踪—(2)点云聚类