# 多目标追踪过滤器（TargetTracker）技术路线

> 本文档将 `drone_controller/target_tracker.py` 的实现抽象为与编程语言无关的技术路线，
> 可作为 C++（或任意语言）重新实现的规格说明。所有算法细节、参数默认值均以现有代码
> 实际实现为准，未包含代码中不存在的功能。

---

## 目录

1. [概述与系统定位](#1-概述与系统定位)
2. [输入输出接口定义](#2-输入输出接口定义)
3. [核心数据结构](#3-核心数据结构)
4. [卡尔曼滤波算法细节](#4-卡尔曼滤波算法细节)
5. [检测-轨迹关联流程](#5-检测-轨迹关联流程)
6. [轨迹生命周期与 ID 管理](#6-轨迹生命周期与-id-管理)
7. [主流程伪代码（每帧处理）](#7-主流程伪代码每帧处理)
8. [关键参数表](#8-关键参数表)
9. [C++ 移植要点与模块划分建议](#9-c-移植要点与模块划分建议)

---

## 1. 概述与系统定位

### 1.1 模块职责

本模块是一个**轻量级多目标追踪过滤器**，位于无人机视觉闭环控制链路中
"目标检测"与"飞控闭环"之间，承担三类职责：

1. **平滑**：检测器（YOLO 推理）输出的边界框逐帧抖动，通过恒定速度卡尔曼滤波
   输出平滑后的目标中心点与边界框，供下游速度闭环（PID）消费。
2. **抗短暂丢失（Coast 预测）**：目标短暂消失（如 2~5 帧）时，不立即判丢，
   而是用滤波器预测值继续输出目标位置（`is_predicted = true`），
   避免控制输出突变导致机体抽搐。
3. **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 节](#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` | 当帧即从轨道表中删除 |

注意两点（与代码一致）：

1. **min_hits 不阻止轨道参与关联或对外输出**，它只影响"重新选择主目标"时
   的候选池优先级（见 6.3）。这与 SORT 中 tentative 轨道不输出的做法不同。
2. **死亡判定使用严格大于**：`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 生命周期状态图

```mermaid
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
```

每帧处理的时序图：

```mermaid
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
```

```cpp
// ---- 基础几何量 ----
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 行为一致性检查清单

移植后建议用以下用例对齐行为：

1. **空检测帧序列**：连续输入空检测，主目标应输出 `is_predicted=true`
   共 `max_lost_frames` 帧，第 `max_lost_frames + 1` 帧 `tracked=false`
   （因死亡判定为 `lost_count > max_lost`）。
2. **ID 单调性**：多次进出画面的目标应获得严格递增、不复用的 ID。
3. **锁定不被抢占**：锁定目标 Coast 期间出现更高置信度新目标，
   `primary_id` 不变；锁定目标死亡后切换到新目标。
4. **min_hits 语义**：新轨道（hits 未达 min_hits）在无其他已确认实测轨道时
   也可被选为主目标（候选池回退逻辑）。
5. **cost 边界**：恰好 `cost = 1.0` 不构成匹配（严格小于）。
6. **置信度优先**：两轨道竞争同一检测时，置信度高者获胜。

### 9.5 线程模型注意

原系统中追踪器在推理主线程更新，结果由 20Hz 控制线程读取。C++ 移植若
保持该模型，仅需保护 `last_result` 的读写（互斥锁或无锁双缓冲），
追踪器内部状态不必加锁（单线程写）。若改为多线程调用 `update()`，
则需整体加锁。