keita
df91b1110e
归档 legacy/：stereo_yolo/stereo_ranger/target_tracker 三包 + 175 条测试 + demo/配置/权重/三份规格；运行链路不再依赖，默认 pytest 446 条，归档测试按需运行。 目标有效性第二判据改为 LOS 运动学一致性（pod_bridge/los_consistency.py），删除 require_vision；pod_locked() 叠加该判据，状态机随之后退重搜。 自定义返航（task_runner/return_home.py）：保持转场高度回起飞点 → 恢复初始航向 → 垂直降落，不再爬 RTL_RETURN_ALT；未起飞/无起飞点/超时/异常仍退回 PX4 原生 RTL。 任务链安全兜底：主循环异常→紧急返航、stop 幂等、landing_stop 保留遥测、in_air 流复用+超时、解锁/起飞与吊舱识别仅执行一次、rtl_timeout 上限。 入口共享件拆到 utils/sitl_support.py；pod_sitl_test.py→l1_sitl_test.py；删除旧架构 test.py；相机脚本移入 tools/。 新增 .gitignore 并剔除历史误跟踪的 59 个 .pyc；requirements 按实际依赖重写；文档全面对齐，新增 legacy/README.md 与 tools/README.md。
365 lines
15 KiB
Python
365 lines
15 KiB
Python
"""target_tracker_test.py —— TargetTracker 单元测试。
|
||
|
||
移植自 reference-c++/target_tracker/tests/target_tracker_test.cpp。
|
||
逐条覆盖 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. 置信度优先：两轨道竞争同一检测，置信度高者获胜
|
||
另含 NaN/Inf 防御与 §2.3/§6.3/§9.5 行为测试。
|
||
"""
|
||
|
||
import math
|
||
|
||
import pytest
|
||
|
||
from target_tracker.target_tracker import (Detection, FrameShape, TargetTracker,
|
||
TrackerConfig)
|
||
|
||
|
||
def make_det(x1, y1, x2, y2, conf, cls=0):
|
||
return Detection(x1, y1, x2, y2, conf, cls)
|
||
|
||
|
||
# ============================================================================
|
||
# 验收用例 1（§9.4-1）：空检测帧序列
|
||
# ============================================================================
|
||
def test_empty_frames_coast_then_lost():
|
||
tracker = TargetTracker() # 默认配置 max_lost_frames = 8
|
||
assert tracker.get_config().max_lost_frames == 8
|
||
|
||
# 第 1 帧：给出一个检测，建立轨道并锁定为主目标
|
||
r = tracker.update([make_det(100.0, 100.0, 140.0, 140.0, 0.9)])
|
||
assert r.tracked
|
||
assert r.primary_id is not None
|
||
assert not r.is_predicted # 当帧有实测
|
||
assert r.lost_frames == 0
|
||
assert r.n_active == 1
|
||
assert r.box is not None
|
||
assert r.raw is not None # 当帧关联到原始检测框
|
||
|
||
# 后续空检测帧：应持续 Coast 共 max_lost_frames 帧（is_predicted=True）
|
||
for i in range(1, 9):
|
||
r = tracker.update([])
|
||
assert r.tracked, f"第 {i} 帧 Coast 期间不应判丢"
|
||
assert r.is_predicted, f"第 {i} 帧应为纯预测输出"
|
||
assert r.lost_frames == i
|
||
assert r.primary_id is not None
|
||
assert r.box is not None # Coast 期间继续输出预测框
|
||
assert r.raw is None # Coast 时 raw 为空（§2.3）
|
||
assert r.confidence == pytest.approx(0.9) # 继承最近实测置信度（§2.3）
|
||
|
||
# 第 max_lost_frames + 1 帧：轨道死亡回收 → tracked=False
|
||
r = tracker.update([])
|
||
assert not r.tracked
|
||
assert r.primary_id is None
|
||
assert r.box is None
|
||
assert r.lost_frames == 0 # 丢失时 lost_frames=0（§2.3）
|
||
assert r.n_active == 0
|
||
|
||
|
||
# ============================================================================
|
||
# 验收用例 2（§9.4-2）：ID 单调性
|
||
# ============================================================================
|
||
def test_id_strictly_increasing_no_reuse():
|
||
cfg = TrackerConfig()
|
||
cfg.max_lost_frames = 1 # 缩短 Coast 窗口，加速轨道死亡
|
||
tracker = TargetTracker(cfg)
|
||
|
||
# 目标 A 出现 → id 0
|
||
r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.9)])
|
||
assert r.primary_id is not None
|
||
id_a = r.primary_id
|
||
assert id_a == 0
|
||
|
||
# 目标 A 消失至死亡（max_lost=1：第 2 帧 Coast、第 3 帧死亡回收）
|
||
tracker.update([])
|
||
r = tracker.update([])
|
||
assert not r.tracked
|
||
assert tracker.active_count() == 0
|
||
|
||
# 目标 B 出现（同一位置）：ID 不复用，严格递增 → id 1
|
||
r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.9)])
|
||
assert r.primary_id is not None
|
||
id_b = r.primary_id
|
||
assert id_b == 1
|
||
assert id_b > id_a
|
||
|
||
# 目标 C 同帧新建 → id 2（同帧多目标亦严格递增）
|
||
r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.9),
|
||
make_det(400.0, 300.0, 430.0, 330.0, 0.8)])
|
||
assert tracker.active_count() == 2
|
||
max_id = max(tracker.active_tracks().keys())
|
||
assert max_id == 2
|
||
|
||
# Reset 后计数器归零（§6.2）：新轨道重新从 id 0 开始
|
||
tracker.reset()
|
||
assert tracker.active_count() == 0
|
||
assert not tracker.last_result().tracked
|
||
r = tracker.update([make_det(50.0, 50.0, 70.0, 70.0, 0.9)])
|
||
assert r.primary_id is not None
|
||
assert r.primary_id == 0
|
||
|
||
|
||
# ============================================================================
|
||
# 验收用例 3（§9.4-3）：锁定不被抢占
|
||
# ============================================================================
|
||
def test_lock_not_stolen_and_switch_after_death():
|
||
cfg = TrackerConfig()
|
||
cfg.max_lost_frames = 3
|
||
tracker = TargetTracker(cfg)
|
||
|
||
# 低置信度目标 A 建立并锁定
|
||
r = tracker.update([make_det(100.0, 100.0, 140.0, 140.0, 0.4)])
|
||
assert r.primary_id is not None
|
||
id_a = r.primary_id
|
||
|
||
# A 消失进入 Coast；期间出现更高置信度新目标 B（远离 A，不会被误关联）
|
||
r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)])
|
||
assert r.primary_id is not None
|
||
assert r.primary_id == id_a, "Coast 期间锁定不应被高置信度目标抢占"
|
||
assert r.is_predicted
|
||
assert tracker.active_count() == 2 # A（Coast）与 B 并存
|
||
|
||
# 继续 Coast，锁定依旧延续
|
||
r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)])
|
||
assert r.primary_id == id_a
|
||
assert r.is_predicted
|
||
|
||
# A 继续丢失直至死亡（max_lost=3：lost 达 4 帧时回收）
|
||
for _ in range(2):
|
||
r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)])
|
||
# A 已死亡删除 → 锁定解除，重新选举切换到 B（此时 B 已确认且实测）
|
||
assert r.tracked
|
||
assert r.primary_id != id_a, "锁定目标死亡后应切换到新目标"
|
||
assert not r.is_predicted
|
||
|
||
# 后续 B 持续作为主目标
|
||
r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)])
|
||
assert r.primary_id is not None
|
||
assert r.primary_id != id_a
|
||
|
||
|
||
# ============================================================================
|
||
# 验收用例 4（§9.4-4）：min_hits 回退
|
||
# ============================================================================
|
||
def test_min_hits_fallback_allows_new_track():
|
||
cfg = TrackerConfig()
|
||
cfg.min_hits = 5 # 抬高确认门槛，保证测试期间轨道始终未确认
|
||
tracker = TargetTracker(cfg)
|
||
|
||
# 首帧新建轨道 hits=1 < min_hits：候选池 confirmed 为空 → 退回 measured
|
||
r = tracker.update([make_det(200.0, 150.0, 240.0, 190.0, 0.6)])
|
||
assert r.tracked, "无已确认轨道时新轨道应能当选主目标"
|
||
assert r.primary_id is not None
|
||
assert r.primary_id == 0
|
||
assert not r.is_predicted
|
||
|
||
# 再跟几帧（hits 增长但仍未达 min_hits=5），依旧当选
|
||
for _ in range(3):
|
||
r = tracker.update([make_det(200.0, 150.0, 240.0, 190.0, 0.6)])
|
||
assert r.tracked
|
||
assert r.primary_id == 0
|
||
|
||
|
||
# ============================================================================
|
||
# 验收用例 5（§9.4-5）：cost 边界（cost = 1.0 恰好不匹配）
|
||
# ============================================================================
|
||
def test_cost_exactly_one_does_not_match():
|
||
cfg = TrackerConfig()
|
||
cfg.max_lost_frames = 1
|
||
cfg.max_association_dist = 200.0
|
||
cfg.dist_weight = 1.0 # 纯距离项，IoU 项权重为 0 且两框零交集
|
||
cfg.iou_weight = 0.0
|
||
tracker = TargetTracker(cfg)
|
||
|
||
# 建立轨道：中心 (100,100)
|
||
r = tracker.update([make_det(90.0, 90.0, 110.0, 110.0, 0.9)])
|
||
assert r.primary_id is not None
|
||
id_a = r.primary_id
|
||
|
||
# 下一帧检测中心 (100,300)：距离 = 200 = Dmax → cost = 1.0，恰好不匹配
|
||
r = tracker.update([make_det(90.0, 290.0, 110.0, 310.0, 0.9)])
|
||
# 未匹配 → 原轨道 Coast、检测新建轨道；两个目标并存
|
||
assert tracker.active_count() == 2, "cost=1.0 不应匹配，应新建轨道"
|
||
# 原轨道（锁定）进入 Coast：lost_frames=1
|
||
assert r.tracked
|
||
assert r.is_predicted
|
||
assert r.primary_id == id_a
|
||
assert r.lost_frames == 1
|
||
|
||
# 对照：距离略小于阈值（199px → cost=0.995 < 1.0）应匹配成功
|
||
tracker2 = TargetTracker(cfg)
|
||
r = tracker2.update([make_det(90.0, 90.0, 110.0, 110.0, 0.9)])
|
||
id_b = r.primary_id
|
||
r = tracker2.update([make_det(90.0, 289.0, 110.0, 309.0, 0.9)])
|
||
assert tracker2.active_count() == 1, "cost<1.0 应匹配成功，不应新建轨道"
|
||
assert r.primary_id is not None
|
||
assert r.primary_id == id_b
|
||
assert not r.is_predicted # 实测命中
|
||
|
||
|
||
# ============================================================================
|
||
# 验收用例 6（§9.4-6）：置信度优先
|
||
# ============================================================================
|
||
def test_higher_confidence_track_wins_detection():
|
||
tracker = TargetTracker()
|
||
|
||
# 同帧建立两条轨道：A 置信度 0.5、B 置信度 0.9，相距 80px（不互相重叠）
|
||
r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.5),
|
||
make_det(180.0, 100.0, 200.0, 120.0, 0.9)])
|
||
assert tracker.active_count() == 2
|
||
|
||
# 找出两条轨道 ID（按置信度区分）
|
||
id_low = None
|
||
id_high = None
|
||
for track_id, track in tracker.active_tracks().items():
|
||
if track.confidence > 0.8:
|
||
id_high = track_id
|
||
else:
|
||
id_low = track_id
|
||
assert id_low is not None
|
||
assert id_high is not None
|
||
|
||
# 下一帧只给一个检测，放在两轨道预测中心之间（各距 40px，对称）：
|
||
# 距离项对两者相同，贪婪按置信度降序 → 高置信度轨道 B 优先匹配
|
||
r = tracker.update([make_det(140.0, 100.0, 160.0, 120.0, 0.7)])
|
||
|
||
tracks = tracker.active_tracks()
|
||
assert len(tracks) == 2
|
||
# 获胜者：命中（lost=0、hits=2、置信度更新）
|
||
assert tracks[id_high].lost_count == 0, "高置信度轨道应获胜"
|
||
assert tracks[id_high].hits == 2
|
||
assert tracks[id_high].confidence == pytest.approx(0.7)
|
||
# 失败者：未匹配进入 Coast（lost=1、hits 不变）
|
||
assert tracks[id_low].lost_count == 1, "低置信度轨道应落选"
|
||
assert tracks[id_low].hits == 1
|
||
assert tracks[id_low].confidence == pytest.approx(0.5)
|
||
|
||
|
||
# ============================================================================
|
||
# NaN/Inf 防御（评审修复项）
|
||
# ============================================================================
|
||
def test_non_finite_detections_are_discarded():
|
||
tracker = TargetTracker()
|
||
nan = math.nan
|
||
inf = math.inf
|
||
|
||
# 第 1 帧：建立正常轨道
|
||
r = tracker.update([make_det(100.0, 100.0, 140.0, 140.0, 0.9)])
|
||
assert r.tracked
|
||
|
||
# 第 2 帧：混入 NaN 坐标 / Inf 坐标 / NaN 置信度检测与一个正常检测：
|
||
# 非有限检测全部被丢弃，仅正常检测参与关联，不新建轨道
|
||
r = tracker.update([make_det(nan, 100.0, 140.0, 140.0, 0.9),
|
||
make_det(100.0, -inf, 140.0, 140.0, 0.9),
|
||
make_det(100.0, 100.0, nan, 140.0, 0.9),
|
||
make_det(102.0, 102.0, 142.0, 142.0, nan),
|
||
make_det(101.0, 101.0, 141.0, 141.0, 0.9)])
|
||
assert tracker.active_count() == 1, "非有限检测不应新建轨道"
|
||
assert r.tracked
|
||
assert not r.is_predicted # 正常检测命中，实测输出
|
||
# 主目标输出未被 NaN 污染
|
||
assert math.isfinite(r.center[0])
|
||
assert math.isfinite(r.center[1])
|
||
assert r.box is not None
|
||
assert math.isfinite(r.box.x1) and math.isfinite(r.box.y1)
|
||
assert math.isfinite(r.box.x2) and math.isfinite(r.box.y2)
|
||
assert math.isfinite(r.confidence)
|
||
assert r.raw is not None # 实测帧 raw 有值
|
||
|
||
# 第 3 帧：仅含 NaN 检测 → 等价空帧，轨道 Coast 不崩溃
|
||
r = tracker.update([make_det(nan, nan, nan, nan, nan)])
|
||
assert r.tracked
|
||
assert r.is_predicted
|
||
assert r.lost_frames == 1
|
||
assert r.raw is None # Coast 时 raw 为空（§2.3）
|
||
assert math.isfinite(r.center[0])
|
||
|
||
# 持续 NaN 检测直至轨道死亡回收，全程不崩溃
|
||
for _ in range(10):
|
||
r = tracker.update([make_det(nan, 0.0, 10.0, 10.0, 0.9)])
|
||
assert not r.tracked
|
||
assert tracker.active_count() == 0
|
||
|
||
|
||
# ============================================================================
|
||
# 补充行为测试（§2.3 / §6.3 / §9.5）
|
||
# ============================================================================
|
||
|
||
def test_lost_result_fields_are_empty():
|
||
tracker = TargetTracker()
|
||
r = tracker.update([]) # 空帧序列起点：无任何轨道
|
||
assert not r.tracked
|
||
assert r.primary_id is None
|
||
assert r.box is None
|
||
assert r.raw is None
|
||
assert r.lost_frames == 0
|
||
assert r.n_active == 0
|
||
assert r.confidence == 0.0
|
||
|
||
# LastResult 返回同一快照（§9.5 跨线程读取入口）
|
||
cached = tracker.last_result()
|
||
assert not cached.tracked
|
||
|
||
|
||
def test_coasting_fallback_chooses_nearest_to_frame_center():
|
||
# Coast 兜底选举：无实测轨道时选离画面中心最近的 Coast 轨道（§6.3）。
|
||
# 锁定延续优先：需先让锁定轨道死亡解除锁定，才会进入重新选举分支。
|
||
cfg = TrackerConfig()
|
||
cfg.max_lost_frames = 2
|
||
tracker = TargetTracker(cfg)
|
||
|
||
# F1：角落目标（高置信度）先建立并被锁定
|
||
tracker.update([make_det(10.0, 10.0, 30.0, 30.0, 0.95)])
|
||
# F2：画面中心附近目标建立；角落目标丢失(1)
|
||
tracker.update([make_det(300.0, 220.0, 340.0, 260.0, 0.9)])
|
||
# F3：远处目标建立；角落丢失(2)、中心目标丢失(1)
|
||
tracker.update([make_det(600.0, 450.0, 620.0, 470.0, 0.85)])
|
||
# F4：全部丢失：角落丢失(3) > max_lost 死亡解除锁定；
|
||
# 其余两条 Coast 存活 → 重新选举选离画面中心最近者
|
||
r = tracker.update([], FrameShape(480, 640))
|
||
assert r.tracked
|
||
assert r.is_predicted
|
||
assert r.center[0] == pytest.approx(320.0, abs=25.0) # 靠近画面中心 (320,240)
|
||
assert r.center[1] == pytest.approx(240.0, abs=25.0)
|
||
assert r.n_active == 2 # 角落轨道已回收
|
||
|
||
|
||
def test_locked_primary_kept_while_measured():
|
||
# 锁定延续优先于置信度选举（§6.3 ①）
|
||
tracker = TargetTracker()
|
||
r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.4)])
|
||
assert r.primary_id is not None
|
||
locked_id = r.primary_id
|
||
|
||
# 高置信度新目标出现并持续，锁定目标也持续实测 → primary_id 不变
|
||
for i in range(5):
|
||
r = tracker.update([make_det(100.0 + i, 100.0, 120.0 + i, 120.0, 0.4),
|
||
make_det(400.0, 300.0, 430.0, 330.0, 0.95)])
|
||
assert r.primary_id is not None
|
||
assert r.primary_id == locked_id
|
||
assert not r.is_predicted
|
||
|
||
|
||
def test_default_frame_center_is_320x240():
|
||
# frame_shape 缺省按 640×480 画面中心 (320,240) 计算（§8.2 / §2.2）
|
||
cfg = TrackerConfig()
|
||
cfg.max_lost_frames = 2
|
||
tracker = TargetTracker(cfg)
|
||
|
||
# F1：角落目标先建立并被锁定（高置信度）
|
||
tracker.update([make_det(0.0, 0.0, 20.0, 20.0, 0.95)])
|
||
# F2：左侧目标（中心 (280,240)，距默认中心 40px）建立
|
||
tracker.update([make_det(270.0, 230.0, 290.0, 250.0, 0.9)])
|
||
# F3：右侧目标（中心 (500,240)，距默认中心 180px）建立
|
||
tracker.update([make_det(490.0, 230.0, 510.0, 250.0, 0.85)])
|
||
# F4：全部丢失：锁定轨道死亡解除锁定，两条 Coast 轨道兜底选举
|
||
r = tracker.update([])
|
||
assert r.tracked
|
||
assert r.is_predicted
|
||
assert r.center[0] == pytest.approx(280.0, abs=30.0) # 左侧更近，当选
|
||
assert r.center[1] == pytest.approx(240.0, abs=30.0)
|