songhaoyu/code-test-drone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
songhaoyu
2026-09-02 15:13:13 +08:00
commit cb00322a50
107 changed files with 10918 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
reference-c++/target_tracker/CMakeLists.txt
+85
View File
@@ -0,0 +1,85 @@
cmake_minimum_required(VERSION 3.16)
# =============================================================================
# target_tracker —— 视觉追踪库（独立动态库工程）
#
# 轻量级多目标追踪过滤器：恒定速度卡尔曼滤波 + 贪婪关联 + 主目标锁定。
# 规格见 docs/视觉追踪技术路线.md；实现文档见 target_tracker.md。
# 零第三方依赖（不引入 spdlog / Eigen / OpenCV），仅链接 C++ 标准库。
# =============================================================================
project(target_tracker VERSION 1.0.0 LANGUAGES CXX)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
# ---- 动态库目标 ----
add_library(target_tracker SHARED
src/kalman_box_filter.cpp
src/tracklet.cpp
src/target_tracker.cpp)
target_include_directories(target_tracker PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
target_compile_features(target_tracker PUBLIC cxx_std_17)
# 版本与符号：VERSION 1.0.0 / SOVERSION 1；默认符号可见性（不加
# visibility=hidden 与导出宏），全部公共类直接对外导出。
set_target_properties(target_tracker PROPERTIES
VERSION 1.0.0
SOVERSION 1)
# 编译告警：与主工程一致（非 MSVC：-Wall -Wextra -Wpedantic）
if(MSVC)
target_compile_options(target_tracker PRIVATE /W4 /permissive-)
else()
target_compile_options(target_tracker PRIVATE -Wall -Wextra -Wpedantic)
endif()
# ---- 单元测试（GTest；缺失时仅提示不失败，与主工程策略一致） ----
include(CTest)
if(BUILD_TESTING)
find_package(GTest QUIET)
if(GTest_FOUND)
include(GoogleTest)
# 测试可执行文件直接链接共享库 target_tracker；
# BUILD_RPATH 指向库输出目录，保证 ctest 无需 LD_LIBRARY_PATH 即可运行
set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR};$ORIGIN")
# 卡尔曼滤波器测试：初始化状态、静止收敛、匀速速度收敛与预测超前、未初始化行为
add_executable(kalman_box_filter_test tests/kalman_box_filter_test.cpp)
target_link_libraries(kalman_box_filter_test PRIVATE
target_tracker GTest::gtest_main)
if(MSVC)
target_compile_options(kalman_box_filter_test PRIVATE /W4 /permissive-)
else()
target_compile_options(kalman_box_filter_test PRIVATE -Wall -Wextra -Wpedantic)
endif()
gtest_discover_tests(kalman_box_filter_test)
# 轨道与几何工具测试：IoU 边界、IsDead/IsCoasting、Predict/Update/MarkMiss 计数
add_executable(tracklet_test tests/tracklet_test.cpp)
target_link_libraries(tracklet_test PRIVATE
target_tracker GTest::gtest_main)
if(MSVC)
target_compile_options(tracklet_test PRIVATE /W4 /permissive-)
else()
target_compile_options(tracklet_test PRIVATE -Wall -Wextra -Wpedantic)
endif()
gtest_discover_tests(tracklet_test)
# 追踪器测试：逐条覆盖文档 §9.4 六条行为一致性验收用例
add_executable(target_tracker_test tests/target_tracker_test.cpp)
target_link_libraries(target_tracker_test PRIVATE
target_tracker GTest::gtest_main)
if(MSVC)
target_compile_options(target_tracker_test PRIVATE /W4 /permissive-)
else()
target_compile_options(target_tracker_test PRIVATE -Wall -Wextra -Wpedantic)
endif()
gtest_discover_tests(target_tracker_test)
else()
message(STATUS "GoogleTest not found; target_tracker tests will not be built")
endif()
endif()
reference-c++/target_tracker/include/target_tracker/kalman_box_filter.h
+87
View File
@@ -0,0 +1,87 @@
#pragma once
// =============================================================================
// kalman_box_filter.h —— 单目标 8 维恒定速度卡尔曼滤波器
//
// 对应规格：docs/视觉追踪技术路线.md §4（卡尔曼滤波算法细节）、§8.2（内部常量）
// 状态向量 x[8] = [cx, cy, w, h, vx, vy, vw, vh]（§4.1）
// 观测向量 z[4] = [cx, cy, w, h]（§4.1）
// 时间步长 dt = 1 帧，与帧率无关（§1.3）
//
// 实现约束（§9.3）：矩阵规模极小（8×8 / 4×8 / 4×4），手写定长矩阵运算，
// 全程 double 精度，不引入 Eigen/OpenCV 等线性代数库。
// =============================================================================
#include <array>
#include <optional>
namespace drone {
namespace tracker {
/// 轴对齐边界框（中心点 + 宽高表示，卡尔曼观测空间）
struct BoxCXCYWH {
double cx = 0.0; // 框中心 x（像素）
double cy = 0.0; // 框中心 y（像素）
double w = 0.0; // 框宽（像素）
double h = 0.0; // 框高（像素）
};
/// 单目标恒定速度卡尔曼滤波器（§3.1 / §4）
class KalmanBoxFilter {
public:
KalmanBoxFilter() = default;
/// 是否已用首次观测初始化（§3.1 initialized 字段）
bool Initialized() const { return initialized_; }
/// 用首次观测 [cx, cy, w, h] 初始化（§4.3）：
/// 状态速度置零；P = diag(2,2,2,2,100,100,100,100)。
void Initiate(const BoxCXCYWH& z);
/// 预测推进一帧（§4.4）：未初始化返回 std::nullopt；
/// 否则 x ← F·x，P ← F·P·Fᵀ + Q，返回状态前 4 维 [cx,cy,w,h] 副本。
std::optional<BoxCXCYWH> Predict();
/// 观测更新（§4.5）：未初始化时直接返回；
/// S = H·P·Hᵀ + R，K = P·Hᵀ·S⁻¹，x ← x + K(z−Hx)，P ← (I−KH)P。
void Update(const BoxCXCYWH& z);
/// 状态向量只读访问（[cx, cy, w, h, vx, vy, vw, vh]，测试与排查用）
const std::array<double, 8>& State() const { return x_; }
// ---- 内部常量（§4.2 / §8.2，经验值，针对 640×480 画面调优） ----
static constexpr double kStdPos = 1.0 / 50.0; // STD_POS：位置噪声比例系数
static constexpr double kStdVel = 1.0 / 200.0; // STD_VEL：速度噪声比例系数
static constexpr double kIniPos = 2.0; // INI_POS：初始位置协方差
static constexpr double kIniVel = 100.0; // INI_VEL：初始速度协方差
static constexpr double kRScale = 2.0; // R 额外缩放因子（增强平滑，§8.2）
private:
std::array<double, 8> x_{}; // 状态向量（§3.1）
std::array<std::array<double, 8>, 8> P_{}; // 状态协方差 8×8（§3.1）
bool initialized_ = false; // 是否已初始化（§3.1）
// 状态转移矩阵 F（常量，§4.1）：I4 + 右上 I4，即 F[i][i]=1、F[i][i+4]=1
static constexpr std::array<std::array<double, 8>, 8> kF = {{
{{1, 0, 0, 0, 1, 0, 0, 0}},
{{0, 1, 0, 0, 0, 1, 0, 0}},
{{0, 0, 1, 0, 0, 0, 1, 0}},
{{0, 0, 0, 1, 0, 0, 0, 1}},
{{0, 0, 0, 0, 1, 0, 0, 0}},
{{0, 0, 0, 0, 0, 1, 0, 0}},
{{0, 0, 0, 0, 0, 0, 1, 0}},
{{0, 0, 0, 0, 0, 0, 0, 1}},
}};
// 观测矩阵 H（常量，§4.1）：[I4 | 0]（4 行 × 8 列），
// 直接观测位置与尺寸、不观测速度
static constexpr std::array<std::array<double, 8>, 4> kH = {{
{{1, 0, 0, 0, 0, 0, 0, 0}},
{{0, 1, 0, 0, 0, 0, 0, 0}},
{{0, 0, 1, 0, 0, 0, 0, 0}},
{{0, 0, 0, 1, 0, 0, 0, 0}},
}};
};
} // namespace tracker
} // namespace drone
reference-c++/target_tracker/include/target_tracker/target_tracker.h
+123
View File
@@ -0,0 +1,123 @@
#pragma once
// =============================================================================
// target_tracker.h —— 多目标追踪过滤器（TargetTracker）对外接口
//
// 对应规格：docs/视觉追踪技术路线.md
// §2 输入输出接口定义（Detection / TrackResult）
// §3.3 TargetTracker（管理器）
// §5 检测-轨迹关联（贪婪最邻近，§5.3）
// §6 轨迹生命周期与 ID 管理（主目标锁定与选举，§6.3）
// §7 主流程（每帧 Update 六步）
//
// 线程模型（§9.5）：Update() 由单线程（推理主循环）调用；库内仅对
// last_result 缓存加一把 std::mutex，LastResult() 返回受保护拷贝。
// 本库零第三方依赖（不依赖 spdlog / Eigen / OpenCV）。
// =============================================================================
#include <mutex>
#include <optional>
#include <unordered_map>
#include <utility>
#include <vector>
#include "tracklet.h"
namespace drone {
namespace tracker {
/// 单个检测框（§2.2 输入 detections 元素：[x1,y1,x2,y2,conf,cls_id]）
struct Detection {
double x1 = 0.0; // 边界框左上角 x（像素坐标）
double y1 = 0.0; // 边界框左上角 y
double x2 = 0.0; // 边界框右下角 x
double y2 = 0.0; // 边界框右下角 y
double conf = 0.0; // 检测置信度 ∈ [0,1]
int cls_id = 0; // 类别编号
};
/// 画面尺寸（height, width，§2.2）：仅在"无实测轨道、只有 Coast 轨道"
/// 时用于选取离画面中心最近的轨道；缺省按 640×480 中心 (320,240)。
struct FrameShape {
int height = 0;
int width = 0;
};
/// 每帧追踪输出（§2.3）
struct TrackResult {
bool tracked = false; // 是否存在有效主目标；false 时调用方应刹车
std::optional<int> primary_id; // 主目标轨道 ID；无目标时为空
std::pair<double, double> center{0.0, 0.0}; // 平滑后的目标中心点（tracked=false 时无效）
std::optional<BoxXYXY> box; // 平滑后的边界框
double confidence = 0.0; // 当前置信度（Coast 期间继承最近实测值）
bool is_predicted = false; // true = Coast（仅预测、无实测）
int lost_frames = 0; // 主目标连续未命中帧数
int n_active = 0; // 当前活跃轨道总数
std::optional<BoxXYXY> raw; // 关联到的原始检测框；Coast 时为空（§2.3）
};
/// 多目标追踪过滤器（§1.1 / §3.3）
class TargetTracker {
public:
/// 追踪器配置参数（§8.1，默认值取自规格）
struct Config {
int max_lost_frames = 8; // 最大连续丢失帧数（严格大于才判死，§6.1）
double max_association_dist = 200.0; // 中心距离软阈值 Dmax（像素，§5.1）
int min_hits = 3; // 轨道"确认"所需累计匹配次数（§6.1）
double dist_weight = 0.5; // 关联代价距离项权重（§5.1）
double iou_weight = 0.5; // 关联代价 (1−IoU) 项权重（§5.1）
};
/// 默认配置构造（等价于 Config{}，§8.1 默认值）
TargetTracker();
/// 指定配置构造
explicit TargetTracker(const Config& cfg);
/// 每帧调用一次（§7 六步主流程）：输入检测列表与可选画面尺寸，
/// 输出主目标追踪结果并缓存（供 LastResult 跨线程读取）。
TrackResult Update(const std::vector<Detection>& detections,
std::optional<FrameShape> frame_shape = std::nullopt);
/// 上一帧结果快照（线程安全：返回受互斥锁保护的拷贝，§9.5）
TrackResult LastResult() const;
/// 清空所有轨道、ID 计数器、锁定状态与缓存结果（§2.1）；next_id 归零（§6.2）
void Reset();
/// 当前存活轨道数量（§2.1）
int ActiveCount() const;
/// 当前所有轨道的只读快照（§2.1；值拷贝）
std::unordered_map<int, Tracklet> ActiveTracks() const;
/// 配置只读访问
const Config& GetConfig() const { return config_; }
private:
/// 贪婪关联结果（§5.3）：matches 元素为 (track_id, 检测索引)
struct AssociationResult {
std::vector<std::pair<int, int>> matches;
std::vector<int> unmatched_track_ids;
std::vector<int> unmatched_det_indices;
};
/// 贪婪最邻近关联（§5.3）：按轨道 confidence 降序逐一匹配，
/// cost = w_dist·dist/max(Dmax,1e-3) + w_iou·(1−IoU)，仅 cost 严格小于
/// best_cost（初始 1.0）才匹配，即 cost ≥ 1.0 判为不可匹配（§5.1）。
AssociationResult AssociateDetections(const std::vector<Detection>& detections);
/// 主目标选择/锁定延续（§6.3 SELECT_PRIMARY）
TrackResult SelectPrimary(std::optional<FrameShape> frame_shape);
Config config_; // 配置参数（§3.3）
int next_id_ = 0; // 下一个可分配 ID，从 0 递增不复用（§6.2）
std::unordered_map<int, Tracklet> tracks_; // 所有存活轨道（§3.3）
long long frame_count_ = 0; // 已处理帧数（仅统计用，§3.3）
std::optional<int> locked_primary_id_; // 当前锁定的主目标 ID（§3.3）
TrackResult last_result_; // 上一帧结果缓存（§3.3）
mutable std::mutex last_result_mutex_; // 仅保护 last_result_（§9.5）
};
} // namespace tracker
} // namespace drone
reference-c++/target_tracker/include/target_tracker/tracklet.h
+93
View File
@@ -0,0 +1,93 @@
#pragma once
// =============================================================================
// tracklet.h —— 轨道（Tracklet）与基础几何工具
//
// 对应规格：docs/视觉追踪技术路线.md
// §3.2 Tracklet（轨道）字段定义与派生状态
// §5.2 IoU 计算
// §7 主流程伪代码末尾 TRACKLET_PREDICT / TRACKLET_UPDATE
//
// 几何量说明：BoxCXCYWH（卡尔曼观测空间 [cx,cy,w,h]）定义于
// kalman_box_filter.h，本文件通过 include 复用并同时提供 BoxXYXY
// （像素坐标 [x1,y1,x2,y2]）与两种表示的互转函数。
// 本库只依赖 C++ 标准库，无任何第三方依赖。
// =============================================================================
#include <optional>
#include <utility>
#include "kalman_box_filter.h"
namespace drone {
namespace tracker {
// ---- 基础几何量（§9.1 模块/类划分草案） ----
/// 轴对齐边界框（像素坐标，左上/右下角表示）
struct BoxXYXY {
double x1 = 0.0;
double y1 = 0.0;
double x2 = 0.0;
double y2 = 0.0;
};
/// [x1,y1,x2,y2] → [cx,cy,w,h]（§7 TRACKLET_UPDATE 中的转换）
BoxCXCYWH ToCenterSize(const BoxXYXY& box);
/// [cx,cy,w,h] → [x1,y1,x2,y2]（§7 TRACKLET_PREDICT 中的 center_size 逆变换）
BoxXYXY ToXYXY(const BoxCXCYWH& box);
/// 标准轴对齐 IoU（§5.2）：
/// 任一框为空、交集面积 ≤ 0 或并集 ≤ 0 时返回 0。
double ComputeIoU(const BoxXYXY& a, const BoxXYXY& b);
// =============================================================================
// Tracklet —— 单条轨道：生命周期计数 + 辅助状态 + 卡尔曼滤波器（§3.2）
// =============================================================================
struct Tracklet {
/// 构造即创建轨道（§7 步骤 5 / §6.1 新建阶段）：
/// 用首次观测初始化滤波器，age=1、hits=1、lost_count=0，
/// last_center / last_box 立即由观测重建（保证创建当帧即可参与输出）。
Tracklet(int id, const BoxCXCYWH& measurement, double conf, int cls,
int max_lost_frames);
int track_id = 0; // 全局唯一轨道 ID（§3.2）
KalmanBoxFilter kf; // 本轨道的滤波器（§3.2）
int lost_count = 0; // 当前连续未匹配帧数（§3.2）
int max_lost = 0; // 最大允许连续丢失帧数（构造时注入，§3.2）
double confidence = 0.0; // 最近一次匹配的置信度（§3.2）
int cls_id = 0; // 最近一次匹配的类别 ID（§3.2）
int age = 1; // 轨道已存在的总帧数（创建时为 1，§3.2）
int hits = 1; // 累计成功匹配次数（创建时为 1，§3.2）
std::pair<double, double> last_center{0.0, 0.0}; // 最近一次中心点（实测或预测）
std::optional<BoxXYXY> last_box; // 最近一次边界框（实测或预测，§3.2）
std::optional<BoxXYXY> last_raw_box; // 最近一次实测关联到的原始检测框；
// Coast 期间保留不更新，仅内部参考；
// 对外输出语义按 §2.3：Coast 时 raw 置空
// ---- 派生状态（§3.2 只读属性） ----
/// is_dead := lost_count > max_lost（严格大于，§6.1 注记 2）
bool IsDead() const { return lost_count > max_lost; }
/// is_coasting := lost_count > 0（处于纯预测状态，§3.2）
bool IsCoasting() const { return lost_count > 0; }
// ---- 生命周期方法（§7 末尾伪代码） ----
/// TRACKLET_PREDICT：卡尔曼预测推进一帧。
/// 预测成功时更新 last_center / last_box 且 age+1。
void Predict();
/// TRACKLET_UPDATE：框 [x1,y1,x2,y2] → [cx,cy,w,h] → kf.update；
/// lost_count 清零、hits+1、age+1，更新
/// last_center / last_box / last_raw_box / confidence / cls_id。
void Update(const BoxXYXY& box, double conf, int cls);
/// 未匹配计丢失：lost_count+1、age+1（§7 步骤 4）。
void MarkMiss();
};
} // namespace tracker
} // namespace drone
reference-c++/target_tracker/src/kalman_box_filter.cpp
+235
View File
@@ -0,0 +1,235 @@
// =============================================================================
// kalman_box_filter.cpp —— 单目标 8 维恒定速度卡尔曼滤波器实现
//
// 对应规格：docs/视觉追踪技术路线.md §4（算法细节）、§8.2（内部常量）、
// §9.3（数值计算：定长数组手写矩阵乘法，4×4 高斯-约当求逆 + 1e-9 正则化）。
// 关键公式逐行注释并标注对照文档小节号。
// =============================================================================
#include "target_tracker/kalman_box_filter.h"
#include <cmath>
#include <utility>
namespace drone {
namespace tracker {
namespace {
// ---- 定长矩阵基础运算（§9.3：无需引入 Eigen/OpenCV） ----
/// 8×8 矩阵乘法：C = A·B
std::array<std::array<double, 8>, 8> MatMul8(const std::array<std::array<double, 8>, 8>& a,
const std::array<std::array<double, 8>, 8>& b) {
std::array<std::array<double, 8>, 8> c{};
for (int i = 0; i < 8; ++i) {
for (int k = 0; k < 8; ++k) {
if (a[i][k] == 0.0) continue; // 稀疏常量矩阵加速，结果不变
for (int j = 0; j < 8; ++j) {
c[i][j] += a[i][k] * b[k][j];
}
}
}
return c;
}
/// 8×8 矩阵转置：R = Aᵀ
std::array<std::array<double, 8>, 8> MatTranspose8(
const std::array<std::array<double, 8>, 8>& a) {
std::array<std::array<double, 8>, 8> r{};
for (int i = 0; i < 8; ++i) {
for (int j = 0; j < 8; ++j) {
r[i][j] = a[j][i];
}
}
return r;
}
/// 4×4 矩阵求逆：高斯-约当消元 + 部分主元（§9.3）。
/// 输入矩阵先对角加 1e-9 正则化防奇异（§9.3 建议）。
/// 返回的 bool 表示是否求逆成功（主元退化时返回 false）。
bool Invert4x4(const std::array<std::array<double, 4>, 4>& m,
std::array<std::array<double, 4>, 4>& out) {
constexpr double kRegularization = 1e-9; // 正则化极小量（§9.3）
// 增广矩阵 [M + εI | I]，消元后左半变为单位阵、右半即为逆矩阵
std::array<std::array<double, 8>, 4> aug{};
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < 4; ++j) {
aug[i][j] = m[i][j] + (i == j ? kRegularization : 0.0);
aug[i][j + 4] = (i == j) ? 1.0 : 0.0;
}
}
for (int col = 0; col < 4; ++col) {
// 部分主元：选取当前列绝对值最大行交换，提高数值稳定性
int pivot = col;
for (int r = col + 1; r < 4; ++r) {
if (std::abs(aug[r][col]) > std::abs(aug[pivot][col])) {
pivot = r;
}
}
if (std::abs(aug[pivot][col]) < 1e-12) {
return false; // 主元退化（正则化后理论上不可达）
}
if (pivot != col) {
std::swap(aug[pivot], aug[col]);
}
// 主元行归一化
const double d = aug[col][col];
for (int j = 0; j < 8; ++j) {
aug[col][j] /= d;
}
// 消去其余行（高斯-约当：一次消成对角单位阵）
for (int r = 0; r < 4; ++r) {
if (r == col) continue;
const double f = aug[r][col];
if (f == 0.0) continue;
for (int j = 0; j < 8; ++j) {
aug[r][j] -= f * aug[col][j];
}
}
}
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < 4; ++j) {
out[i][j] = aug[i][j + 4];
}
}
return true;
}
} // namespace
void KalmanBoxFilter::Initiate(const BoxCXCYWH& z) {
// §4.3 初始化：状态 = [cx, cy, w, h, 0, 0, 0, 0]（速度置零）
x_ = {{z.cx, z.cy, z.w, z.h, 0.0, 0.0, 0.0, 0.0}};
// §4.3 协方差：P = diag(2,2,2,2,100,100,100,100)
// （位置置信度中等，速度置信度低，允许前几帧快速收敛速度估计）
for (auto& row : P_) row.fill(0.0);
for (int i = 0; i < 4; ++i) {
P_[i][i] = kIniPos; // INI_POS = 2.0（§4.2）
P_[i + 4][i + 4] = kIniVel; // INI_VEL = 100.0（§4.2）
}
initialized_ = true;
}
std::optional<BoxCXCYWH> KalmanBoxFilter::Predict() {
// §4.4：未初始化返回空
if (!initialized_) {
return std::nullopt;
}
// §4.2 动态过程噪声：Q = diag(σ_pos², σ_vel²)，σ 按状态中 w/h 缩放
// σ_pos = [w/50, h/50, w/50, h/50]
// σ_vel = [w/200, h/200, w/200, h/200]
// （Q 直接使用状态值，不做 abs，与规格 §4.2 注记保持一致）
const double w = x_[2];
const double h = x_[3];
const double std_pos[4] = {w * kStdPos, h * kStdPos, w * kStdPos, h * kStdPos};
const double std_vel[4] = {w * kStdVel, h * kStdVel, w * kStdVel, h * kStdVel};
std::array<std::array<double, 8>, 8> q{};
for (int i = 0; i < 4; ++i) {
q[i][i] = std_pos[i] * std_pos[i]; // 前 4 对角元来自 σ_pos（各分量先平方）
q[i + 4][i + 4] = std_vel[i] * std_vel[i]; // 后 4 对角元来自 σ_vel
}
// §4.4 状态转移：x ← F·x（恒定速度模型：位置/尺寸各自加上对应速度）
std::array<double, 8> x_new{};
for (int i = 0; i < 8; ++i) {
double sum = 0.0;
for (int j = 0; j < 8; ++j) {
sum += kF[i][j] * x_[j];
}
x_new[i] = sum;
}
x_ = x_new;
// §4.4 协方差传播：P ← F·P·Fᵀ + Q
P_ = MatMul8(MatMul8(kF, P_), MatTranspose8(kF));
for (int i = 0; i < 8; ++i) {
for (int j = 0; j < 8; ++j) {
P_[i][j] += q[i][j];
}
}
// §4.4 返回预测的位置/尺寸（状态向量前 4 维的副本）
return BoxCXCYWH{x_[0], x_[1], x_[2], x_[3]};
}
void KalmanBoxFilter::Update(const BoxCXCYWH& z) {
// §4.5：未初始化直接返回
if (!initialized_) {
return;
}
// §4.2 动态观测噪声：R = diag(2(w/50)², 2(h/50)², 2(w/50)², 2(h/50)²)
// （R 使用 abs(w)/abs(h) 防御负尺寸，并额外乘 2 增强平滑；
// 该差异与规格 §4.2 注记保持一致）
const double w = std::abs(x_[2]);
const double h = std::abs(x_[3]);
std::array<std::array<double, 4>, 4> r{};
const double rw = kRScale * (w * kStdPos) * (w * kStdPos);
const double rh = kRScale * (h * kStdPos) * (h * kStdPos);
r[0][0] = rw;
r[1][1] = rh;
r[2][2] = rw;
r[3][3] = rh;
// §4.5 新息协方差：S = H·P·Hᵀ + R（4×4）
// H = [I4 | 0]，故 H·P·Hᵀ 即 P 的左上 4×4 子块
std::array<std::array<double, 4>, 4> s{};
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < 4; ++j) {
s[i][j] = P_[i][j] + r[i][j];
}
}
// §4.5 / §9.3：S⁻¹（4×4 高斯-约当求逆 + 对角加 1e-9 正则化防奇异）
std::array<std::array<double, 4>, 4> s_inv{};
if (!Invert4x4(s, s_inv)) {
return; // 正则化后理论不可达；防御性跳过本次更新
}
// §4.5 卡尔曼增益：K = P·Hᵀ·S⁻¹（8×4）
// H = [I4 | 0]，故 P·Hᵀ 即 P 的前 4 列（8×4）
std::array<std::array<double, 4>, 8> k{};
for (int i = 0; i < 8; ++i) {
for (int j = 0; j < 4; ++j) {
double sum = 0.0;
for (int l = 0; l < 4; ++l) {
sum += P_[i][l] * s_inv[l][j];
}
k[i][j] = sum;
}
}
// §4.5 残差（innovation）：y = z − H·x（H·x 即状态前 4 维）
const double z_vec[4] = {z.cx, z.cy, z.w, z.h};
double y[4];
for (int i = 0; i < 4; ++i) {
y[i] = z_vec[i] - x_[i];
}
// §4.5 状态更新：x ← x + K·y
for (int i = 0; i < 8; ++i) {
double sum = 0.0;
for (int j = 0; j < 4; ++j) {
sum += k[i][j] * y[j];
}
x_[i] += sum;
}
// §4.5 协方差更新：P ← (I8 − K·H)·P
// K·H（8×8）= K 的前 4 列铺到前 4 列上（H 选通前 4 维）
std::array<std::array<double, 8>, 8> ikh{};
for (int i = 0; i < 8; ++i) {
for (int j = 0; j < 8; ++j) {
ikh[i][j] = (i == j ? 1.0 : 0.0) - (j < 4 ? k[i][j] : 0.0);
}
}
P_ = MatMul8(ikh, P_);
}
} // namespace tracker
} // namespace drone
reference-c++/target_tracker/src/target_tracker.cpp
+312
View File
@@ -0,0 +1,312 @@
// =============================================================================
// target_tracker.cpp —— 多目标追踪过滤器（TargetTracker）实现
//
// 对应规格：docs/视觉追踪技术路线.md
// §5 检测-轨迹关联（贪婪最邻近，§5.3）
// §6 轨迹生命周期与 ID 管理（主目标锁定与选举，§6.3）
// §7 主流程六步（UPDATE 伪代码）
// §9.5 线程模型（单线程写 Update，仅 last_result 缓存加锁）
// =============================================================================
#include "target_tracker/target_tracker.h"
#include <algorithm>
#include <cmath>
#include <limits>
namespace drone {
namespace tracker {
TargetTracker::TargetTracker() : TargetTracker(Config{}) {}
TargetTracker::TargetTracker(const Config& cfg) : config_(cfg) {}
TrackResult TargetTracker::Update(const std::vector<Detection>& detections,
std::optional<FrameShape> frame_shape) {
// §7：frame_count += 1（仅统计用）
++frame_count_;
// ---- 入口防御：过滤坐标或置信度含 NaN/Inf 的检测 ----
// 防御性增强：上游检测器理论上已过滤，但库自身必须免疫——
// 非有限值一旦进入卡尔曼状态将永久污染轨道并传导到主目标输出
std::vector<Detection> valid_detections;
valid_detections.reserve(detections.size());
for (const Detection& det : detections) {
if (std::isfinite(det.x1) && std::isfinite(det.y1) &&
std::isfinite(det.x2) && std::isfinite(det.y2) &&
std::isfinite(det.conf)) {
valid_detections.push_back(det);
}
}
// ---- 步骤 1：所有存活轨道先行预测（推进一帧）----
// （§7 步骤 1：t.predict() 更新 last_center / last_box、age += 1）
for (auto& kv : tracks_) {
kv.second.Predict();
}
// ---- 步骤 2：构造检测中心表并贪婪关联（§7 步骤 2 / §5.3） ----
AssociationResult assoc;
if (!valid_detections.empty()) {
assoc = AssociateDetections(valid_detections);
} else {
// 无检测：全部轨道视为未匹配，全部进入 Coast 计数（§7 步骤 2 否则分支）
for (const auto& kv : tracks_) {
assoc.unmatched_track_ids.push_back(kv.first);
}
}
// ---- 步骤 3：用匹配到的检测更新轨道（§7 步骤 3） ----
for (const auto& match : assoc.matches) {
const int track_id = match.first;
const Detection& det = valid_detections[static_cast<size_t>(match.second)];
// §7 步骤 3：由检测重建原始框 raw_box = [cx−w/2, cy−h/2, cx+w/2, cy+h/2]
// （等价于检测框本身：cx=(x1+x2)/2、w=x2−x1 的逆运算，浮点上精确还原）
const BoxXYXY raw_box{det.x1, det.y1, det.x2, det.y2};
tracks_.at(track_id).Update(raw_box, det.conf, det.cls_id);
}
// ---- 步骤 4：未匹配轨道计丢失，死亡者回收（§7 步骤 4） ----
for (const int track_id : assoc.unmatched_track_ids) {
auto it = tracks_.find(track_id);
if (it == tracks_.end()) {
continue;
}
it->second.MarkMiss(); // lost_count+1、age+1
if (it->second.IsDead()) { // lost_count > max_lost（严格大于，§6.1）
// 锁定的唯一解除途径之一：锁定轨道死亡被回收时清锁（§6.3）
if (locked_primary_id_.has_value() && *locked_primary_id_ == track_id) {
locked_primary_id_.reset();
}
tracks_.erase(it);
}
}
// ---- 步骤 5：未匹配检测 → 创建新轨道（§7 步骤 5 / §6.2） ----
for (const int det_idx : assoc.unmatched_det_indices) {
const Detection& det = valid_detections[static_cast<size_t>(det_idx)];
// §7 步骤 5：以 [cx,cy,w,h] 创建；构造函数内重建 last_box，
// 保证创建当帧即可参与输出；next_id 从 0 递增永不复用（§6.2）
tracks_.emplace(next_id_,
Tracklet(next_id_, ToCenterSize({det.x1, det.y1, det.x2, det.y2}),
det.conf, det.cls_id, config_.max_lost_frames));
++next_id_;
}
// ---- 步骤 6：选取/延续主目标，组装输出并缓存（§7 步骤 6 / §6.3） ----
TrackResult result = SelectPrimary(frame_shape);
{
std::lock_guard<std::mutex> lock(last_result_mutex_);
last_result_ = result; // 缓存供跨线程读取（§9.5）
}
return result;
}
TrackResult TargetTracker::LastResult() const {
// §9.5：LastResult() 返回受互斥锁保护的拷贝
std::lock_guard<std::mutex> lock(last_result_mutex_);
return last_result_;
}
void TargetTracker::Reset() {
// §2.1：清空所有轨道、ID 计数器、锁定状态与缓存结果
tracks_.clear();
next_id_ = 0; // §6.2：reset() 时计数器归零
frame_count_ = 0;
locked_primary_id_.reset(); // 锁定的唯一解除途径之二（§6.3）
std::lock_guard<std::mutex> lock(last_result_mutex_);
last_result_ = TrackResult{};
}
int TargetTracker::ActiveCount() const {
return static_cast<int>(tracks_.size());
}
std::unordered_map<int, Tracklet> TargetTracker::ActiveTracks() const {
return tracks_; // 值拷贝只读快照（§2.1）
}
TargetTracker::AssociationResult TargetTracker::AssociateDetections(
const std::vector<Detection>& detections) {
AssociationResult result;
// §5.3：按轨道 confidence 降序排序（复制 ID 到 vector 后 sort，
// 不直接排哈希表，§9.2）
std::vector<int> sorted_ids;
sorted_ids.reserve(tracks_.size());
for (const auto& kv : tracks_) {
sorted_ids.push_back(kv.first);
}
std::sort(sorted_ids.begin(), sorted_ids.end(), [this](int a, int b) {
const double raw_a = tracks_.at(a).confidence;
const double raw_b = tracks_.at(b).confidence;
// NaN 安全：非有限置信度按 −∞ 处理（排最后），避免 NaN 参与比较
// 破坏 std::sort 要求的严格弱序（未定义行为）；映射后为全序，
// 等价传递性恒成立
const double ca = std::isfinite(raw_a)
? raw_a
: -std::numeric_limits<double>::infinity();
const double cb = std::isfinite(raw_b)
? raw_b
: -std::numeric_limits<double>::infinity();
if (ca != cb) return ca > cb; // 置信度从高到低
return a < b; // 平局按 ID 升序作次级键，保证确定性
});
std::vector<bool> det_assigned(detections.size(), false);
for (const int track_id : sorted_ids) {
const Tracklet& track = tracks_.at(track_id);
// §5.3：每条轨道最优代价初始化为 1.0（仅 cost 严格小于 1.0 才记录，
// 即 cost ≥ 1.0 判为不可匹配，§5.1 匹配有效性判据）
double best_cost = 1.0;
int best_det = -1;
for (size_t d = 0; d < detections.size(); ++d) {
if (det_assigned[d]) continue; // 跳过已分配检测
const Detection& det = detections[d];
// §5.1：中心点欧氏距离（像素）
const double det_cx = (det.x1 + det.x2) / 2.0;
const double det_cy = (det.y1 + det.y2) / 2.0;
const double dist =
std::hypot(track.last_center.first - det_cx,
track.last_center.second - det_cy);
// §5.1：归一化距离，取 max(Dmax, 1e-3) 防除零
const double norm_dist =
dist / std::max(config_.max_association_dist, 1e-3);
// §5.2：预测框与检测框的 IoU
const double iou = track.last_box.has_value()
? ComputeIoU(*track.last_box,
{det.x1, det.y1, det.x2, det.y2})
: 0.0;
// §5.1：cost = w_dist·norm_dist + w_iou·(1−IoU)
const double cost = config_.dist_weight * norm_dist +
config_.iou_weight * (1.0 - iou);
if (cost < best_cost) { // 严格小于才更新（§5.1 / §5.3）
best_cost = cost;
best_det = static_cast<int>(d);
}
}
if (best_det >= 0) {
result.matches.emplace_back(track_id, best_det);
det_assigned[static_cast<size_t>(best_det)] = true;
} else {
result.unmatched_track_ids.push_back(track_id);
}
}
// §5.3：未分配检测收集
for (size_t d = 0; d < detections.size(); ++d) {
if (!det_assigned[d]) {
result.unmatched_det_indices.push_back(static_cast<int>(d));
}
}
return result;
}
TrackResult TargetTracker::SelectPrimary(std::optional<FrameShape> frame_shape) {
TrackResult result;
result.n_active = static_cast<int>(tracks_.size());
// §6.3：轨道表为空 → 清除锁定，返回 tracked=false
if (tracks_.empty()) {
locked_primary_id_.reset();
return result;
}
// §6.3 ① 锁定延续：锁定轨道仍存活则直接输出（Coast 中照样输出，
// is_predicted=true；不被其他更高置信度目标抢占）
Tracklet* chosen = nullptr;
bool is_predicted = false;
if (locked_primary_id_.has_value()) {
auto it = tracks_.find(*locked_primary_id_);
if (it != tracks_.end()) {
chosen = &it->second;
is_predicted = it->second.IsCoasting();
}
}
// §6.3 ② 重新选举（无锁定 / 锁定目标已死亡被回收）
if (chosen == nullptr) {
// measured ← 所有 lost_count == 0 的轨道；coasting ← 其余存活轨道
std::vector<Tracklet*> measured;
std::vector<Tracklet*> coasting;
for (auto& kv : tracks_) {
if (kv.second.IsCoasting()) {
coasting.push_back(&kv.second);
} else {
measured.push_back(&kv.second);
}
}
if (!measured.empty()) {
// confirmed ← measured 中 hits ≥ min_hits 者；为空则退回 measured
// （min_hits 只影响候选池优先级，不阻止输出，§6.1 注记 1）
std::vector<Tracklet*> pool;
for (Tracklet* t : measured) {
if (t->hits >= config_.min_hits) {
pool.push_back(t);
}
}
if (pool.empty()) {
pool = measured;
}
// chosen ← pool 中 confidence 最大者；平局取 unordered_map 遍历序
// 中首个最大者，不做确定性承诺（规格 §6.3 未规定平局规则）
chosen = pool.front();
for (Tracklet* t : pool) {
if (t->confidence > chosen->confidence) {
chosen = t;
}
}
is_predicted = false;
} else if (!coasting.empty()) {
// 无实测轨道：选 coasting 中离画面中心最近者（欧氏距离平方排序）
// §2.2：frame_shape 缺省按 640×480 画面中心 (320, 240)
const double center_x =
(frame_shape.has_value() && frame_shape->width > 0)
? frame_shape->width / 2.0
: 320.0;
const double center_y =
(frame_shape.has_value() && frame_shape->height > 0)
? frame_shape->height / 2.0
: 240.0;
double best_dist_sq = std::numeric_limits<double>::infinity();
for (Tracklet* t : coasting) {
const double dx = t->last_center.first - center_x;
const double dy = t->last_center.second - center_y;
const double dist_sq = dx * dx + dy * dy;
if (dist_sq < best_dist_sq) {
best_dist_sq = dist_sq;
chosen = t;
}
}
if (chosen == nullptr) {
// NaN 残余的防御兜底：若所有 coasting 轨道中心均为 NaN，
// dist_sq 恒为 NaN 无人当选，取首个避免空指针解引用；
// 正常输入已在 Update 入口经有限性过滤，本分支理论上不可达
chosen = coasting.front();
}
is_predicted = true; // Coast 兜底：仅预测、无实测
} else {
// 理论上不可达（轨道非空则 measured/coasting 必居其一）；防御清锁
locked_primary_id_.reset();
return result;
}
locked_primary_id_ = chosen->track_id; // 选举后锁定（§6.3）
}
// ---- 组装输出（§2.3 九字段） ----
result.tracked = true;
result.primary_id = chosen->track_id;
result.center = chosen->last_center;
result.box = chosen->last_box;
result.confidence = chosen->confidence; // Coast 期间继承最近实测置信度
result.is_predicted = is_predicted;
result.lost_frames = chosen->lost_count;
// §2.3：raw = 关联到的原始检测框；Coast 时为空（锁定延续分支同样遵守）
result.raw = is_predicted ? std::nullopt : chosen->last_raw_box;
return result;
}
} // namespace tracker
} // namespace drone
reference-c++/target_tracker/src/tracklet.cpp
+101
View File
@@ -0,0 +1,101 @@
// =============================================================================
// tracklet.cpp —— 轨道（Tracklet）与基础几何工具实现
//
// 对应规格：docs/视觉追踪技术路线.md
// §5.2 IoU 计算
// §7 TRACKLET_PREDICT / TRACKLET_UPDATE 伪代码
// §3.2 Tracklet 字段语义
// =============================================================================
#include "target_tracker/tracklet.h"
#include <algorithm>
namespace drone {
namespace tracker {
BoxCXCYWH ToCenterSize(const BoxXYXY& box) {
// §7 TRACKLET_UPDATE：cx=(x1+x2)/2；cy=(y1+y2)/2；w=x2−x1；h=y2−y1
return BoxCXCYWH{(box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0,
box.x2 - box.x1, box.y2 - box.y1};
}
BoxXYXY ToXYXY(const BoxCXCYWH& box) {
// §7 TRACKLET_PREDICT：center_size → [cx−w/2, cy−h/2, cx+w/2, cy+h/2]
return BoxXYXY{box.cx - box.w / 2.0, box.cy - box.h / 2.0,
box.cx + box.w / 2.0, box.cy + box.h / 2.0};
}
double ComputeIoU(const BoxXYXY& a, const BoxXYXY& b) {
// §5.2：任一框为空（宽或高 ≤ 0）返回 0
const double area_a = (a.x2 - a.x1) * (a.y2 - a.y1);
const double area_b = (b.x2 - b.x1) * (b.y2 - b.y1);
if (area_a <= 0.0 || area_b <= 0.0) {
return 0.0;
}
// 交集宽高与 0 取最大值（§5.2）
const double inter_w = std::min(a.x2, b.x2) - std::max(a.x1, b.x1);
const double inter_h = std::min(a.y2, b.y2) - std::max(a.y1, b.y1);
if (inter_w <= 0.0 || inter_h <= 0.0) {
return 0.0; // 交集面积 ≤ 0 返回 0（§5.2）
}
const double inter_area = inter_w * inter_h;
const double union_area = area_a + area_b - inter_area;
if (union_area <= 0.0) {
return 0.0; // 并集 ≤ 0 返回 0（§5.2）
}
// IoU = area(A∩B) / (area(A)+area(B)−area(A∩B))（§5.2）
return inter_area / union_area;
}
Tracklet::Tracklet(int id, const BoxCXCYWH& measurement, double conf, int cls,
int max_lost_frames)
: track_id(id),
max_lost(max_lost_frames),
confidence(conf),
cls_id(cls) {
// §4.3：用首次观测初始化滤波器（速度置零、P 取默认对角）
kf.Initiate(measurement);
// §7 步骤 5：创建当帧即赋 last_center / last_box（保证当帧可参与输出）
last_center = {measurement.cx, measurement.cy};
last_box = ToXYXY(measurement);
// 创建即源于实测检测：同步记录原始检测框（Coast 期间保留，
// 对外输出按 §2.3 在 Coast 时置空）
last_raw_box = last_box;
// age / hits / lost_count 由类内默认值保证（1 / 1 / 0，§3.2）
}
void Tracklet::Predict() {
// §7 TRACKLET_PREDICT：卡尔曼预测推进一帧
const auto pred = kf.Predict();
if (pred.has_value()) {
// 预测成功：更新 last_center / last_box，age += 1
last_center = {pred->cx, pred->cy};
last_box = ToXYXY(*pred);
++age;
}
}
void Tracklet::Update(const BoxXYXY& box, double conf, int cls) {
// §7 TRACKLET_UPDATE：框 → [cx,cy,w,h] → kf.update
kf.Update(ToCenterSize(box));
last_center = {(box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0};
last_box = box;
last_raw_box = box; // 实测关联到的原始检测框（Coast 期间保留不更新）
confidence = conf;
cls_id = cls;
lost_count = 0; // 匹配成功：连续丢失计数清零
++hits; // 累计匹配次数 +1
++age; // 帧龄 +1
}
void Tracklet::MarkMiss() {
// §7 步骤 4：未匹配计丢失 lost_count+1、age+1
++lost_count;
++age;
}
} // namespace tracker
} // namespace drone
reference-c++/target_tracker/target_tracker.md
+217
View File
@@ -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 目录式部署约定一致）
```
reference-c++/target_tracker/tests/kalman_box_filter_test.cpp
+124
View File
@@ -0,0 +1,124 @@
// =============================================================================
// kalman_box_filter_test.cpp —— KalmanBoxFilter 单元测试
//
// 覆盖 docs/视觉追踪技术路线.md §4 行为：
// - 初始化状态正确（§4.3）
// - 静止目标多帧收敛到观测（§4.5）
// - 匀速目标速度收敛且预测超前一个 dt（§4.4）
// - 未初始化 predict 返回 nullopt（§4.4）
// =============================================================================
#include <gtest/gtest.h>
#include <array>
#include <cmath>
#include "target_tracker/kalman_box_filter.h"
using drone::tracker::BoxCXCYWH;
using drone::tracker::KalmanBoxFilter;
namespace {
// 未初始化时 predict 返回 nullopt（§4.4）；update 直接返回不改变状态（§4.5）
TEST(KalmanBoxFilterTest, UninitializedPredictReturnsNullopt) {
KalmanBoxFilter kf;
EXPECT_FALSE(kf.Initialized());
EXPECT_FALSE(kf.Predict().has_value());
// 未初始化时 update 无副作用
kf.Update({100.0, 100.0, 20.0, 20.0});
EXPECT_FALSE(kf.Initialized());
EXPECT_FALSE(kf.Predict().has_value());
for (const double v : kf.State()) {
EXPECT_DOUBLE_EQ(v, 0.0);
}
}
// 初始化状态正确（§4.3）：速度置零、位置/尺寸取首次观测
TEST(KalmanBoxFilterTest, InitiateSetsStateCorrectly) {
KalmanBoxFilter kf;
kf.Initiate({320.0, 240.0, 40.0, 30.0});
EXPECT_TRUE(kf.Initialized());
const std::array<double, 8> expected = {
{320.0, 240.0, 40.0, 30.0, 0.0, 0.0, 0.0, 0.0}};
for (size_t i = 0; i < expected.size(); ++i) {
EXPECT_DOUBLE_EQ(kf.State()[i], expected[i]) << "index=" << i;
}
// 初始化后 predict 返回状态前 4 维副本（速度为零 → 位置不变，§4.4）
const auto pred = kf.Predict();
ASSERT_TRUE(pred.has_value());
EXPECT_NEAR(pred->cx, 320.0, 1e-9);
EXPECT_NEAR(pred->cy, 240.0, 1e-9);
EXPECT_NEAR(pred->w, 40.0, 1e-9);
EXPECT_NEAR(pred->h, 30.0, 1e-9);
}
// 静止目标：多帧重复观测同一框，滤波输出应收敛到观测值（§4.5）
TEST(KalmanBoxFilterTest, StaticTargetConvergesToObservation) {
KalmanBoxFilter kf;
const BoxCXCYWH z{100.0, 150.0, 40.0, 60.0};
kf.Initiate({104.0, 156.0, 42.0, 62.0}); // 带初始偏差，验证收敛能力
for (int i = 0; i < 50; ++i) {
kf.Predict();
kf.Update(z);
}
EXPECT_NEAR(kf.State()[0], z.cx, 0.5); // cx
EXPECT_NEAR(kf.State()[1], z.cy, 0.5); // cy
EXPECT_NEAR(kf.State()[2], z.w, 0.5); // w
EXPECT_NEAR(kf.State()[3], z.h, 0.5); // h
// 静止目标速度应收敛到约 0
EXPECT_NEAR(kf.State()[4], 0.0, 0.1);
EXPECT_NEAR(kf.State()[5], 0.0, 0.1);
}
// 匀速目标：速度估计收敛到真实速度，且 predict 超前一个 dt（§4.4）
TEST(KalmanBoxFilterTest, ConstantVelocityConvergesAndPredictLeadsOneDt) {
KalmanBoxFilter kf;
const double vx = 3.0; // 像素/帧
const double vy = 2.0; // 像素/帧
// 目标从 (100, 100) 出发匀速运动，每帧观测一次（predict → update）
BoxCXCYWH z{100.0, 100.0, 30.0, 30.0};
kf.Initiate(z);
for (int i = 0; i < 60; ++i) {
kf.Predict();
z.cx += vx;
z.cy += vy;
kf.Update(z);
}
// 速度收敛到真实速度（像素/帧）
EXPECT_NEAR(kf.State()[4], vx, 0.3) << "vx 未收敛";
EXPECT_NEAR(kf.State()[5], vy, 0.3) << "vy 未收敛";
// 位置贴合当前观测
EXPECT_NEAR(kf.State()[0], z.cx, 1.5);
EXPECT_NEAR(kf.State()[1], z.cy, 1.5);
// 再预测一帧：应超前当前观测一个 dt（≈ 速度 × 1 帧）
const auto pred = kf.Predict();
ASSERT_TRUE(pred.has_value());
EXPECT_NEAR(pred->cx, z.cx + vx, 1.0) << "预测未超前一个 dt";
EXPECT_NEAR(pred->cy, z.cy + vy, 1.0) << "预测未超前一个 dt";
}
// 动态噪声不退化：小尺寸目标多帧观测仍稳定收敛（§4.2 噪声与尺寸成比例）
TEST(KalmanBoxFilterTest, SmallTargetNoiseScalingStable) {
KalmanBoxFilter kf;
const BoxCXCYWH z{50.0, 60.0, 4.0, 4.0}; // 小目标（4×4 像素）
kf.Initiate(z);
for (int i = 0; i < 30; ++i) {
kf.Predict();
kf.Update(z);
}
EXPECT_NEAR(kf.State()[0], z.cx, 0.5);
EXPECT_NEAR(kf.State()[1], z.cy, 0.5);
EXPECT_TRUE(std::isfinite(kf.State()[2]));
EXPECT_TRUE(std::isfinite(kf.State()[3]));
}
} // namespace
reference-c++/target_tracker/tests/target_tracker_test.cpp
+402
View File
@@ -0,0 +1,402 @@
// =============================================================================
// target_tracker_test.cpp —— TargetTracker 单元测试
//
// 逐条覆盖 docs/视觉追踪技术路线.md §9.4 行为一致性验收用例（硬验收标准）：
// 1. 空检测帧序列：Coast 持续 max_lost_frames 帧，第 max_lost_frames+1 帧判丢
// 2. ID 严格递增不复用（含 Reset 后归零）
// 3. 锁定不被抢占；锁定目标死亡后切换到新目标
// 4. min_hits 回退：无已确认实测轨道时新轨道也可当选主目标
// 5. cost=1.0 恰好不构成匹配（严格小于）
// 6. 置信度优先：两轨道竞争同一检测，置信度高者获胜
// =============================================================================
#include <gtest/gtest.h>
#include <cmath>
#include <limits>
#include <optional>
#include <vector>
#include "target_tracker/target_tracker.h"
using drone::tracker::Detection;
using drone::tracker::FrameShape;
using drone::tracker::TargetTracker;
using drone::tracker::TrackResult;
namespace {
Detection MakeDet(double x1, double y1, double x2, double y2, double conf,
int cls = 0) {
return Detection{x1, y1, x2, y2, conf, cls};
}
// ============================================================================
// 验收用例 1（§9.4-1）：空检测帧序列
// 连续输入空检测，主目标输出 is_predicted=true 共 max_lost_frames 帧，
// 第 max_lost_frames + 1 帧 tracked=false（死亡判定 lost_count > max_lost）。
// ============================================================================
TEST(TargetTrackerAcceptanceTest, EmptyFramesCoastThenLost) {
TargetTracker tracker; // 默认配置 max_lost_frames = 8
ASSERT_EQ(tracker.GetConfig().max_lost_frames, 8);
// 第 1 帧：给出一个检测，建立轨道并锁定为主目标
TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 140.0, 140.0, 0.9)});
ASSERT_TRUE(r.tracked);
ASSERT_TRUE(r.primary_id.has_value());
EXPECT_FALSE(r.is_predicted); // 当帧有实测
EXPECT_EQ(r.lost_frames, 0);
EXPECT_EQ(r.n_active, 1);
ASSERT_TRUE(r.box.has_value());
ASSERT_TRUE(r.raw.has_value()); // 当帧关联到原始检测框
// 后续空检测帧：应持续 Coast 共 max_lost_frames 帧（is_predicted=true）
for (int i = 1; i <= 8; ++i) {
r = tracker.Update({});
ASSERT_TRUE(r.tracked) << "第 " << i << " 帧 Coast 期间不应判丢";
EXPECT_TRUE(r.is_predicted) << "第 " << i << " 帧应为纯预测输出";
EXPECT_EQ(r.lost_frames, i);
EXPECT_TRUE(r.primary_id.has_value());
EXPECT_TRUE(r.box.has_value()); // Coast 期间继续输出预测框
EXPECT_FALSE(r.raw.has_value()); // Coast 时 raw 为空（§2.3）
EXPECT_DOUBLE_EQ(r.confidence, 0.9); // 继承最近实测置信度（§2.3）
}
// 第 max_lost_frames + 1 帧：轨道死亡回收 → tracked=false
r = tracker.Update({});
EXPECT_FALSE(r.tracked);
EXPECT_FALSE(r.primary_id.has_value());
EXPECT_FALSE(r.box.has_value());
EXPECT_EQ(r.lost_frames, 0); // 丢失时 lost_frames=0（§2.3）
EXPECT_EQ(r.n_active, 0);
}
// ============================================================================
// 验收用例 2（§9.4-2）：ID 单调性
// 多次进出画面的目标获得严格递增、不复用的 ID；Reset 后计数器归零。
// ============================================================================
TEST(TargetTrackerAcceptanceTest, IdStrictlyIncreasingNoReuse) {
TargetTracker::Config cfg;
cfg.max_lost_frames = 1; // 缩短 Coast 窗口，加速轨道死亡
TargetTracker tracker(cfg);
// 目标 A 出现 → id 0
TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.9)});
ASSERT_TRUE(r.primary_id.has_value());
const int id_a = *r.primary_id;
EXPECT_EQ(id_a, 0);
// 目标 A 消失至死亡（max_lost=1：第 2 帧 Coast、第 3 帧死亡回收）
tracker.Update({});
r = tracker.Update({});
EXPECT_FALSE(r.tracked);
EXPECT_EQ(tracker.ActiveCount(), 0);
// 目标 B 出现（同一位置）：ID 不复用，严格递增 → id 1
r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.9)});
ASSERT_TRUE(r.primary_id.has_value());
const int id_b = *r.primary_id;
EXPECT_EQ(id_b, 1);
EXPECT_GT(id_b, id_a);
// 目标 C 同帧新建 → id 2（同帧多目标亦严格递增）
r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.9),
MakeDet(400.0, 300.0, 430.0, 330.0, 0.8)});
EXPECT_EQ(tracker.ActiveCount(), 2);
int max_id = 0;
for (const auto& kv : tracker.ActiveTracks()) {
max_id = std::max(max_id, kv.first);
}
EXPECT_EQ(max_id, 2);
// Reset 后计数器归零（§6.2）：新轨道重新从 id 0 开始
tracker.Reset();
EXPECT_EQ(tracker.ActiveCount(), 0);
EXPECT_FALSE(tracker.LastResult().tracked);
r = tracker.Update({MakeDet(50.0, 50.0, 70.0, 70.0, 0.9)});
ASSERT_TRUE(r.primary_id.has_value());
EXPECT_EQ(*r.primary_id, 0);
}
// ============================================================================
// 验收用例 3（§9.4-3）：锁定不被抢占
// 锁定目标 Coast 期间出现更高置信度新目标，primary_id 不变；
// 锁定目标死亡后切换到新目标。
// ============================================================================
TEST(TargetTrackerAcceptanceTest, LockNotStolenAndSwitchAfterDeath) {
TargetTracker::Config cfg;
cfg.max_lost_frames = 3;
TargetTracker tracker(cfg);
// 低置信度目标 A 建立并锁定
TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 140.0, 140.0, 0.4)});
ASSERT_TRUE(r.primary_id.has_value());
const int id_a = *r.primary_id;
// A 消失进入 Coast；期间出现更高置信度新目标 B（远离 A，不会被误关联）
r = tracker.Update({MakeDet(500.0, 300.0, 540.0, 340.0, 0.95)});
ASSERT_TRUE(r.primary_id.has_value());
EXPECT_EQ(*r.primary_id, id_a) << "Coast 期间锁定不应被高置信度目标抢占";
EXPECT_TRUE(r.is_predicted);
EXPECT_EQ(tracker.ActiveCount(), 2); // A（Coast）与 B 并存
// 继续 Coast，锁定依旧延续
r = tracker.Update({MakeDet(500.0, 300.0, 540.0, 340.0, 0.95)});
EXPECT_EQ(*r.primary_id, id_a);
EXPECT_TRUE(r.is_predicted);
// A 继续丢失直至死亡（max_lost=3：lost 达 4 帧时回收）
for (int i = 0; i < 2; ++i) {
r = tracker.Update({MakeDet(500.0, 300.0, 540.0, 340.0, 0.95)});
}
// A 已死亡删除 → 锁定解除，重新选举切换到 B（此时 B 已确认且实测）
ASSERT_TRUE(r.tracked);
EXPECT_NE(*r.primary_id, id_a) << "锁定目标死亡后应切换到新目标";
EXPECT_FALSE(r.is_predicted);
// 后续 B 持续作为主目标
r = tracker.Update({MakeDet(500.0, 300.0, 540.0, 340.0, 0.95)});
ASSERT_TRUE(r.primary_id.has_value());
EXPECT_NE(*r.primary_id, id_a);
}
// ============================================================================
// 验收用例 4（§9.4-4）：min_hits 回退
// 新轨道（hits 未达 min_hits）在无其他已确认实测轨道时也可当选主目标。
// ============================================================================
TEST(TargetTrackerAcceptanceTest, MinHitsFallbackAllowsNewTrack) {
TargetTracker::Config cfg;
cfg.min_hits = 5; // 抬高确认门槛，保证测试期间轨道始终未确认
TargetTracker tracker(cfg);
// 首帧新建轨道 hits=1 < min_hits：候选池 confirmed 为空 → 退回 measured
TrackResult r = tracker.Update({MakeDet(200.0, 150.0, 240.0, 190.0, 0.6)});
ASSERT_TRUE(r.tracked) << "无已确认轨道时新轨道应能当选主目标";
ASSERT_TRUE(r.primary_id.has_value());
EXPECT_EQ(*r.primary_id, 0);
EXPECT_FALSE(r.is_predicted);
// 再跟几帧（hits 增长但仍未达 min_hits=5），依旧当选
for (int i = 0; i < 3; ++i) {
r = tracker.Update({MakeDet(200.0, 150.0, 240.0, 190.0, 0.6)});
ASSERT_TRUE(r.tracked);
EXPECT_EQ(*r.primary_id, 0);
}
}
// ============================================================================
// 验收用例 5（§9.4-5）：cost 边界
// 恰好 cost = 1.0 不构成匹配（仅严格小于才匹配）。
// 构造：w_dist=1、w_iou=0（纯距离项），中心距离恰等于 Dmax → cost=1.0；
// 同时给出略小于阈值的对照帧证明边界两侧行为正确。
// ============================================================================
TEST(TargetTrackerAcceptanceTest, CostExactlyOneDoesNotMatch) {
TargetTracker::Config cfg;
cfg.max_lost_frames = 1;
cfg.max_association_dist = 200.0;
cfg.dist_weight = 1.0; // 纯距离项，IoU 项权重为 0 且两框零交集
cfg.iou_weight = 0.0;
TargetTracker tracker(cfg);
// 建立轨道：中心 (100,100)
TrackResult r = tracker.Update({MakeDet(90.0, 90.0, 110.0, 110.0, 0.9)});
ASSERT_TRUE(r.primary_id.has_value());
const int id_a = *r.primary_id;
// 下一帧检测中心 (100,300)：距离 = 200 = Dmax → cost = 1.0，恰好不匹配
// （预测后轨道中心速度为 0，仍位于 (100,100)；两框零交集 IoU=0 无影响）
r = tracker.Update({MakeDet(90.0, 290.0, 110.0, 310.0, 0.9)});
// 未匹配 → 原轨道 Coast、检测新建轨道；两个目标并存
EXPECT_EQ(tracker.ActiveCount(), 2) << "cost=1.0 不应匹配，应新建轨道";
// 原轨道（锁定）进入 Coast：lost_frames=1
EXPECT_TRUE(r.tracked);
EXPECT_TRUE(r.is_predicted);
EXPECT_EQ(*r.primary_id, id_a);
EXPECT_EQ(r.lost_frames, 1);
// 对照：距离略小于阈值（199px → cost=0.995 < 1.0）应匹配成功
TargetTracker tracker2(cfg);
r = tracker2.Update({MakeDet(90.0, 90.0, 110.0, 110.0, 0.9)});
const int id_b = *r.primary_id;
r = tracker2.Update({MakeDet(90.0, 289.0, 110.0, 309.0, 0.9)});
EXPECT_EQ(tracker2.ActiveCount(), 1) << "cost<1.0 应匹配成功，不应新建轨道";
ASSERT_TRUE(r.primary_id.has_value());
EXPECT_EQ(*r.primary_id, id_b);
EXPECT_FALSE(r.is_predicted); // 实测命中
}
// ============================================================================
// 验收用例 6（§9.4-6）：置信度优先
// 两轨道竞争同一检测时，置信度高者获胜。
// ============================================================================
TEST(TargetTrackerAcceptanceTest, HigherConfidenceTrackWinsDetection) {
TargetTracker tracker;
// 同帧建立两条轨道：A 置信度 0.5、B 置信度 0.9，相距 80px（不互相重叠）
TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.5),
MakeDet(180.0, 100.0, 200.0, 120.0, 0.9)});
ASSERT_EQ(tracker.ActiveCount(), 2);
// 找出两条轨道 ID（按置信度区分）
int id_low = -1;
int id_high = -1;
for (const auto& kv : tracker.ActiveTracks()) {
if (kv.second.confidence > 0.8) {
id_high = kv.first;
} else {
id_low = kv.first;
}
}
ASSERT_NE(id_low, -1);
ASSERT_NE(id_high, -1);
// 下一帧只给一个检测，放在两轨道预测中心之间（各距 40px，对称）：
// 距离项对两者相同，贪婪按置信度降序 → 高置信度轨道 B 优先匹配
r = tracker.Update({MakeDet(140.0, 100.0, 160.0, 120.0, 0.7)});
const auto tracks = tracker.ActiveTracks();
ASSERT_EQ(tracks.size(), 2u);
// 获胜者：命中（lost=0、hits=2、置信度更新）
EXPECT_EQ(tracks.at(id_high).lost_count, 0) << "高置信度轨道应获胜";
EXPECT_EQ(tracks.at(id_high).hits, 2);
EXPECT_DOUBLE_EQ(tracks.at(id_high).confidence, 0.7);
// 失败者：未匹配进入 Coast（lost=1、hits 不变）
EXPECT_EQ(tracks.at(id_low).lost_count, 1) << "低置信度轨道应落选";
EXPECT_EQ(tracks.at(id_low).hits, 1);
EXPECT_DOUBLE_EQ(tracks.at(id_low).confidence, 0.5);
}
// ============================================================================
// NaN/Inf 防御（评审修复项）：坐标或置信度含非有限值的检测被丢弃，
// 不崩溃、不新建轨道、不污染主目标输出
// ============================================================================
TEST(TargetTrackerAcceptanceTest, NonFiniteDetectionsAreDiscarded) {
TargetTracker tracker;
const double nan = std::numeric_limits<double>::quiet_NaN();
const double inf = std::numeric_limits<double>::infinity();
// 第 1 帧：建立正常轨道
TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 140.0, 140.0, 0.9)});
ASSERT_TRUE(r.tracked);
// 第 2 帧：混入 NaN 坐标 / Inf 坐标 / NaN 置信度检测与一个正常检测：
// 非有限检测全部被丢弃，仅正常检测参与关联，不新建轨道
r = tracker.Update({MakeDet(nan, 100.0, 140.0, 140.0, 0.9),
MakeDet(100.0, -inf, 140.0, 140.0, 0.9),
MakeDet(100.0, 100.0, nan, 140.0, 0.9),
MakeDet(102.0, 102.0, 142.0, 142.0, nan),
MakeDet(101.0, 101.0, 141.0, 141.0, 0.9)});
EXPECT_EQ(tracker.ActiveCount(), 1) << "非有限检测不应新建轨道";
ASSERT_TRUE(r.tracked);
EXPECT_FALSE(r.is_predicted); // 正常检测命中，实测输出
// 主目标输出未被 NaN 污染
EXPECT_TRUE(std::isfinite(r.center.first));
EXPECT_TRUE(std::isfinite(r.center.second));
ASSERT_TRUE(r.box.has_value());
EXPECT_TRUE(std::isfinite(r.box->x1) && std::isfinite(r.box->y1));
EXPECT_TRUE(std::isfinite(r.box->x2) && std::isfinite(r.box->y2));
EXPECT_TRUE(std::isfinite(r.confidence));
ASSERT_TRUE(r.raw.has_value()); // 实测帧 raw 有值
// 第 3 帧：仅含 NaN 检测 → 等价空帧，轨道 Coast 不崩溃
r = tracker.Update({MakeDet(nan, nan, nan, nan, nan)});
EXPECT_TRUE(r.tracked);
EXPECT_TRUE(r.is_predicted);
EXPECT_EQ(r.lost_frames, 1);
EXPECT_FALSE(r.raw.has_value()); // Coast 时 raw 为空（§2.3）
EXPECT_TRUE(std::isfinite(r.center.first));
// 持续 NaN 检测直至轨道死亡回收，全程不崩溃
for (int i = 0; i < 10; ++i) {
r = tracker.Update({MakeDet(nan, 0.0, 10.0, 10.0, 0.9)});
}
EXPECT_FALSE(r.tracked);
EXPECT_EQ(tracker.ActiveCount(), 0);
}
// ============================================================================
// 补充行为测试（§2.3 / §6.3 / §9.5）
// ============================================================================
// 丢失时返回值约定：tracked=false、定位字段空、lost_frames=0（§2.3）
TEST(TargetTrackerBehaviorTest, LostResultFieldsAreEmpty) {
TargetTracker tracker;
TrackResult r = tracker.Update({}); // 空帧序列起点：无任何轨道
EXPECT_FALSE(r.tracked);
EXPECT_FALSE(r.primary_id.has_value());
EXPECT_FALSE(r.box.has_value());
EXPECT_FALSE(r.raw.has_value());
EXPECT_EQ(r.lost_frames, 0);
EXPECT_EQ(r.n_active, 0);
EXPECT_DOUBLE_EQ(r.confidence, 0.0);
// LastResult 返回同一快照（§9.5 跨线程读取入口）
const TrackResult cached = tracker.LastResult();
EXPECT_FALSE(cached.tracked);
}
// Coast 兜底选举：无实测轨道时选离画面中心最近的 Coast 轨道（§6.3）。
// 注意锁定延续优先：需先让锁定轨道死亡解除锁定，才会进入重新选举分支，
// 故采用错帧创建 + 缩短 max_lost 的方式构造场景。
TEST(TargetTrackerBehaviorTest, CoastingFallbackChoosesNearestToFrameCenter) {
TargetTracker::Config cfg;
cfg.max_lost_frames = 2;
TargetTracker tracker(cfg);
// F1：角落目标（高置信度）先建立并被锁定
tracker.Update({MakeDet(10.0, 10.0, 30.0, 30.0, 0.95)});
// F2：画面中心附近目标建立；角落目标丢失(1)
tracker.Update({MakeDet(300.0, 220.0, 340.0, 260.0, 0.9)});
// F3：远处目标建立；角落丢失(2)、中心目标丢失(1)
tracker.Update({MakeDet(600.0, 450.0, 620.0, 470.0, 0.85)});
// F4：全部丢失：角落丢失(3) > max_lost 死亡解除锁定；
// 其余两条 Coast 存活 → 重新选举选离画面中心最近者
TrackResult r = tracker.Update({}, FrameShape{480, 640});
ASSERT_TRUE(r.tracked);
EXPECT_TRUE(r.is_predicted);
EXPECT_NEAR(r.center.first, 320.0, 25.0); // 靠近画面中心 (320,240) 者当选
EXPECT_NEAR(r.center.second, 240.0, 25.0);
EXPECT_EQ(r.n_active, 2); // 角落轨道已回收
}
// 锁定延续优先于置信度选举：锁定轨道实测时即使置信度较低也保持主目标（§6.3 ①）
TEST(TargetTrackerBehaviorTest, LockedPrimaryKeptWhileMeasured) {
TargetTracker tracker;
TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.4)});
ASSERT_TRUE(r.primary_id.has_value());
const int locked_id = *r.primary_id;
// 高置信度新目标出现并持续，锁定目标也持续实测 → primary_id 不变
for (int i = 0; i < 5; ++i) {
r = tracker.Update({MakeDet(100.0 + i, 100.0, 120.0 + i, 120.0, 0.4),
MakeDet(400.0, 300.0, 430.0, 330.0, 0.95)});
ASSERT_TRUE(r.primary_id.has_value());
EXPECT_EQ(*r.primary_id, locked_id);
EXPECT_FALSE(r.is_predicted);
}
}
// frame_shape 缺省按 640×480 画面中心 (320,240) 计算（§8.2 / §2.2）：
// 两条 Coast 轨道分别位于默认中心两侧，当选者应为更近的一侧
TEST(TargetTrackerBehaviorTest, DefaultFrameCenterIs320x240) {
TargetTracker::Config cfg;
cfg.max_lost_frames = 2;
TargetTracker tracker(cfg);
// F1：角落目标先建立并被锁定（高置信度）
tracker.Update({MakeDet(0.0, 0.0, 20.0, 20.0, 0.95)});
// F2：左侧目标（中心 (280,240)，距默认中心 40px）建立
tracker.Update({MakeDet(270.0, 230.0, 290.0, 250.0, 0.9)});
// F3：右侧目标（中心 (500,240)，距默认中心 180px）建立
tracker.Update({MakeDet(490.0, 230.0, 510.0, 250.0, 0.85)});
// F4：全部丢失：锁定轨道死亡解除锁定，两条 Coast 轨道兜底选举
// （不传 frame_shape → 按默认画面中心 (320,240)；两候选间距足够大，
// 速度估计漂移不影响最近者判定）
TrackResult r = tracker.Update({});
ASSERT_TRUE(r.tracked);
EXPECT_TRUE(r.is_predicted);
EXPECT_NEAR(r.center.first, 280.0, 30.0); // 左侧更近，当选
EXPECT_NEAR(r.center.second, 240.0, 30.0);
}
} // namespace
reference-c++/target_tracker/tests/tracklet_test.cpp
+177
View File
@@ -0,0 +1,177 @@
// =============================================================================
// tracklet_test.cpp —— Tracklet 与几何工具单元测试
//
// 覆盖 docs/视觉追踪技术路线.md：
// - IoU 边界（空框 / 零交集 / 完全重合=1，§5.2）
// - IsDead / IsCoasting 判定（§3.2 派生状态，§6.1 严格大于）
// - Predict / Update / MarkMiss 计数语义（§7 末尾伪代码）
// - BoxXYXY ↔ BoxCXCYWH 互转（§7）
// =============================================================================
#include <gtest/gtest.h>
#include "target_tracker/tracklet.h"
using drone::tracker::BoxCXCYWH;
using drone::tracker::BoxXYXY;
using drone::tracker::ComputeIoU;
using drone::tracker::ToCenterSize;
using drone::tracker::ToXYXY;
using drone::tracker::Tracklet;
namespace {
// ---- 框表示互转（§7） ----
TEST(BoxConversionTest, RoundTripPreservesValues) {
const BoxXYXY box{10.0, 20.0, 50.0, 80.0};
const BoxCXCYWH c = ToCenterSize(box);
EXPECT_DOUBLE_EQ(c.cx, 30.0); // (10+50)/2
EXPECT_DOUBLE_EQ(c.cy, 50.0); // (20+80)/2
EXPECT_DOUBLE_EQ(c.w, 40.0); // 50−10
EXPECT_DOUBLE_EQ(c.h, 60.0); // 80−20
const BoxXYXY back = ToXYXY(c);
EXPECT_DOUBLE_EQ(back.x1, box.x1);
EXPECT_DOUBLE_EQ(back.y1, box.y1);
EXPECT_DOUBLE_EQ(back.x2, box.x2);
EXPECT_DOUBLE_EQ(back.y2, box.y2);
}
// ---- IoU 边界（§5.2） ----
TEST(IoUTest, IdenticalBoxesReturnOne) {
const BoxXYXY a{10.0, 10.0, 30.0, 30.0};
EXPECT_NEAR(ComputeIoU(a, a), 1.0, 1e-12); // 完全重合 = 1
}
TEST(IoUTest, DisjointBoxesReturnZero) {
const BoxXYXY a{0.0, 0.0, 10.0, 10.0};
const BoxXYXY b{20.0, 20.0, 30.0, 30.0};
EXPECT_DOUBLE_EQ(ComputeIoU(a, b), 0.0); // 零交集
}
TEST(IoUTest, TouchingEdgesReturnZero) {
// 仅边/角相接：交集宽高为 0 → 交集面积 ≤ 0 返回 0（§5.2）
const BoxXYXY a{0.0, 0.0, 10.0, 10.0};
const BoxXYXY b{10.0, 0.0, 20.0, 10.0}; // 右边缘相接
EXPECT_DOUBLE_EQ(ComputeIoU(a, b), 0.0);
const BoxXYXY c{10.0, 10.0, 20.0, 20.0}; // 角点相接
EXPECT_DOUBLE_EQ(ComputeIoU(a, c), 0.0);
}
TEST(IoUTest, EmptyBoxReturnsZero) {
const BoxXYXY normal{0.0, 0.0, 10.0, 10.0};
const BoxXYXY zero_area{5.0, 5.0, 5.0, 5.0}; // 零面积
const BoxXYXY inverted{10.0, 10.0, 0.0, 0.0}; // 反向（负宽高）
EXPECT_DOUBLE_EQ(ComputeIoU(normal, zero_area), 0.0);
EXPECT_DOUBLE_EQ(ComputeIoU(zero_area, normal), 0.0);
EXPECT_DOUBLE_EQ(ComputeIoU(normal, inverted), 0.0);
EXPECT_DOUBLE_EQ(ComputeIoU(zero_area, zero_area), 0.0);
}
TEST(IoUTest, PartialOverlapIsCorrect) {
// A = [0,0,10,10]（面积 100），B = [5,5,15,15]（面积 100），交集 25
const BoxXYXY a{0.0, 0.0, 10.0, 10.0};
const BoxXYXY b{5.0, 5.0, 15.0, 15.0};
EXPECT_NEAR(ComputeIoU(a, b), 25.0 / 175.0, 1e-12);
EXPECT_NEAR(ComputeIoU(b, a), 25.0 / 175.0, 1e-12); // 对称性
}
// ---- 构造与派生状态（§3.2 / §6.1） ----
TEST(TrackletTest, ConstructionInitialFields) {
const BoxCXCYWH meas{100.0, 100.0, 20.0, 20.0};
Tracklet t(7, meas, 0.9, 1, 8);
EXPECT_EQ(t.track_id, 7);
EXPECT_EQ(t.lost_count, 0);
EXPECT_EQ(t.max_lost, 8);
EXPECT_DOUBLE_EQ(t.confidence, 0.9);
EXPECT_EQ(t.cls_id, 1);
EXPECT_EQ(t.age, 1); // 创建时为 1（§3.2）
EXPECT_EQ(t.hits, 1); // 创建时为 1（§3.2）
EXPECT_DOUBLE_EQ(t.last_center.first, 100.0);
EXPECT_DOUBLE_EQ(t.last_center.second, 100.0);
ASSERT_TRUE(t.last_box.has_value()); // 创建当帧即赋 last_box（§7 步骤 5）
EXPECT_DOUBLE_EQ(t.last_box->x1, 90.0);
EXPECT_DOUBLE_EQ(t.last_box->y1, 90.0);
EXPECT_DOUBLE_EQ(t.last_box->x2, 110.0);
EXPECT_DOUBLE_EQ(t.last_box->y2, 110.0);
EXPECT_FALSE(t.IsDead());
EXPECT_FALSE(t.IsCoasting());
}
// IsDead 使用严格大于：lost_count == max_lost 仍存活（§6.1 注记 2）
TEST(TrackletTest, IsDeadUsesStrictlyGreater) {
Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8);
for (int i = 0; i < 8; ++i) {
t.MarkMiss();
EXPECT_FALSE(t.IsDead()) << "lost_count=" << t.lost_count
<< " 未超过 max_lost 不应判死";
}
EXPECT_EQ(t.lost_count, 8);
t.MarkMiss(); // lost_count = 9 > 8
EXPECT_TRUE(t.IsDead());
}
TEST(TrackletTest, IsCoastingWhenLostPositive) {
Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8);
EXPECT_FALSE(t.IsCoasting());
t.MarkMiss();
EXPECT_TRUE(t.IsCoasting()); // lost_count > 0 即 Coast（§3.2）
}
// ---- 生命周期方法计数语义（§7） ----
TEST(TrackletTest, PredictAdvancesAgeAndUpdatesLastState) {
Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8);
const int age_before = t.age;
t.Predict(); // 速度为零：位置不变，但 age+1、last_box 更新为预测框
EXPECT_EQ(t.age, age_before + 1);
ASSERT_TRUE(t.last_box.has_value());
EXPECT_NEAR(t.last_center.first, 100.0, 1e-9);
EXPECT_NEAR(t.last_center.second, 100.0, 1e-9);
// hits / lost_count / confidence 不受预测影响
EXPECT_EQ(t.hits, 1);
EXPECT_EQ(t.lost_count, 0);
EXPECT_DOUBLE_EQ(t.confidence, 0.9);
}
TEST(TrackletTest, UpdateResetsLostAndIncrementsHits) {
Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8);
t.MarkMiss();
t.MarkMiss();
ASSERT_EQ(t.lost_count, 2);
ASSERT_TRUE(t.IsCoasting());
const int age_before = t.age;
const BoxXYXY box{150.0, 160.0, 170.0, 180.0};
t.Update(box, 0.75, 2);
EXPECT_EQ(t.lost_count, 0); // 匹配成功清零（§7）
EXPECT_EQ(t.hits, 2); // hits+1
EXPECT_EQ(t.age, age_before + 1); // age+1
EXPECT_DOUBLE_EQ(t.confidence, 0.75);
EXPECT_EQ(t.cls_id, 2);
EXPECT_DOUBLE_EQ(t.last_center.first, 160.0); // (150+170)/2
EXPECT_DOUBLE_EQ(t.last_center.second, 170.0); // (160+180)/2
ASSERT_TRUE(t.last_box.has_value());
EXPECT_DOUBLE_EQ(t.last_box->x1, box.x1);
ASSERT_TRUE(t.last_raw_box.has_value()); // 实测框同步记录
EXPECT_DOUBLE_EQ(t.last_raw_box->x2, box.x2);
EXPECT_FALSE(t.IsCoasting());
}
TEST(TrackletTest, MarkMissIncrementsLostAndAge) {
Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8);
const int age_before = t.age;
t.MarkMiss();
EXPECT_EQ(t.lost_count, 1);
EXPECT_EQ(t.age, age_before + 1);
// MarkMiss 不影响 hits / confidence
EXPECT_EQ(t.hits, 1);
EXPECT_DOUBLE_EQ(t.confidence, 0.9);
}
} // namespace