Files

T

2026-09-02 15:13:13 +08:00

11 KiB

Raw Blame History

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`

主要接口签名：

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）

锁定延续优先：locked_primary_id 存活则直接输出（Coast 中照样输出 is_predicted=true），不被高置信度目标抢占；
重新选举（无锁定/锁定已死亡）：measured（lost==0）中 hits≥min_hits 的已确认池取 confidence 最大；池空则退回全部 measured；
无 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 规定的开发机构建环境）。

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 六条验收用例：

EmptyFramesCoastThenLost：空检测帧 Coast 持续 max_lost_frames 帧（is_predicted=true），第 max_lost_frames+1 帧 tracked=false；
IdStrictlyIncreasingNoReuse：ID 严格递增不复用，Reset 后归零；
LockNotStolenAndSwitchAfterDeath：Coast 期间高置信度新目标不抢占锁定，锁定目标死亡后切换；
MinHitsFallbackAllowsNewTrack：无已确认实测轨道时新轨道（hits<min_hits）可当选主目标；
CostExactlyOneDoesNotMatch：纯距离项构造 cost=1.0 恰好不匹配，并附 cost<1.0 对照帧；
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 的对照注释。

七、未来接线指引（接入主工程视频链路）

本库以独立动态库交付，主工程接入建议采用：

订阅解码帧主题作为每帧节拍：接线部件订阅解码帧 Topic，以解码帧到达为帧节拍驱动追踪（保证与推理节奏一致）；
按 frame_sequence 聚合 DetectionResult：YOLO 每目标发布一条 DetectionResult（同帧共享 frame_sequence），接线部件按 frame_sequence 聚合成 std::vector<Detection> 后调用 Update()；
主工程 include/common/types.h 新增 TrackedTarget 消息与 kTrackedTarget 主题：由接线部件把 TrackResult 转换为 TrackedTarget 发布，供控制线程（20Hz PID）订阅消费；Coast （is_predicted=true）时调用方清 PID 积分、tracked=false 时零速刹车（技术路线 §1.2 上游行为约定）。

主工程链接该 so 的 CMake 片段示例：

# 主工程 CMakeLists.txt 中引入视觉追踪库（独立子工程）
add_subdirectory(target_tracker)

target_link_libraries(drone_control PRIVATE target_tracker)
# 部署时需将 libtarget_tracker.so.1 一并打包到可执行文件目录
# （或设置 INSTALL_RPATH=$ORIGIN，与 docs/部署与打包.md 目录式部署约定一致）

11 KiB Raw Blame History Unescape Escape