first commit

2026-09-02 15:13:13 +08:00
commit cb00322a50
107 changed files with 10918 additions and 0 deletions
@@ -0,0 +1,217 @@
+# target_tracker 视觉追踪库实现文档
+
+> 独立动态库（`libtarget_tracker.so`），实现轻量级多目标追踪过滤器：
+> 恒定速度卡尔曼滤波平滑 + Coast 抗短暂丢失 + ID 稳定与主目标锁定。
+> 算法行为的唯一权威规格为 `docs/视觉追踪技术路线.md`（下称"技术路线"），
+> 本文档不重复抄录算法全文，仅描述实现结构与对照关系。
+
+---
+
+## 一、功能职责与边界
+
+**做什么**：
+
+- 每帧接收 YOLO 检测框列表，输出**一个主目标**的平滑中心点/边界框/状态标志
+  （`TrackResult`），供下游速度闭环（PID）消费。
+- 平滑：8 维恒定速度卡尔曼滤波（位置/尺寸 + 速度），抑制检测框逐帧抖动。
+- Coast 预测：目标短暂消失时用滤波器预测值继续输出（`is_predicted=true`），
+  避免控制输出突变；连续丢失超过 `max_lost_frames`（严格大于）才判丢。
+- ID 稳定：贪婪最邻近关联（中心距离 + IoU 加权代价）维持轨道 ID；
+  主目标锁定后不被高置信度新目标抢占，仅在轨道死亡或 `Reset()` 时解除。
+
+**不做什么**（边界）：
+
+- 不做光流估计、不做激光/惯导等多源融合（属 perception 模块职责）；
+- 不做 Topic 订阅/发布接线、不依赖 spdlog/Eigen/OpenCV，零第三方依赖；
+- 不做画面边界钳制与负尺寸约束（与技术路线 §4.5 注记保持一致）；
+- 不要求检测置信度下限过滤（上游检测器已完成阈值过滤）。
+
+## 二、接口与数据流
+
+命名空间 `drone::tracker`，头文件保护 `#pragma once`。
+
+| 头文件 | 内容 |
+|--------|------|
+| `include/target_tracker/kalman_box_filter.h` | `BoxCXCYWH`、`KalmanBoxFilter` |
+| `include/target_tracker/tracklet.h` | `BoxXYXY`、`ToCenterSize`/`ToXYXY`/`ComputeIoU`、`Tracklet` |
+| `include/target_tracker/target_tracker.h` | `Detection`、`FrameShape`、`TrackResult`、`TargetTracker` |
+
+主要接口签名：
+
+```cpp
+class KalmanBoxFilter {
+    void Initiate(const BoxCXCYWH& z);              // 首次观测初始化（§4.3）
+    std::optional<BoxCXCYWH> Predict();             // 未初始化返回 nullopt（§4.4）
+    void Update(const BoxCXCYWH& z);                // 观测更新（§4.5）
+    bool Initialized() const;
+    const std::array<double, 8>& State() const;     // [cx,cy,w,h,vx,vy,vw,vh]
+};
+
+struct Tracklet {
+    Tracklet(int id, const BoxCXCYWH& measurement, double conf, int cls,
+             int max_lost_frames);
+    bool IsDead() const;      // lost_count > max_lost（严格大于）
+    bool IsCoasting() const;  // lost_count > 0
+    void Predict();           // kf.predict 非空时更新 last_center/last_box、age+1
+    void Update(const BoxXYXY& box, double conf, int cls);  // 匹配更新
+    void MarkMiss();          // lost+1、age+1
+    // 字段：track_id / kf / lost_count / max_lost / confidence / cls_id /
+    //       age / hits / last_center / last_box / last_raw_box
+};
+
+class TargetTracker {
+    struct Config {  // §8.1 默认值
+        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;
+    };
+    TargetTracker();                               // 默认配置
+    explicit TargetTracker(const Config& cfg);
+    TrackResult Update(const std::vector<Detection>& detections,
+                       std::optional<FrameShape> frame_shape = std::nullopt);
+    TrackResult LastResult() const;                // 线程安全拷贝
+    void Reset();                                  // 清全部状态且 next_id 归零
+    int ActiveCount() const;
+    std::unordered_map<int, Tracklet> ActiveTracks() const;  // 只读快照
+};
+```
+
+数据流：调用方每帧将检测列表（像素坐标 `[x1,y1,x2,y2,conf,cls_id]`）传入
+`Update()`，得到 `TrackResult`（九字段：tracked / primary_id / center / box /
+confidence / is_predicted / lost_frames / n_active / raw）。丢失时
+`tracked=false`、定位字段空、`lost_frames=0`（技术路线 §2.3）。
+
+## 三、关键实现点
+
+### 3.1 卡尔曼滤波（对照技术路线 §4）
+
+- 状态 `x[8]=[cx,cy,w,h,vx,vy,vw,vh]`，观测 `z[4]=[cx,cy,w,h]`，dt=1 帧。
+- 常量矩阵：`F`（8×8，I4 + 右上 I4）、`H`（4×8，[I4|0]）为 `constexpr`；
+  矩阵乘法/转置用 `std::array` 定长手写，全程 `double`（§9.3）。
+- 常量：`kStdPos=1/50`、`kStdVel=1/200`、`kIniPos=2.0`、`kIniVel=100.0`、
+  `kRScale=2.0`（§4.2/§8.2）。
+- **动态噪声**：`Predict()` 时 Q=diag(σ_pos²,σ_vel²)，σ 直接用状态中的 w/h
+  缩放；`Update()` 时 R 用 `abs(w)/abs(h)` 缩放并乘 2——两处差异与规格
+  §4.2 注记严格保持一致。
+- `Initiate()`：速度置零，P=diag(2,2,2,2,100,100,100,100)（§4.3）。
+- `Update()` 中 S=HPHᵀ+R 为 4×4，用**高斯-约当消元 + 部分主元**求逆，
+  消元前对角加 1e-9 正则化防奇异（§9.3）；随后 K=PHᵀS⁻¹、
+  x←x+K(z−Hx)、P←(I−KH)P（§4.5）。
+
+### 3.2 贪婪关联（对照 §5）
+
+- `cost = dist_weight·dist/max(Dmax,1e-3) + iou_weight·(1−IoU)`，
+  距离用 `std::hypot`；每条轨道 `best_cost` 初始化为 1.0，**仅 cost 严格
+  小于才匹配**（cost=1.0 恰好不匹配，§5.1）。
+- 按轨道 confidence 降序（平局按 ID 升序保证确定性）逐一贪心匹配；
+  排序时复制 ID 到 `std::vector` 后 `std::sort`，不直接排哈希表（§9.2）。
+
+### 3.3 主目标选举三级回退（对照 §6.3）
+
+1. **锁定延续优先**：`locked_primary_id` 存活则直接输出（Coast 中照样输出
+   `is_predicted=true`），不被高置信度目标抢占；
+2. **重新选举**（无锁定/锁定已死亡）：measured（lost==0）中 hits≥min_hits
+   的已确认池取 confidence 最大；池空则退回全部 measured；
+3. 无 measured 时从 coasting 中选离画面中心最近者（欧氏距离平方，
+   `frame_shape` 缺省按 640×480 中心 (320,240)），`is_predicted=true`；
+   都没有则 `tracked=false`。
+
+锁定仅在轨道死亡删除处或 `Reset()` 时解除。
+
+### 3.4 主流程六步（对照 §7）
+
+`Update()` 严格按：①全体轨道 predict ②检测中心表 + 贪婪关联 ③匹配轨道
+update（raw_box 由检测重建）④未匹配轨道 mark_miss、死亡即删、若为锁定
+目标同时清锁 ⑤未匹配检测新建轨道（创建当帧赋 last_box/last_raw_box，
+保证当帧可参与输出；next_id 从 0 递增永不复用）⑥主目标选举组装输出并
+缓存 last_result。
+
+### 3.5 线程模型（对照 §9.5）
+
+单线程写 `Update()`（推理主循环）；库内仅对 `last_result` 缓存加一把
+`std::mutex`，`LastResult()` 返回受保护拷贝。内部轨道状态不加锁；
+若未来改为多线程调用 `Update()` 需整体加锁。
+
+### 3.6 符号导出说明
+
+当前不加 `-fvisibility=hidden`、不使用导出宏，共享库默认导出全部公共符号。
+日后若启用隐藏可见性（如 `CXX_VISIBILITY_PRESET hidden`），需补
+`TARGET_TRACKER_API` 宏（导出/导入双态）并标注所有公共类与函数，
+否则下游链接将缺失符号。
+
+## 四、日志行为
+
+本库为**纯算法层，不打任何日志**（不依赖 spdlog）。若未来接入主工程，
+接线层（订阅/发布适配部件）按主工程日志纪律负责关键路径日志与节流，
+本库内部不新增日志。
+
+## 五、测试方式
+
+构建环境：WSL2 Ubuntu（AGENTS.md 规定的开发机构建环境）。
+
+```bash
+cd /mnt/d/ProgramData/drone_test/drone_test
+cmake -S target_tracker -B target_tracker/build
+cmake --build target_tracker/build -j$(nproc)
+ctest --test-dir target_tracker/build --output-on-failure
+```
+
+测试共 27 条（kalman_box_filter_test 5 条 / tracklet_test 12 条 /
+target_tracker_test 10 条），其中 `target_tracker_test.cpp` 逐条覆盖
+技术路线 §9.4 六条验收用例：
+
+1. `EmptyFramesCoastThenLost`：空检测帧 Coast 持续 max_lost_frames 帧
+   （is_predicted=true），第 max_lost_frames+1 帧 tracked=false；
+2. `IdStrictlyIncreasingNoReuse`：ID 严格递增不复用，Reset 后归零；
+3. `LockNotStolenAndSwitchAfterDeath`：Coast 期间高置信度新目标不抢占锁定，
+   锁定目标死亡后切换；
+4. `MinHitsFallbackAllowsNewTrack`：无已确认实测轨道时新轨道（hits<min_hits）
+   可当选主目标；
+5. `CostExactlyOneDoesNotMatch`：纯距离项构造 cost=1.0 恰好不匹配，
+   并附 cost<1.0 对照帧；
+6. `HigherConfidenceTrackWinsDetection`：两轨道竞争同一检测，置信度高者获胜。
+
+其余为补充行为测试（丢失输出字段约定、Coast 兜底选举、锁定延续、
+默认画面中心、IoU 边界、滤波器收敛性等）。
+
+## 六、排查 / 修改要点
+
+- **主目标"甩头"**：确认是否误改锁定解除逻辑——锁定只能在轨道死亡删除处
+  或 `Reset()` 解除；选举分支不得覆盖存活锁定。
+- **Coast 时间不对**：死亡判定为 `lost_count > max_lost`（严格大于），
+  默认允许 Coast 8 帧、第 9 帧判丢；改成 `>=` 属于行为变更。
+- **关联不上 / 乱匹配**：检查 `max_association_dist` 与目标帧间位移是否
+  匹配；cost 判据为严格小于 1.0；dist/iou 权重之和建议保持 1.0。
+- **滤波发散或跳变**：检查 Q/R 动态噪声是否按状态 w/h 缩放（R 用 abs 且
+  乘 2、Q 不用 abs，该差异是规格要求）；4×4 求逆已加 1e-9 正则化。
+- **测试链接失败（找不到 so）**：测试可执行文件通过 `CMAKE_BUILD_RPATH`
+  定位库；若把测试挪到别处运行需设置 `LD_LIBRARY_PATH`。
+- 修改任何行为后必须同步更新本文档与 `docs/视觉追踪技术路线.md` 的对照注释。
+
+## 七、未来接线指引（接入主工程视频链路）
+
+本库以独立动态库交付，主工程接入建议采用：
+
+1. **订阅解码帧主题作为每帧节拍**：接线部件订阅解码帧 Topic，以解码帧
+   到达为帧节拍驱动追踪（保证与推理节奏一致）；
+2. **按 frame_sequence 聚合 DetectionResult**：YOLO 每目标发布一条
+   DetectionResult（同帧共享 frame_sequence），接线部件按 frame_sequence
+   聚合成 `std::vector<Detection>` 后调用 `Update()`；
+3. **主工程 `include/common/types.h` 新增 `TrackedTarget` 消息与
+   `kTrackedTarget` 主题**：由接线部件把 `TrackResult` 转换为
+   `TrackedTarget` 发布，供控制线程（20Hz PID）订阅消费；Coast
+   （is_predicted=true）时调用方清 PID 积分、tracked=false 时零速刹车
+   （技术路线 §1.2 上游行为约定）。
+
+主工程链接该 so 的 CMake 片段示例：
+
+```cmake
+# 主工程 CMakeLists.txt 中引入视觉追踪库（独立子工程）
+add_subdirectory(target_tracker)
+
+target_link_libraries(drone_control PRIVATE target_tracker)
+# 部署时需将 libtarget_tracker.so.1 一并打包到可执行文件目录
+# （或设置 INSTALL_RPATH=$ORIGIN，与 docs/部署与打包.md 目录式部署约定一致）
+```