26 KiB
多目标追踪过滤器（TargetTracker）技术路线
本文档将
drone_controller/target_tracker.py的实现抽象为与编程语言无关的技术路线， 可作为 C++（或任意语言）重新实现的规格说明。所有算法细节、参数默认值均以现有代码 实际实现为准，未包含代码中不存在的功能。
目录
1. 概述与系统定位
1.1 模块职责
本模块是一个轻量级多目标追踪过滤器，位于无人机视觉闭环控制链路中 "目标检测"与"飞控闭环"之间，承担三类职责：
- 平滑：检测器（YOLO 推理）输出的边界框逐帧抖动，通过恒定速度卡尔曼滤波 输出平滑后的目标中心点与边界框，供下游速度闭环（PID）消费。
- 抗短暂丢失（Coast 预测）：目标短暂消失（如 2~5 帧）时，不立即判丢，
而是用滤波器预测值继续输出目标位置（
is_predicted = true）， 避免控制输出突变导致机体抽搐。 - ID 稳定与主目标锁定：多目标场景下通过"中心距离 + IoU 加权代价"的贪婪 关联维持稳定 ID；选定主目标（primary）后持续锁定，直到该轨道真正死亡， 避免被新出现的高置信度目标抢走导致"甩头"。
1.2 在系统中的位置
摄像头/RTSP ──► NPU 推理(YOLO) ──► 后处理检测框
│
▼
┌──────────────────┐
│ TargetTracker │ ◄── 每帧调用 update()
│ (本模块) │
└──────────────────┘
│ 平滑中心点 / 框 / 状态标志
┌────────────────┼────────────────┐
▼ ▼ ▼
20Hz 控制线程 Coast 期间 tracked=false
PID → 速度指令 清空 PID 积分 触发零速刹车
- 调用时机：由推理主循环在每一帧检测完成后调用一次
update()； 结果由独立的控制线程（约 20Hz 速度闭环）读取消费。 - 上游行为约定（来自调用方，供移植时对齐）：
- 输出
is_predicted == true（Coast 中）时，调用方会清空 PID 积分项， 防止积分饱和（windup）； - 输出
tracked == false（彻底丢失）时，调用方执行零速刹车； - 停止视觉闭环时调用方会调用
reset()清空全部追踪状态。
- 输出
1.3 设计特点
- 恒定速度模型，8 维状态向量，纯线性卡尔曼滤波（无 UKF/EKF）。
- 关联采用贪婪最邻近（按轨道置信度降序逐一匹配），不使用匈牙利算法； 代价函数为中心距离与 (1−IoU) 的加权和。
- 所有坐标使用原始像素坐标（与检测后处理输出空间一致），不做归一化。
- 时间步长以"帧"为单位（dt = 1），与帧率无关，参数按约 30fps、640×480 画面调优。
2. 输入输出接口定义
2.1 主要接口
| 接口 | 说明 |
|---|---|
构造(max_lost_frames, max_association_dist, min_hits, dist_weight, iou_weight) |
创建追踪器，参数见第 8 节 |
update(detections, frame_shape) → TrackResult |
每帧调用一次，输入检测列表，输出主目标追踪结果 |
get_last_result() → TrackResult |
获取上一帧结果快照（线程安全读取场景使用） |
reset() |
清空所有轨道、ID 计数器、锁定状态与缓存结果 |
active_count() → int |
当前存活轨道数量 |
active_tracks() → {track_id: Tracklet} |
当前所有轨道的只读快照（浅复制） |
2.2 输入：detections
每帧检测列表，每个元素为 6 元组：
[x1, y1, x2, y2, conf, cls_id]
| 字段 | 类型 | 含义 |
|---|---|---|
| x1, y1 | 浮点 | 边界框左上角（像素坐标） |
| x2, y2 | 浮点 | 边界框右下角（像素坐标） |
| conf | 浮点 | 检测置信度 ∈ [0,1] |
| cls_id | 整数 | 类别编号 |
- 允许传入空列表（表示本帧无任何检测），此时所有轨道进入 Coast 计数。
frame_shape = (height, width)，可选。仅在"无实测轨道、只有 Coast 轨道" 时用于选取主目标的兜底排序（选最接近画面中心的轨道）；未提供时按 640×480 的画面中心 (320, 240) 计算。
2.3 输出：TrackResult
update() 每帧返回一个结构体（代码中以字典表示），字段如下：
| 字段 | 类型 | 含义 |
|---|---|---|
| tracked | 布尔 | 是否存在有效主目标。false 时调用方应刹车 |
| primary_id | 整数或空 | 主目标轨道 ID；无目标时为空 |
| center | (cx, cy) 或空 | 平滑后的目标中心点（像素坐标） |
| box | [x1,y1,x2,y2] 或空 | 平滑后的边界框 |
| confidence | 浮点 | 当前置信度（Coast 期间继承最近一次实测的置信度） |
| is_predicted | 布尔 | true = 当前帧处于 Coast（仅有预测、无实测） |
| lost_frames | 整数 | 主目标连续未命中帧数 |
| n_active | 整数 | 当前活跃轨道总数 |
| raw | [x1,y1,x2,y2] 或空 | 关联到的原始检测框；Coast 时为空 |
丢失（无主目标）时的返回值：tracked=false，其余定位字段为空，lost_frames=0。
3. 核心数据结构
模块由三层结构组成：
TargetTracker（多目标管理器）
└── Tracklet × N（单条轨道：生命周期 + 辅助状态）
└── KalmanBoxFilter（单个 8 维恒定速度卡尔曼滤波器）
3.1 KalmanBoxFilter（单目标滤波器）
| 成员 | 类型 | 含义 |
|---|---|---|
| x | 数组[8] | 状态向量 [cx, cy, w, h, vx, vy, vw, vh] |
| P | 矩阵 8×8 | 状态协方差 |
| F | 矩阵 8×8 | 状态转移矩阵（常量） |
| H | 矩阵 4×8 | 观测矩阵（常量） |
| initialized | 布尔 | 是否已用首次观测初始化 |
3.2 Tracklet（轨道）
| 成员 | 类型 | 含义 |
|---|---|---|
| track_id | 整数 | 全局唯一轨道 ID |
| kf | KalmanBoxFilter | 本轨道的滤波器 |
| lost_count | 整数 | 当前连续未匹配帧数 |
| max_lost | 整数 | 最大允许连续丢失帧数（构造时注入） |
| confidence | 浮点 | 最近一次匹配的置信度 |
| cls_id | 整数 | 最近一次匹配的类别 ID |
| age | 整数 | 轨道已存在的总帧数（创建时为 1） |
| hits | 整数 | 累计成功匹配次数（创建时为 1） |
| last_center | (cx, cy) | 最近一次中心点（实测或预测） |
| last_box | [x1,y1,x2,y2] 或空 | 最近一次边界框（实测或预测） |
派生状态（只读属性）：
is_dead := lost_count > max_lost（可回收）is_coasting := lost_count > 0（处于纯预测状态）
3.3 TargetTracker（管理器）
| 成员 | 类型 | 含义 |
|---|---|---|
| max_lost_frames 等 5 项 | 浮点/整数 | 配置参数，见第 8 节 |
| next_id | 整数 | 下一个可分配的轨道 ID（从 0 递增，不复用） |
| tracks | 哈希表 {track_id → Tracklet} | 所有存活轨道 |
| frame_count | 整数 | 已处理帧数（仅统计用） |
| locked_primary_id | 整数或空 | 当前锁定的主目标轨道 ID |
| last_result | TrackResult | 上一帧结果缓存（供跨线程读取） |
4. 卡尔曼滤波算法细节
4.1 状态空间模型
状态向量（8 维）与观测向量（4 维）：
[ \mathbf{x} = [c_x,\ c_y,\ w,\ h,\ v_x,\ v_y,\ v_w,\ v_h]^T, \qquad \mathbf{z} = [c_x,\ c_y,\ w,\ h]^T ]
其中 (c_x, c_y) 为框中心像素坐标，(w, h) 为框宽高，后四项为对应速度 （单位：像素/帧）。
状态转移（恒定速度模型，dt = 1 帧）：
[ \mathbf{x}k = F,\mathbf{x}{k-1},\quad F = \begin{bmatrix} I_4 & I_4 \ 0 & I_4 \end{bmatrix} ]
即前 4 维（位置/尺寸）各自加上对应速度：F[i][i+4] = 1 (i=0..3)，其余为单位阵。
观测模型（直接观测位置与尺寸，不观测速度）：
[ \mathbf{z}_k = H,\mathbf{x}_k,\quad H = [,I_4 \mid 0,] \in \mathbb{R}^{4\times 8} ]
4.2 噪声参数（经验值，针对 640×480 画面调优）
| 常量 | 值 | 用途 |
|---|---|---|
| STD_POS | 1/50 = 0.02 | 位置过程噪声比例系数 |
| STD_VEL | 1/200 = 0.005 | 速度过程噪声比例系数 |
| INI_POS | 2.0 | 初始位置协方差（对角） |
| INI_VEL | 100.0 | 初始速度协方差（对角） |
关键设计：噪声与目标尺寸成比例（动态 Q/R）。 预测时过程噪声 Q 按当前状态中的目标宽高缩放：
[ \sigma_{pos} = [\tfrac{w}{50},\ \tfrac{h}{50},\ \tfrac{w}{50},\ \tfrac{h}{50}], \quad \sigma_{vel} = [\tfrac{w}{200},\ \tfrac{h}{200},\ \tfrac{w}{200},\ \tfrac{h}{200}] ]
[ Q = \mathrm{diag}\big(\sigma_{pos}^2,\ \sigma_{vel}^2\big) ]
（Q 为 8×8 对角阵，前 4 个对角元来自 (\sigma_{pos})，后 4 个来自 (\sigma_{vel})； 每个分量先平方再上对角。）
更新时观测噪声 R 同样按当前状态的宽高缩放，并额外乘 2 增强平滑：
[ R = \mathrm{diag}\big(2,(\tfrac{|w|}{50})^2,\ 2,(\tfrac{|h|}{50})^2,\ 2,(\tfrac{|w|}{50})^2,\ 2,(\tfrac{|h|}{50})^2\big) ]
实现注意：R 中使用
abs(w)、abs(h)防御负尺寸；Q 中直接使用状态值。 该差异与代码保持一致即可，正常情形下尺寸为正，二者等价。
4.3 初始化（initiate）
用首次观测 ([c_x, c_y, w, h]) 初始化：
- 状态：(\mathbf{x} = [c_x, c_y, w, h, 0, 0, 0, 0]^T)（速度置零）
- 协方差：(P = \mathrm{diag}(2, 2, 2, 2, 100, 100, 100, 100)) （位置置信度中等，速度置信度低，允许前几帧快速收敛速度估计）
4.4 预测（predict）
未初始化时返回空。否则：
[ \mathbf{x} \leftarrow F,\mathbf{x}, \qquad P \leftarrow F,P,F^T + Q ]
返回预测的位置/尺寸 ([c_x, c_y, w, h])（状态向量前 4 维的副本）。
4.5 更新（update）
未初始化时直接返回。给定观测 (\mathbf{z} = [c_x, c_y, w, h])：
[ S = H,P,H^T + R ]
[ K = P,H^T,S^{-1} ]
[ \mathbf{y} = \mathbf{z} - H,\mathbf{x} \quad \text{（残差 / innovation）} ]
[ \mathbf{x} \leftarrow \mathbf{x} + K,\mathbf{y}, \qquad P \leftarrow (I_8 - K,H),P ]
实现注意：S 为 4×4 矩阵，代码中使用通用矩阵求逆。C++ 中可用 4×4 专用求逆（如高斯消元或 LDL 分解），无需引入大型线性代数库。 代码未对位置/尺寸做画面边界钳制，也未做负尺寸约束，移植时保持一致 （若需增强鲁棒性可另行添加，但属于行为变更）。
5. 检测-轨迹关联流程
5.1 代价函数
对每条轨道（预测后的中心 ((p_x, p_y))、预测框 (B_t)）与每个检测 （中心 ((d_x, d_y))、检测框 (B_d)），定义加权代价：
[ \mathrm{cost} = w_{dist}\cdot \frac{\lVert (p_x,p_y)-(d_x,d_y) \rVert_2}{D_{max}} ;+; w_{iou}\cdot (1 - \mathrm{IoU}(B_t, B_d)) ]
- (\lVert\cdot\rVert_2) 为中心点欧氏距离（像素）；
- (D_{max}) 为距离软阈值（
max_association_dist，默认 200 px）， 用于把距离项归一化到与 IoU 项同量级；实际使用时取 (\max(D_{max},\ 10^{-3})) 防止除零； - 默认 (w_{dist} = w_{iou} = 0.5)。
匹配有效性判据：仅当 cost < 1.0 时视为有效匹配
（实现上：每条轨道的最优代价初始化为 1.0，只有严格小于 1.0 的候选才会被记录）。
5.2 IoU 计算
轴对齐框 ([x_1,y_1,x_2,y_2]) 的标准交并比：
[ \mathrm{IoU}(A,B) = \frac{\mathrm{area}(A \cap B)}{\mathrm{area}(A) + \mathrm{area}(B) - \mathrm{area}(A \cap B)} ]
交集宽高需与 0 取最大值；任一框为空、交集面积 ≤ 0 或并集 ≤ 0 时返回 0。
5.3 贪婪关联算法（非匈牙利）
按轨道置信度从高到低排序后逐一贪心匹配（高置信轨道优先抢占检测）：
ASSOCIATE(tracks, detections):
若 tracks 或 detections 为空：
返回 matches=[], unmatched_tracks=全部轨道, unmatched_dets=全部检测
预计算每个检测的中心与框
sorted_tracks ← 按 track.confidence 降序排序的轨道列表
assigned_dets ← 空集合; assigned_tracks ← 空集合; matches ← []
对 sorted_tracks 中每条轨道 t：
若 t.id ∈ assigned_tracks：跳过
best_cost ← 1.0; best_det ← 空
对每个未分配的检测 d：
dist ← 欧氏距离(t.last_center, d.center)
norm_dist ← dist / max(Dmax, 1e-3)
iou ← IoU(t.last_box, d.box)
cost ← w_dist × norm_dist + w_iou × (1 − iou)
若 cost < best_cost：
best_cost ← cost; best_det ← d
若 best_det 非空：
matches.append((t.id, best_det.idx))
assigned_dets.add(best_det.idx); assigned_tracks.add(t.id)
unmatched_tracks ← tracks 中不在 assigned_tracks 的轨道
unmatched_dets ← detections 中不在 assigned_dets 的检测
返回 (matches, unmatched_tracks, unmatched_dets)
复杂度：O(M×N)（M 条轨道 × N 个检测），另有 O(M log M) 排序。 在机载小目标数量（通常个位数）下开销可忽略。
说明：代码明确选择贪心而非匈牙利最优匹配——贪心实现简单、无第三方依赖， 配合"置信度优先 + 距离/IoU 加权"在无人机跟随单主目标的场景下已足够稳定。
6. 轨迹生命周期与 ID 管理
6.1 生命周期状态
代码中没有显式状态枚举，但由字段组合可归纳出四个阶段：
| 阶段 | 判定条件 | 行为 |
|---|---|---|
| 新建（未确认） | hits < min_hits |
正常参与关联与预测，但主目标选举时优先级低于已确认轨道 |
| 已确认 | hits ≥ min_hits |
主目标选举的优先候选池 |
| Coast（滑行预测） | 0 < lost_count ≤ max_lost |
无实测，输出滤波器预测值，is_predicted=true |
| 死亡 | lost_count > max_lost |
当帧即从轨道表中删除 |
注意两点（与代码一致）：
- min_hits 不阻止轨道参与关联或对外输出，它只影响"重新选择主目标"时 的候选池优先级（见 6.3）。这与 SORT 中 tentative 轨道不输出的做法不同。
- 死亡判定使用严格大于：
lost_count > max_lost，即默认配置下允许 连续 8 帧 Coast，第 9 帧未命中才回收。
6.2 ID 分配策略
- 全局计数器
next_id从 0 开始，每创建一个新轨道自增 1； - ID 永不复用；
reset()时计数器归零； - 未匹配到任何现有轨道的检测，每个都会创建一个新轨道（无预过滤， 不要求检测置信度下限——上游检测器已完成阈值过滤）。
6.3 主目标（primary）锁定与选举
对外每帧只输出一个主目标，供控制回路使用。选择规则：
SELECT_PRIMARY:
若轨道表为空：清除锁定，返回 tracked=false
① 锁定延续：若 locked_primary_id 非空且该轨道仍存活：
直接以该轨道输出（Coast 中照样输出，is_predicted=true）
—— 不被其他更高置信度目标抢占
② 否则（无锁定 / 锁定目标已死亡被回收）→ 重新选举并锁定：
measured ← 所有 lost_count == 0 的轨道（本帧有实测）
coasting ← 其余存活轨道
若 measured 非空：
confirmed ← measured 中 hits ≥ min_hits 者
pool ← confirmed（若为空则退回 measured）
chosen ← pool 中 confidence 最大者
否则若 coasting 非空：
chosen ← coasting 中离画面中心最近者（欧氏距离平方排序）
is_predicted ← true
否则：
清除锁定，返回 tracked=false
locked_primary_id ← chosen.id
以 chosen 输出结果
锁定的唯一解除途径：锁定轨道死亡被回收时清除 locked_primary_id
（在轨道删除的同一处处理），或调用 reset()。
6.4 生命周期状态图
graph TB
A[未匹配检测] --> B[新建轨道 hits=1 lost=0]
B -->|每帧匹配成功| C{hits >= min_hits?}
C -->|否, 选举低优先级| B
C -->|是, 已确认| D[正常跟踪 lost=0]
D -->|某帧未匹配| E[Coast lost_count+1 输出预测值]
E -->|重新匹配| D
E -->|lost_count > max_lost| F[死亡回收 删除轨道]
B -->|连续未匹配| E
F -->|若是锁定主目标| G[清除锁定 下帧重新选举]
7. 主流程伪代码（每帧处理）
update(detections, frame_shape) 的完整单帧流程：
UPDATE(detections, frame_shape):
frame_count += 1
# ---- 步骤 1：所有存活轨道先行预测（推进一帧）----
对 tracks 中每条轨道 t：
t.predict() # 卡尔曼预测；更新 t.last_center / t.last_box；age += 1
# ---- 步骤 2：构造检测中心表并关联 ----
若 detections 非空：
det_centers ← []
对每个检测 i = [x1,y1,x2,y2,conf,cls]：
cx ← (x1+x2)/2; cy ← (y1+y2)/2
w ← x2−x1; h ← y2−y1
det_centers.append((i, cx, cy, w, h, conf, cls))
(matches, unmatched_tracks, unmatched_dets) ← ASSOCIATE(tracks, det_centers)
否则：
matches ← []; unmatched_tracks ← 全部轨道; unmatched_dets ← []
# ---- 步骤 3：用匹配到的检测更新轨道 ----
对每个匹配 (tid, did)：
由 det_centers[did] 重建原始框 raw_box = [cx−w/2, cy−h/2, cx+w/2, cy+h/2]
tracks[tid].update(raw_box, conf, cls)
# 内部：框→[cx,cy,w,h] → kf.update；lost_count←0；hits+1；age+1
# 更新 last_center / last_box / confidence / cls_id
# ---- 步骤 4：未匹配轨道计丢失，死亡者回收 ----
对每个 tid ∈ unmatched_tracks：
tracks[tid].mark_miss() # lost_count+1, age+1
若 tracks[tid].is_dead： # lost_count > max_lost
若 tid == locked_primary_id：locked_primary_id ← 空
从 tracks 中删除 tid
# ---- 步骤 5：未匹配检测 → 创建新轨道 ----
对每个 did ∈ unmatched_dets：
以 (next_id, [cx,cy,w,h], conf, cls, max_lost=max_lost_frames) 创建 Tracklet
new_track.last_box ← 由中心/宽高重建的框 # 保证创建当帧即可参与输出
tracks[next_id] ← new_track
next_id += 1
# ---- 步骤 6：选取/延续主目标，组装输出 ----
result ← SELECT_PRIMARY(frame_shape)
last_result ← result # 缓存供跨线程读取
返回 result
其中 Tracklet.predict() 与 Tracklet.update()：
TRACKLET_PREDICT():
pred ← kf.predict()
若 pred 非空：
last_center ← (pred.cx, pred.cy)
last_box ← center_size → [x1,y1,x2,y2]
age += 1
返回 (last_center, last_box)
TRACKLET_UPDATE(box=[x1,y1,x2,y2], conf, cls):
cx ← (x1+x2)/2; cy ← (y1+y2)/2
w ← x2−x1; h ← y2−y1
kf.update([cx, cy, w, h])
last_center ← (cx, cy); last_box ← box
confidence ← conf; cls_id ← cls
lost_count ← 0; hits += 1; age += 1
每帧处理的时序图：
graph TB
A[接收本帧检测] --> B[全部轨道卡尔曼预测]
B --> C{有检测?}
C -->|是| D[中心距离+IoU 贪婪关联]
C -->|否| E[全部轨道标记 miss]
D --> F[匹配轨道: 卡尔曼更新]
D --> G[未匹配轨道: miss 计数]
D --> H[未匹配检测: 新建轨道]
G --> I{lost > max_lost?}
I -->|是| J[回收轨道 必要时解除锁定]
I -->|否| K[主目标选择/锁定延续]
E --> I
F --> K
H --> K
J --> K
K --> L[输出 TrackResult]
8. 关键参数表
以下默认值全部取自代码实际实现。
8.1 追踪器配置参数
| 参数 | 默认值 | 含义与调优建议 |
|---|---|---|
| max_lost_frames | 8 | 最大连续丢失帧数（约 250~300ms @ 30fps）。越大越能容忍遮挡，但 Coast 漂移时间越长；判丢后下游刹车 |
| max_association_dist | 200.0（像素） | 中心距离软阈值，用于归一化距离代价。目标运动快/帧率低时调大 |
| min_hits | 3 | 轨道"确认"所需累计匹配次数，仅影响主目标选举优先级 |
| dist_weight | 0.5 | 关联代价中归一化距离项权重 |
| iou_weight | 0.5 | 关联代价中 (1−IoU) 项权重。两项建议之和保持 1.0，使 cost<1.0 判据语义稳定 |
工程部署时以上参数可由外部配置文件注入（项目中即通过配置文件的
target_tracker段传入，未配置项回落到代码默认值）。
8.2 卡尔曼滤波内部常量
| 常量 | 值 | 含义 |
|---|---|---|
| dt | 1.0 | 状态转移步长，以"帧"为单位 |
| STD_POS | 1/50 | 位置过程/观测噪声比例系数（乘以目标宽高） |
| STD_VEL | 1/200 | 速度过程噪声比例系数（乘以目标宽高） |
| INI_POS | 2.0 | 初始位置协方差 |
| INI_VEL | 100.0 | 初始速度协方差 |
| R 缩放因子 | 2.0 | 观测噪声额外加倍，增强输出平滑 |
| 匹配代价上限 | 1.0 | cost ≥ 1.0 判为不可匹配 |
| Coast 兜底画面中心 | (320, 240) | frame_shape 缺省时按 640×480 计算 |
9. C++ 移植要点与模块划分建议
9.1 模块 / 类划分草案
与代码三层结构一一对应：
target_tracker/
├── kalman_box_filter.h/.cpp // class KalmanBoxFilter
├── tracklet.h/.cpp // struct/class Tracklet
└── target_tracker.h/.cpp // class TargetTracker + struct TrackResult
// ---- 基础几何量 ----
struct BoxXYXY { double x1, y1, x2, y2; };
struct BoxCXCYWH{ double cx, cy, w, h; };
// ---- 单目标滤波器 ----
class KalmanBoxFilter {
double x[8]; // 状态向量
double P[8][8]; // 协方差
static constexpr double F[8][8] = /* I4 右上 I4 */;
static constexpr double H[4][8] = /* [I4 | 0] */;
bool initialized = false;
void initiate(const BoxCXCYWH& z);
bool predict(BoxCXCYWH& out); // 未初始化返回 false
void update(const BoxCXCYWH& z);
};
// ---- 轨道 ----
struct Tracklet {
int track_id;
KalmanBoxFilter kf;
int lost_count = 0;
int max_lost;
double confidence;
int cls_id;
int age = 1;
int hits = 1;
std::pair<double,double> last_center;
std::optional<BoxXYXY> last_box; // 首帧预测前可能为空（创建时即赋实测框）
bool is_dead() const { return lost_count > max_lost; }
bool is_coasting() const { return lost_count > 0; }
};
// ---- 输出结构 ----
struct TrackResult {
bool tracked = false;
std::optional<int> primary_id;
std::pair<double,double> center; // tracked=false 时无效
std::optional<BoxXYXY> box;
double confidence = 0.0;
bool is_predicted = false;
int lost_frames = 0;
int n_active = 0;
std::optional<BoxXYXY> raw;
};
// ---- 管理器 ----
class TargetTracker {
public:
struct Config {
int max_lost_frames = 8;
double max_association_dist = 200.0;
int min_hits = 3;
double dist_weight = 0.5;
double iou_weight = 0.5;
};
explicit TargetTracker(const Config& cfg);
TrackResult update(const std::vector<Detection>& dets,
std::optional<FrameShape> frame_shape);
TrackResult last_result() const; // 需要跨线程时加锁/原子快照
void reset();
};
9.2 STL 容器选择
| 代码中的结构 | C++ 建议 |
|---|---|
轨道哈希表 {id: Tracklet} |
std::unordered_map<int, Tracklet>；若需按 ID 有序遍历可用 std::map |
| 关联中的已分配集合 | std::unordered_set<int>，或目标数量少时直接用 std::vector<bool> 位标记 |
| 按置信度排序的轨道视图 | 复制 ID/指针到 std::vector 后 std::sort（不直接排哈希表） |
| 检测列表 | std::vector<Detection>，Detection 为 POD 结构体 |
| 结果缓存 | TrackResult 值类型；跨线程用互斥锁保护的拷贝，或双缓冲原子交换 |
9.3 数值计算
- 矩阵规模极小（8×8、4×8、4×4），无需引入 Eigen/OpenCV：
用固定大小二维数组或
std::array<double, 64>手写矩阵乘法即可。 - 唯一需要通用实现的是 4×4 矩阵求逆（S 矩阵）。可用高斯-约当消元或 利用 S 接近对称正定的特性做 Cholesky/LDL 分解；建议加极小量 正则化（对角加 1e-9）防奇异。
- 距离计算用
std::hypot；IoU 为纯标量运算。 - 全程
double精度，与代码一致（代码为 float64）。
9.4 行为一致性检查清单
移植后建议用以下用例对齐行为：
- 空检测帧序列：连续输入空检测，主目标应输出
is_predicted=true共max_lost_frames帧，第max_lost_frames + 1帧tracked=false（因死亡判定为lost_count > max_lost）。 - ID 单调性：多次进出画面的目标应获得严格递增、不复用的 ID。
- 锁定不被抢占：锁定目标 Coast 期间出现更高置信度新目标，
primary_id不变；锁定目标死亡后切换到新目标。 - min_hits 语义：新轨道（hits 未达 min_hits）在无其他已确认实测轨道时 也可被选为主目标（候选池回退逻辑）。
- cost 边界：恰好
cost = 1.0不构成匹配（严格小于）。 - 置信度优先：两轨道竞争同一检测时，置信度高者获胜。
9.5 线程模型注意
原系统中追踪器在推理主线程更新，结果由 20Hz 控制线程读取。C++ 移植若
保持该模型，仅需保护 last_result 的读写（互斥锁或无锁双缓冲），
追踪器内部状态不必加锁（单线程写）。若改为多线程调用 update()，
则需整体加锁。