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/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