218 lines
11 KiB
Markdown
218 lines
11 KiB
Markdown
# 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 目录式部署约定一致）
|
||
```
|