first commit

utils/tests/target_tracker/kalman_box_filter_test.py

@@ -0,0 +1,107 @@
+"""kalman_box_filter_test.py —— KalmanBoxFilter 单元测试。
+
+移植自 reference-c++/target_tracker/tests/kalman_box_filter_test.cpp。
+覆盖（视觉追踪技术路线 §4）：初始化状态、静止目标收敛、匀速目标速度收敛
+且预测超前一个 dt、未初始化 predict 返回 None。
+"""
+
+import math
+
+import pytest
+
+from target_tracker.kalman_box_filter import BoxCXCYWH, KalmanBoxFilter
+
+
+def test_uninitialized_predict_returns_none():
+    kf = KalmanBoxFilter()
+    assert not kf.initialized
+    assert kf.predict() is None
+
+    # 未初始化时 update 无副作用
+    kf.update(BoxCXCYWH(100.0, 100.0, 20.0, 20.0))
+    assert not kf.initialized
+    assert kf.predict() is None
+    for v in kf.state:
+        assert v == 0.0
+
+
+def test_initiate_sets_state_correctly():
+    kf = KalmanBoxFilter()
+    kf.initiate(BoxCXCYWH(320.0, 240.0, 40.0, 30.0))
+    assert kf.initialized
+
+    expected = [320.0, 240.0, 40.0, 30.0, 0.0, 0.0, 0.0, 0.0]
+    state = kf.state
+    for i, want in enumerate(expected):
+        assert state[i] == want, f"index={i}"
+
+    # 初始化后 predict 返回状态前 4 维（速度为零 → 位置不变，§4.4）
+    pred = kf.predict()
+    assert pred is not None
+    assert pred.cx == pytest.approx(320.0, abs=1e-9)
+    assert pred.cy == pytest.approx(240.0, abs=1e-9)
+    assert pred.w == pytest.approx(40.0, abs=1e-9)
+    assert pred.h == pytest.approx(30.0, abs=1e-9)
+
+
+def test_static_target_converges_to_observation():
+    kf = KalmanBoxFilter()
+    z = BoxCXCYWH(100.0, 150.0, 40.0, 60.0)
+    kf.initiate(BoxCXCYWH(104.0, 156.0, 42.0, 62.0))  # 带初始偏差，验证收敛能力
+
+    for _ in range(50):
+        kf.predict()
+        kf.update(z)
+
+    state = kf.state
+    assert state[0] == pytest.approx(z.cx, abs=0.5)   # cx
+    assert state[1] == pytest.approx(z.cy, abs=0.5)   # cy
+    assert state[2] == pytest.approx(z.w, abs=0.5)    # w
+    assert state[3] == pytest.approx(z.h, abs=0.5)    # h
+    # 静止目标速度应收敛到约 0
+    assert state[4] == pytest.approx(0.0, abs=0.1)
+    assert state[5] == pytest.approx(0.0, abs=0.1)
+
+
+def test_constant_velocity_converges_and_predict_leads_one_dt():
+    kf = KalmanBoxFilter()
+    vx = 3.0   # 像素/帧
+    vy = 2.0   # 像素/帧
+
+    # 目标从 (100, 100) 出发匀速运动，每帧观测一次（predict → update）
+    z = BoxCXCYWH(100.0, 100.0, 30.0, 30.0)
+    kf.initiate(z)
+    for _ in range(60):
+        kf.predict()
+        z.cx += vx
+        z.cy += vy
+        kf.update(z)
+
+    state = kf.state
+    # 速度收敛到真实速度（像素/帧）
+    assert state[4] == pytest.approx(vx, abs=0.3), "vx 未收敛"
+    assert state[5] == pytest.approx(vy, abs=0.3), "vy 未收敛"
+    # 位置贴合当前观测
+    assert state[0] == pytest.approx(z.cx, abs=1.5)
+    assert state[1] == pytest.approx(z.cy, abs=1.5)
+
+    # 再预测一帧：应超前当前观测一个 dt（≈ 速度 × 1 帧）
+    pred = kf.predict()
+    assert pred is not None
+    assert pred.cx == pytest.approx(z.cx + vx, abs=1.0), "预测未超前一个 dt"
+    assert pred.cy == pytest.approx(z.cy + vy, abs=1.0), "预测未超前一个 dt"
+
+
+def test_small_target_noise_scaling_stable():
+    # 动态噪声不退化：小尺寸目标多帧观测仍稳定收敛（§4.2 噪声与尺寸成比例）
+    kf = KalmanBoxFilter()
+    z = BoxCXCYWH(50.0, 60.0, 4.0, 4.0)  # 小目标（4×4 像素）
+    kf.initiate(z)
+    for _ in range(30):
+        kf.predict()
+        kf.update(z)
+    state = kf.state
+    assert state[0] == pytest.approx(z.cx, abs=0.5)
+    assert state[1] == pytest.approx(z.cy, abs=0.5)
+    assert math.isfinite(state[2])
+    assert math.isfinite(state[3])

utils/tests/target_tracker/target_tracker_test.py

@@ -0,0 +1,364 @@
+"""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)

utils/tests/target_tracker/tracklet_test.py

@@ -0,0 +1,159 @@
+"""tracklet_test.py —— Tracklet 与几何工具单元测试。
+
+移植自 reference-c++/target_tracker/tests/tracklet_test.cpp。
+覆盖（视觉追踪技术路线 §3.2/§5.2/§6.1/§7）：框互转、IoU 边界、
+IsDead/IsCoasting 判定（严格大于）、Predict/Update/MarkMiss 计数语义。
+"""
+
+import pytest
+
+from target_tracker.tracklet import (BoxXYXY, BoxCXCYWH, Tracklet, compute_iou,
+                                     to_center_size, to_xyxy)
+
+
+# ---- 框表示互转（§7） ----
+
+def test_round_trip_preserves_values():
+    box = BoxXYXY(10.0, 20.0, 50.0, 80.0)
+    c = to_center_size(box)
+    assert c.cx == 30.0   # (10+50)/2
+    assert c.cy == 50.0   # (20+80)/2
+    assert c.w == 40.0    # 50−10
+    assert c.h == 60.0    # 80−20
+
+    back = to_xyxy(c)
+    assert back.x1 == box.x1
+    assert back.y1 == box.y1
+    assert back.x2 == box.x2
+    assert back.y2 == box.y2
+
+
+# ---- IoU 边界（§5.2） ----
+
+def test_identical_boxes_return_one():
+    a = BoxXYXY(10.0, 10.0, 30.0, 30.0)
+    assert compute_iou(a, a) == pytest.approx(1.0, abs=1e-12)  # 完全重合 = 1
+
+
+def test_disjoint_boxes_return_zero():
+    a = BoxXYXY(0.0, 0.0, 10.0, 10.0)
+    b = BoxXYXY(20.0, 20.0, 30.0, 30.0)
+    assert compute_iou(a, b) == 0.0  # 零交集
+
+
+def test_touching_edges_return_zero():
+    # 仅边/角相接：交集宽高为 0 → 交集面积 ≤ 0 返回 0（§5.2）
+    a = BoxXYXY(0.0, 0.0, 10.0, 10.0)
+    b = BoxXYXY(10.0, 0.0, 20.0, 10.0)   # 右边缘相接
+    assert compute_iou(a, b) == 0.0
+    c = BoxXYXY(10.0, 10.0, 20.0, 20.0)  # 角点相接
+    assert compute_iou(a, c) == 0.0
+
+
+def test_empty_box_returns_zero():
+    normal = BoxXYXY(0.0, 0.0, 10.0, 10.0)
+    zero_area = BoxXYXY(5.0, 5.0, 5.0, 5.0)    # 零面积
+    inverted = BoxXYXY(10.0, 10.0, 0.0, 0.0)   # 反向（负宽高）
+    assert compute_iou(normal, zero_area) == 0.0
+    assert compute_iou(zero_area, normal) == 0.0
+    assert compute_iou(normal, inverted) == 0.0
+    assert compute_iou(zero_area, zero_area) == 0.0
+
+
+def test_partial_overlap_is_correct():
+    # A = [0,0,10,10]（面积 100），B = [5,5,15,15]（面积 100），交集 25
+    a = BoxXYXY(0.0, 0.0, 10.0, 10.0)
+    b = BoxXYXY(5.0, 5.0, 15.0, 15.0)
+    assert compute_iou(a, b) == pytest.approx(25.0 / 175.0, abs=1e-12)
+    assert compute_iou(b, a) == pytest.approx(25.0 / 175.0, abs=1e-12)  # 对称性
+
+
+# ---- 构造与派生状态（§3.2 / §6.1） ----
+
+def test_construction_initial_fields():
+    t = Tracklet(7, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 1, 8)
+
+    assert t.track_id == 7
+    assert t.lost_count == 0
+    assert t.max_lost == 8
+    assert t.confidence == 0.9
+    assert t.cls_id == 1
+    assert t.age == 1   # 创建时为 1（§3.2）
+    assert t.hits == 1  # 创建时为 1（§3.2）
+    assert t.last_center == (100.0, 100.0)
+    assert t.last_box is not None  # 创建当帧即赋 last_box（§7 步骤 5）
+    assert t.last_box.x1 == 90.0
+    assert t.last_box.y1 == 90.0
+    assert t.last_box.x2 == 110.0
+    assert t.last_box.y2 == 110.0
+    assert not t.is_dead
+    assert not t.is_coasting
+
+
+def test_is_dead_uses_strictly_greater():
+    # IsDead 使用严格大于：lost_count == max_lost 仍存活（§6.1 注记 2）
+    t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
+    for _ in range(8):
+        t.mark_miss()
+        assert not t.is_dead, f"lost_count={t.lost_count} 未超过 max_lost 不应判死"
+    assert t.lost_count == 8
+    t.mark_miss()  # lost_count = 9 > 8
+    assert t.is_dead
+
+
+def test_is_coasting_when_lost_positive():
+    t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
+    assert not t.is_coasting
+    t.mark_miss()
+    assert t.is_coasting  # lost_count > 0 即 Coast（§3.2）
+
+
+# ---- 生命周期方法计数语义（§7） ----
+
+def test_predict_advances_age_and_updates_last_state():
+    t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
+    age_before = t.age
+    t.predict()  # 速度为零：位置不变，但 age+1、last_box 更新为预测框
+    assert t.age == age_before + 1
+    assert t.last_box is not None
+    assert t.last_center[0] == pytest.approx(100.0, abs=1e-9)
+    assert t.last_center[1] == pytest.approx(100.0, abs=1e-9)
+    # hits / lost_count / confidence 不受预测影响
+    assert t.hits == 1
+    assert t.lost_count == 0
+    assert t.confidence == 0.9
+
+
+def test_update_resets_lost_and_increments_hits():
+    t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
+    t.mark_miss()
+    t.mark_miss()
+    assert t.lost_count == 2
+    assert t.is_coasting
+
+    age_before = t.age
+    box = BoxXYXY(150.0, 160.0, 170.0, 180.0)
+    t.update(box, 0.75, 2)
+
+    assert t.lost_count == 0   # 匹配成功清零（§7）
+    assert t.hits == 2         # hits+1
+    assert t.age == age_before + 1  # age+1
+    assert t.confidence == 0.75
+    assert t.cls_id == 2
+    assert t.last_center == (160.0, 170.0)  # ((150+170)/2, (160+180)/2)
+    assert t.last_box is not None
+    assert t.last_box.x1 == box.x1
+    assert t.last_raw_box is not None  # 实测框同步记录
+    assert t.last_raw_box.x2 == box.x2
+    assert not t.is_coasting
+
+
+def test_mark_miss_increments_lost_and_age():
+    t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
+    age_before = t.age
+    t.mark_miss()
+    assert t.lost_count == 1
+    assert t.age == age_before + 1
+    # MarkMiss 不影响 hits / confidence
+    assert t.hits == 1
+    assert t.confidence == 0.9

utils/tests/visual_track_control/frame_transform_test.py

@@ -0,0 +1,43 @@
+"""frame_transform_test.py —— rotate_body_to_ned 单元测试。
+
+移植自 reference-c++/visual_track_control/tests/frame_transform_test.cpp。
+覆盖（物理追踪思路 §2.3，A4）：零航向恒等、朝东时机体前向 → 东向、
+垂直分量不变、模长守恒。
+"""
+
+import math
+
+import pytest
+
+from visual_track_control.frame_transform import rotate_body_to_ned
+
+
+def test_zero_yaw_is_identity():
+    north, east, down = rotate_body_to_ned(1.0, 0.5, -2.0, 0.0)
+    assert north == pytest.approx(1.0, abs=1e-12)   # 北 = 前
+    assert east == pytest.approx(0.5, abs=1e-12)    # 东 = 右
+    assert down == pytest.approx(-2.0, abs=1e-12)
+
+
+def test_yaw_east_90_maps_forward_to_east():
+    north, east, _ = rotate_body_to_ned(2.0, 0.0, 0.0, math.pi / 2.0)  # 机头朝东
+    assert north == pytest.approx(0.0, abs=1e-9)  # 北向 ≈ 0
+    assert east == pytest.approx(2.0, abs=1e-9)   # 东向 = 前向速度
+
+
+def test_right_is_east_when_heading_north():
+    north, east, _ = rotate_body_to_ned(0.0, 1.0, 0.0, 0.0)
+    assert north == pytest.approx(0.0, abs=1e-12)
+    assert east == pytest.approx(1.0, abs=1e-12)
+
+
+def test_down_component_unchanged():
+    _, _, down = rotate_body_to_ned(1.0, 1.0, 3.5, math.pi / 3.0)
+    assert down == 3.5
+
+
+def test_preserves_horizontal_norm():
+    # 旋转为正交变换：水平合速度不变（限幅约束不被破坏的前提）
+    north, east, _ = rotate_body_to_ned(3.0, -4.0, 1.0, 0.7)
+    assert math.hypot(north, east) == pytest.approx(math.hypot(3.0, -4.0),
+                                                    abs=1e-9)

utils/tests/visual_track_control/line_of_sight_test.py

@@ -0,0 +1,63 @@
+"""line_of_sight_test.py —— PixelToAngle 单元测试。
+
+移植自 reference-c++/visual_track_control/tests/line_of_sight_test.cpp。
+覆盖（物理追踪思路 §4.1/§5.1，C2）：零偏移、画面边缘偏移 = 半视场角、
+奇对称、单调性、与线性近似的偏离（全程针孔模型）、构造期非法参数抛异常。
+"""
+
+import math
+
+import pytest
+
+from visual_track_control.line_of_sight import PixelToAngle
+
+
+def test_zero_offset_gives_zero_angle():
+    conv = PixelToAngle(1000.0, math.radians(90.0))
+    assert conv.convert(0.0) == 0.0
+
+
+def test_half_size_offset_gives_half_fov():
+    # 针孔模型：偏移 = size/2 时 angle = atan(tan(θ/2)) = θ/2
+    conv = PixelToAngle(1000.0, math.radians(90.0))
+    assert conv.convert(500.0) == pytest.approx(math.radians(45.0), abs=1e-12)
+    assert conv.convert(-500.0) == pytest.approx(-math.radians(45.0), abs=1e-12)
+
+
+def test_odd_symmetry():
+    conv = PixelToAngle(1920.0, math.radians(60.0))
+    for offset in (1.0, 100.0, 730.5):
+        assert conv.convert(-offset) == pytest.approx(-conv.convert(offset),
+                                                      abs=1e-15)
+
+
+def test_monotonic_increasing():
+    conv = PixelToAngle(1080.0, math.radians(35.0))
+    prev = conv.convert(-600.0)
+    for x in range(-500, 601, 100):
+        cur = conv.convert(float(x))
+        assert cur > prev
+        prev = cur
+
+
+def test_deviates_from_linear_approximation():
+    # C2：全程针孔模型。θ=90°、偏移比例 0.25 处线性近似误差约 4°，
+    # 针孔结果必须与之明显偏离（证明未走线性支路）。
+    conv = PixelToAngle(1000.0, math.radians(90.0))
+    pinhole_deg = math.degrees(conv.convert(250.0))  # atan(250·2·tan45°/1000)
+    linear_deg = 250.0 * (90.0 / 1000.0)             # θ·offset/size = 22.5°
+    assert pinhole_deg == pytest.approx(math.degrees(math.atan(0.5)), abs=1e-9)
+    assert abs(pinhole_deg - linear_deg) > 2.0
+
+
+def test_constructor_rejects_invalid_args():
+    with pytest.raises(ValueError):
+        PixelToAngle(0.0, 1.0)
+    with pytest.raises(ValueError):
+        PixelToAngle(-10.0, 1.0)
+    with pytest.raises(ValueError):
+        PixelToAngle(1000.0, 0.0)
+    with pytest.raises(ValueError):
+        PixelToAngle(1000.0, math.pi)
+    with pytest.raises(ValueError):
+        PixelToAngle(1000.0, math.pi + 0.1)

utils/tests/visual_track_control/mavsdk_gateway_test.py

@@ -0,0 +1,176 @@
+"""mavsdk_gateway_test.py —— MavsdkCommandGateway 单元测试（mock offboard）。
+
+移植自 reference-c++/visual_track_control/tests/mavlink_command_gateway_test.cpp。
+C++ 版校验 MAVLink ID 84 线格式字节流；Python 版采用 MAVSDK 语义映射，
+改为以 FakeOffboard 捕获设定点对象，校验三分支映射（物理追踪思路 §2.2、
+A2 掩码三分支的语义等价）：
+
+  角速率非零 → 机体速度设定点 + 非零 yawspeed；
+  航向角非零（角速率为零）→ NED 速度设定点 + 绝对 yaw；
+  两者均零 → 机体速度设定点 + 零 yawspeed（保持航向，A2 回归语义）；
+  刹车 → 全零机体速度设定点。
+  传输失败（异常）→ 返回 False 且不向外抛。
+"""
+
+import asyncio
+
+import pytest
+
+from visual_track_control.mavsdk_gateway import MavsdkCommandGateway
+from visual_track_control.types import ControlOutput
+
+
+class FakeOffboard:
+    """捕获式 offboard mock：记录每次调用的设定点，可注入失败。"""
+
+    def __init__(self):
+        self.body_setpoints = []
+        self.ned_setpoints = []
+        self.attempts = 0
+        self.start_count = 0
+        self.stop_count = 0
+        self.succeed = True
+
+    async def set_velocity_body(self, setpoint):
+        self.attempts += 1  # 与 C++ CaptureTransport 一致：失败也计入发送次数
+        if not self.succeed:
+            raise RuntimeError("mock transport failure")
+        self.body_setpoints.append(setpoint)
+
+    async def set_velocity_ned(self, setpoint):
+        self.attempts += 1
+        if not self.succeed:
+            raise RuntimeError("mock transport failure")
+        self.ned_setpoints.append(setpoint)
+
+    async def start(self):
+        if not self.succeed:
+            raise RuntimeError("mock start failure")
+        self.start_count += 1
+
+    async def stop(self):
+        self.stop_count += 1
+
+    @property
+    def send_count(self):
+        return self.attempts
+
+
+class FakeDrone:
+    def __init__(self):
+        self.offboard = FakeOffboard()
+
+
+def run(coro):
+    return asyncio.run(coro)
+
+
+def make_gateway():
+    drone = FakeDrone()
+    return MavsdkCommandGateway(drone), drone.offboard
+
+
+def test_start_sends_initial_setpoint_then_starts():
+    gateway, offboard = make_gateway()
+    assert run(gateway.start())
+    assert offboard.start_count == 1
+    # PX4 要求先流式发送首条设定点（全零）再请求启动
+    assert len(offboard.body_setpoints) == 1
+    sp = offboard.body_setpoints[0]
+    assert sp.forward_m_s == 0.0
+    assert sp.right_m_s == 0.0
+    assert sp.down_m_s == 0.0
+    assert sp.yawspeed_deg_s == 0.0
+
+
+def test_start_failure_returns_false():
+    gateway, offboard = make_gateway()
+    offboard.succeed = False
+    assert not run(gateway.start())
+
+
+def _make_velocity_heading_output():
+    from visual_track_control.types import ControlMode
+    out = ControlOutput()
+    out.mode = ControlMode.VELOCITY_HEADING
+    return out
+
+
+def test_yaw_rate_branch_uses_body_frame_with_yawspeed():
+    from visual_track_control.types import ControlMode
+    gateway, offboard = make_gateway()
+    out = _make_velocity_heading_output()
+    out.vx_body_mps = 1.5
+    out.vz_mps = 0.25
+    out.yaw_rate_dps = 5.0
+    assert run(gateway.send_velocity_heading(out, 12345))
+    assert offboard.send_count == 1
+    assert len(offboard.body_setpoints) == 1
+    sp = offboard.body_setpoints[0]
+    assert sp.forward_m_s == pytest.approx(1.5)
+    assert sp.right_m_s == 0.0
+    assert sp.down_m_s == pytest.approx(0.25)
+    assert sp.yawspeed_deg_s == pytest.approx(5.0)
+    assert offboard.ned_setpoints == []
+
+
+def test_yaw_only_branch_uses_ned_frame_with_absolute_yaw():
+    # 航向角非零且角速率为零 → 航向掩码语义：NED 速度 + 绝对航向
+    gateway, offboard = make_gateway()
+    out = _make_velocity_heading_output()
+    out.vx_mps = 2.0
+    out.vy_mps = -1.0
+    out.vz_mps = 0.5
+    out.yaw_deg = 10.0
+    out.yaw_rate_dps = 0.0
+    assert run(gateway.send_velocity_heading(out, 1))
+    assert offboard.send_count == 1
+    assert len(offboard.ned_setpoints) == 1
+    sp = offboard.ned_setpoints[0]
+    assert sp.north_m_s == pytest.approx(2.0)
+    assert sp.east_m_s == pytest.approx(-1.0)
+    assert sp.down_m_s == pytest.approx(0.5)
+    assert sp.yaw_deg == pytest.approx(10.0)
+    assert offboard.body_setpoints == []
+
+
+def test_both_zero_keeps_heading_via_body_zero_yawspeed():
+    # A2 回归：两者均零时机体速度设定点配零角速率 = 保持当前航向，
+    # 不得误发绝对航向 0（正北）。
+    gateway, offboard = make_gateway()
+    out = _make_velocity_heading_output()
+    out.vx_body_mps = 1.0
+    out.yaw_rate_dps = 0.0
+    out.yaw_deg = 0.0
+    assert run(gateway.send_velocity_heading(out, 1))
+    assert len(offboard.body_setpoints) == 1
+    sp = offboard.body_setpoints[0]
+    assert sp.forward_m_s == pytest.approx(1.0)
+    assert sp.yawspeed_deg_s == 0.0
+    assert offboard.ned_setpoints == []  # 不得走绝对航向支路
+
+
+def test_brake_hover_sends_zero_velocity_body():
+    gateway, offboard = make_gateway()
+    assert run(gateway.send_brake_hover(777))
+    assert offboard.send_count == 1
+    sp = offboard.body_setpoints[0]
+    assert sp.forward_m_s == 0.0
+    assert sp.right_m_s == 0.0
+    assert sp.down_m_s == 0.0
+    assert sp.yawspeed_deg_s == 0.0
+
+
+def test_transport_failure_propagates_as_false():
+    gateway, offboard = make_gateway()
+    offboard.succeed = False
+    out = _make_velocity_heading_output()
+    assert not run(gateway.send_velocity_heading(out, 1))
+    assert not run(gateway.send_brake_hover(1))
+    assert offboard.send_count == 2
+
+
+def test_stop_calls_offboard_stop():
+    gateway, offboard = make_gateway()
+    run(gateway.stop())
+    assert offboard.stop_count == 1

utils/tests/visual_track_control/pid_controller_test.py

@@ -0,0 +1,103 @@
+"""pid_controller_test.py —— PidController 单元测试。
+
+移植自 reference-c++/visual_track_control/tests/pid_controller_test.cpp。
+覆盖（物理追踪思路 §6.2/§6.4）：位置式 P/I/D 三项、积分钳位、输出限幅、
+异常节拍保护、anti-windup 条件积分（C1 修复回归）、ResetIntegral / Reset。
+"""
+
+import pytest
+
+from visual_track_control.pid_controller import PidController, PidParams
+
+
+def make_params(kp, ki, kd, integral_limit, output_limit, filter_coef=0.0):
+    return PidParams(kp=kp, ki=ki, kd=kd, integral_limit=integral_limit,
+                     derivative_filter_coef=filter_coef, output_limit=output_limit)
+
+
+def test_proportional_only():
+    pid = PidController(make_params(2.0, 0.0, 0.0, 0.0, 0.0))
+    assert pid.update(3.0, 0.1) == 6.0
+    assert pid.update(-1.5, 0.1) == -3.0
+
+
+def test_integral_accumulates():
+    pid = PidController(make_params(0.0, 1.0, 0.0, 0.0, 0.0))
+    # 两拍 error=2、dt=0.5：积分累计 1.0 → 2.0
+    assert pid.update(2.0, 0.5) == 1.0
+    assert pid.update(2.0, 0.5) == 2.0
+
+
+def test_integral_clamped_to_limit():
+    pid = PidController(make_params(0.0, 1.0, 0.0, integral_limit=1.0, output_limit=0.0))
+    for _ in range(5):
+        pid.update(10.0, 1.0)
+    # 积分候选值被钳位在 1.0 → 输出恒为 1.0
+    assert pid.last_output == 1.0
+
+
+def test_output_clamped_to_limit():
+    pid = PidController(make_params(10.0, 0.0, 0.0, 0.0, output_limit=3.0))
+    assert pid.update(1.0, 0.1) == 3.0
+    assert pid.update(-1.0, 0.1) == -3.0
+
+
+def test_anti_windup_freeze_while_saturated_same_direction():
+    # kp=0、ki=2、output_limit=5：error=+1 持续饱和时积分冻结，
+    # 输出恒钉在 +5（10 拍累计也不会让积分继续增长）。
+    pid = PidController(make_params(0.0, 2.0, 0.0, 0.0, 5.0))
+    for _ in range(10):
+        assert pid.update(10.0, 1.0) == 5.0
+
+
+def test_anti_windup_unwinds_after_error_flips():
+    # C1 回归：误差反号后积分必须立即回卷（旧判据"积分与输出同向"会
+    # 冻结回卷、输出一直钉在 +5）。
+    # kp=0、ki=2、limit=5：+10 饱和 3 拍、积分冻结在 0；
+    # error=-1 首拍 → 输出 2×(-1) = -2（立即回卷）；
+    # 持续负误差使输出到达负饱和 -5（积分冻结在 -2，输出 = clamp(-6)）。
+    pid = PidController(make_params(0.0, 2.0, 0.0, 0.0, 5.0))
+    for _ in range(3):
+        assert pid.update(10.0, 1.0) == 5.0
+    assert pid.update(-1.0, 1.0) == -2.0  # 首拍立即回卷
+    out = -2.0
+    for _ in range(11):
+        out = pid.update(-1.0, 1.0)
+    assert out == pytest.approx(-5.0, abs=1e-9)  # 旧判据此处仍为 +5.0
+
+
+def test_derivative_first_step_is_zero():
+    pid = PidController(make_params(0.0, 0.0, 1.0, 0.0, 0.0))
+    # 首拍无微分历史：微分项为 0
+    assert pid.update(5.0, 1.0) == 0.0
+
+
+def test_derivative_with_low_pass_filter():
+    pid = PidController(make_params(0.0, 0.0, 1.0, 0.0, 0.0, filter_coef=0.5))
+    pid.update(0.0, 1.0)  # 建立微分历史（导数 0）
+    # 第二拍原始导数 1.0，低通后 0.5×0 + 0.5×1.0 = 0.5
+    assert pid.update(1.0, 1.0) == 0.5
+
+
+def test_zero_or_negative_dt_holds_last_output():
+    pid = PidController(make_params(1.0, 0.0, 0.0, 0.0, 0.0))
+    assert pid.update(2.0, 0.1) == 2.0
+    assert pid.update(99.0, 0.0) == 2.0
+    assert pid.update(99.0, -1.0) == 2.0
+
+
+def test_reset_integral_clears_only_integral():
+    pid = PidController(make_params(0.0, 1.0, 1.0, 0.0, 0.0))
+    pid.update(2.0, 1.0)  # 积分 = 2
+    pid.reset_integral()
+    # 积分清零、微分历史保留：输出应为 kd×(0−2)/1 = −2
+    assert pid.update(0.0, 1.0) == -2.0
+
+
+def test_reset_clears_everything():
+    pid = PidController(make_params(1.0, 1.0, 1.0, 0.0, 0.0))
+    pid.update(5.0, 1.0)
+    pid.reset()
+    assert pid.last_output == 0.0
+    # 全清后等同全新控制器：首拍无微分历史
+    assert pid.update(3.0, 1.0) == 3.0 + 3.0  # kp·e + ki·∫e

utils/tests/visual_track_control/rate_limiter_test.py

@@ -0,0 +1,68 @@
+"""rate_limiter_test.py —— Clamp / SlewRateLimiter 单元测试。
+
+移植自 reference-c++/visual_track_control/tests/rate_limiter_test.cpp。
+覆盖（物理追踪思路 §8、状态机设计 S6）：静态限幅、变化率限幅步进、
+首条指令从零渐变、异常节拍保护、Reset 归零。
+"""
+
+import pytest
+
+from visual_track_control.rate_limiter import SlewRateLimiter, clamp
+
+
+def test_clamp_clamps_value():
+    assert clamp(5.0, -2.0, 2.0) == 2.0
+    assert clamp(-5.0, -2.0, 2.0) == -2.0
+    assert clamp(1.5, -2.0, 2.0) == 1.5
+    # 边界值：落在区间内
+    assert clamp(2.0, -2.0, 2.0) == 2.0
+    assert clamp(-2.0, -2.0, 2.0) == -2.0
+
+
+def test_first_command_ramps_from_zero():
+    # S6：初始输出为 0，首条大指令只能按变化率渐变
+    lim = SlewRateLimiter(2.0)
+    assert lim.limit(10.0, 0.5) == 1.0    # 0 + 2×0.5
+    assert lim.limit(10.0, 0.5) == 2.0    # 继续渐变
+    assert lim.limit(10.0, 10.0) == 10.0  # 步长足够 → 到达目标
+
+
+def test_tracks_target_within_step():
+    lim = SlewRateLimiter(10.0)
+    # 步长 10×1=10，目标 3 在步长内 → 直接到达
+    assert lim.limit(3.0, 1.0) == 3.0
+    assert lim.limit(3.0, 1.0) == 3.0  # 已在目标：保持
+
+
+def test_negative_direction():
+    lim = SlewRateLimiter(2.0)
+    assert lim.limit(-10.0, 0.5) == -1.0
+    assert lim.limit(-10.0, 0.5) == -2.0
+
+
+def test_zero_or_negative_dt_holds():
+    lim = SlewRateLimiter(2.0)
+    lim.limit(10.0, 1.0)  # 输出 2.0
+    assert lim.limit(10.0, 0.0) == 2.0
+    assert lim.limit(10.0, -1.0) == 2.0
+
+
+def test_zero_rate_limit_freezes_output():
+    lim = SlewRateLimiter(0.0)
+    assert lim.limit(5.0, 1.0) == 0.0
+
+
+def test_reset_to_value():
+    lim = SlewRateLimiter(2.0)
+    lim.limit(10.0, 1.0)
+    lim.reset()
+    assert lim.last_output == 0.0
+    lim.reset(3.5)
+    assert lim.last_output == 3.5
+    # 复位后从新值继续渐变
+    assert lim.limit(10.0, 1.0) == 5.5  # 3.5 + 2×1
+
+
+def test_custom_initial_value():
+    lim = SlewRateLimiter(1.0, initial=5.0)
+    assert lim.limit(10.0, 1.0) == 6.0

utils/tests/visual_track_control/staleness_checker_test.py

@@ -0,0 +1,32 @@
+"""staleness_checker_test.py —— is_stale 单元测试。
+
+移植自 reference-c++/visual_track_control/tests/staleness_checker_test.cpp。
+覆盖（物理追踪思路 §7.1）：从未取样判超龄、有效期内新鲜、超出有效期超龄、
+边界恰好在有效期上（不算超龄）、时间戳不一致判超龄（C4）。
+"""
+
+from visual_track_control.staleness_checker import is_stale
+
+
+def test_never_sampled_is_stale():
+    assert is_stale(0, 1000, 200)
+    assert is_stale(-5, 1000, 200)
+
+
+def test_within_validity_is_fresh():
+    assert not is_stale(900, 1000, 200)
+    assert not is_stale(1000, 1000, 200)  # 刚取样
+
+
+def test_beyond_validity_is_stale():
+    assert is_stale(799, 1000, 200)  # 201 > 200
+
+
+def test_exactly_at_boundary_is_fresh():
+    # 超龄判定为严格大于：恰好等于有效期仍算新鲜
+    assert not is_stale(800, 1000, 200)
+
+
+def test_inconsistent_timestamp_is_stale():
+    # C4：now < sample 时间戳不一致 → 年龄不可信 → 判超龄（宁降级不轻信）
+    assert is_stale(2000, 1000, 200)

utils/tests/visual_track_control/tracking_control_law_test.py

@@ -0,0 +1,338 @@
+"""tracking_control_law_test.py —— TrackingControlLaw 主控律单元测试。
+
+移植自 reference-c++/visual_track_control/tests/tracking_control_law_test.cpp。
+覆盖（物理追踪思路 §2.3/§4/§5/§6/§7/§9）：超龄降级（姿态缺失/过期、视觉
+超龄、降级优先级）、判丢零速刹车、Coast 清积分与标志、速率式/位置式航向
+通道（含绝对航向合成与回绕，A2）、垂直统一速度支路（A1）、距离通道三种
+无距离降级动作、首拍变化率限幅（S6）、机体系→NED 输出旋转（A4）。
+"""
+
+import math
+
+import pytest
+
+from visual_track_control.config import (HeadingMode, NoDistanceAction,
+                                         TrackingConfig)
+from visual_track_control.tracking_control_law import TrackingControlLaw
+from visual_track_control.types import (ControlMode, ControlSnapshot,
+                                        DegradedReason, TrackState)
+
+
+def make_config() -> TrackingConfig:
+    """测试基准配置：大限幅使限幅器近似透明，便于独立验证各通道数学。
+    画面 1000×1000、视场角 90° → 针孔系数 2·tan(45°)/1000 = 0.002，
+    偏移 100 像素 → 视线角 atan(0.2) ≈ 11.3099°。
+    """
+    cfg = TrackingConfig()
+    cfg.control.frequency_hz = 20.0
+    cfg.control.visual_stale_ms = 200
+    cfg.control.radar_stale_ms = 500
+    cfg.control.attitude_stale_ms = 300
+    cfg.camera.image_width = 1000
+    cfg.camera.image_height = 1000
+    cfg.camera.fov_h_deg = 90.0
+    cfg.camera.fov_v_deg = 90.0
+    cfg.heading.mode = HeadingMode.RATE
+    cfg.heading.gain = 1.0
+    cfg.heading.yaw_rate_limit_dps = 30.0
+    cfg.heading.yaw_slew_limit_dps2 = 100000.0
+    cfg.vertical.vz_gain_mps_per_deg = 0.1
+    cfg.vertical.vz_limit_mps = 2.0
+    cfg.distance.d_exp_m = 10.0
+    cfg.distance.kp = 0.5
+    cfg.distance.ki = 0.0
+    cfg.distance.kd = 0.0
+    cfg.distance.integral_limit = 0.0
+    cfg.distance.derivative_filter_coef = 0.0
+    cfg.distance.approach_velocity_limit_mps = 3.0
+    cfg.distance.retreat_velocity_limit_mps = 1.0
+    cfg.distance.no_distance_action = NoDistanceAction.HOLD
+    cfg.distance.no_distance_approach_limit_mps = 0.5
+    cfg.accel_limit.ax_mps2 = 1000.0
+    cfg.accel_limit.ay_mps2 = 1000.0
+    cfg.accel_limit.az_mps2 = 1000.0
+    return cfg
+
+
+def make_snapshot(now_ms: int) -> ControlSnapshot:
+    """新鲜快照：目标在画面中心，姿态/追踪/雷达均新鲜。"""
+    return ControlSnapshot(
+        track=TrackState(tracked=True, is_predicted=False,
+                         center=(500.0, 500.0)),
+        track_time_ms=now_ms,
+        attitude_present=True,
+        attitude_time_ms=now_ms,
+    )
+
+
+# ---- 超龄降级（§7.1 / §9.2） ----
+
+def test_attitude_missing_degrades():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.attitude_present = False
+    out = law.update(snap, 1000)
+    assert out.mode is ControlMode.DEGRADED_HOLD
+    assert out.degraded_reason is DegradedReason.ATTITUDE_STALE
+    assert not out.valid
+
+
+def test_attitude_stale_degrades():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.attitude_time_ms = 600  # 400 ms 前 > 300 ms 阈值
+    out = law.update(snap, 1000)
+    assert out.mode is ControlMode.DEGRADED_HOLD
+    assert out.degraded_reason is DegradedReason.ATTITUDE_STALE
+
+
+def test_vision_stale_degrades():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.track_time_ms = 700  # 300 ms 前 > 200 ms 阈值
+    out = law.update(snap, 1000)
+    assert out.mode is ControlMode.DEGRADED_HOLD
+    assert out.degraded_reason is DegradedReason.VISION_STALE
+
+
+def test_never_sampled_track_degrades():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.track_time_ms = 0  # 从未取样
+    out = law.update(snap, 1000)
+    assert out.mode is ControlMode.DEGRADED_HOLD
+    assert out.degraded_reason is DegradedReason.VISION_STALE
+
+
+def test_attitude_stale_beats_vision_stale():
+    # 降级优先级：姿态校验在前（§9.2 姿态过期禁止一切空间转换）
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.attitude_present = False
+    snap.track_time_ms = 0
+    out = law.update(snap, 1000)
+    assert out.degraded_reason is DegradedReason.ATTITUDE_STALE
+
+
+# ---- 判丢刹车（§6.4） ----
+
+def test_lost_target_brakes():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.track.tracked = False
+    out = law.update(snap, 1000)
+    assert out.mode is ControlMode.BRAKE_HOVER
+    assert out.valid
+    assert out.vx_mps == 0.0
+    assert out.vy_mps == 0.0
+    assert out.vz_mps == 0.0
+
+
+# ---- Coast（§6.4） ----
+
+def test_coast_sets_flag_and_keeps_tracking():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.track.is_predicted = True
+    out = law.update(snap, 1000)
+    assert out.mode is ControlMode.VELOCITY_HEADING
+    assert out.coast_active
+    assert out.valid
+
+
+# ---- 水平航向通道（§4.2，A2） ----
+
+def test_rate_mode_maps_beta_to_yaw_rate():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.track.center = (600.0, 500.0)  # 目标偏右 100 像素
+    out = law.update(snap, 1000)
+    beta_deg = math.degrees(math.atan(0.2))  # ≈ 11.31°
+    assert out.yaw_rate_dps == pytest.approx(beta_deg, abs=1e-6)  # gain=1，右转正
+    assert out.yaw_deg == 0.0
+
+
+def test_rate_mode_clamps_to_yaw_rate_limit():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.track.center = (950.0, 500.0)  # 大偏移：β≈43.5° > 30°/s 上限
+    out = law.update(snap, 1000)
+    assert out.yaw_rate_dps == pytest.approx(30.0, abs=1e-9)
+
+
+def test_position_mode_composes_absolute_heading():
+    # A2：位置式输出为绝对 NED 航向 = 当前航向 + 限幅修正量
+    cfg = make_config()
+    cfg.heading.mode = HeadingMode.POSITION
+    law = TrackingControlLaw(cfg)
+    snap = make_snapshot(1000)
+    snap.track.center = (600.0, 500.0)
+    snap.yaw_rad = math.radians(45.0)
+    out = law.update(snap, 1000)
+    beta_deg = math.degrees(math.atan(0.2))
+    assert out.yaw_deg == pytest.approx(45.0 + beta_deg, abs=1e-6)
+    assert out.yaw_rate_dps == 0.0
+
+
+def test_position_mode_wraps_around_180():
+    cfg = make_config()
+    cfg.heading.mode = HeadingMode.POSITION
+    law = TrackingControlLaw(cfg)
+    snap = make_snapshot(1000)
+    snap.track.center = (600.0, 500.0)
+    snap.yaw_rad = math.radians(175.0)
+    out = law.update(snap, 1000)
+    beta_deg = math.degrees(math.atan(0.2))
+    # 175 + 11.31 = 186.31 → 回绕到 ≈ -173.69
+    assert out.yaw_deg == pytest.approx(175.0 + beta_deg - 360.0, abs=1e-6)
+    assert -180.0 < out.yaw_deg < 180.0
+
+
+# ---- 垂直统一速度支路（§5，A1） ----
+
+def test_vertical_offset_drives_vz():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.track.center = (500.0, 600.0)  # 目标偏低 100 像素 → α>0 → 下降
+    out = law.update(snap, 1000)
+    alpha_deg = math.degrees(math.atan(0.2))
+    assert out.vz_mps == pytest.approx(0.1 * alpha_deg, abs=1e-6)
+    assert out.mode is ControlMode.VELOCITY_HEADING  # A1：不切姿态微调模式
+
+
+def test_vertical_velocity_clamped():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.track.center = (500.0, 990.0)  # α≈44.5° × 0.1 = 4.45 > 2 上限
+    out = law.update(snap, 1000)
+    assert out.vz_mps == pytest.approx(2.0, abs=1e-9)
+
+
+# ---- 距离通道（§6.2 / §6.3） ----
+
+def test_distance_approach_when_far():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.radar_distance_m = 30.0  # Δd = +20 → kp 输出 10 → 限幅 3
+    snap.associated_to_target = True
+    snap.radar_time_ms = 1000
+    out = law.update(snap, 1000)
+    assert out.vx_mps == pytest.approx(3.0, abs=1e-9)
+
+
+def test_distance_retreat_clamped_when_too_close():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.radar_distance_m = 5.0  # Δd = -5 → kp 输出 -2.5 → 后退限幅 -1
+    snap.associated_to_target = True
+    snap.radar_time_ms = 1000
+    out = law.update(snap, 1000)
+    assert out.vx_mps == pytest.approx(-1.0, abs=1e-9)
+
+
+def test_no_distance_hold_zeros_vx():
+    law = TrackingControlLaw(make_config())  # 缺省 kHold
+    snap = make_snapshot(1000)
+    snap.associated_to_target = False
+    out = law.update(snap, 1000)
+    assert out.mode is ControlMode.VELOCITY_HEADING
+    assert out.vx_mps == 0.0
+
+
+def test_no_distance_slow_approach():
+    cfg = make_config()
+    cfg.distance.no_distance_action = NoDistanceAction.SLOW_APPROACH
+    law = TrackingControlLaw(cfg)
+    snap = make_snapshot(1000)
+    snap.associated_to_target = False
+    out = law.update(snap, 1000)
+    assert out.vx_mps == pytest.approx(0.5, abs=1e-9)
+
+
+def test_no_distance_exit_degrades():
+    cfg = make_config()
+    cfg.distance.no_distance_action = NoDistanceAction.EXIT
+    law = TrackingControlLaw(cfg)
+    snap = make_snapshot(1000)
+    snap.associated_to_target = False
+    out = law.update(snap, 1000)
+    assert out.mode is ControlMode.DEGRADED_HOLD
+    assert out.degraded_reason is DegradedReason.NO_DISTANCE_EXIT
+    assert not out.valid
+
+
+def test_stale_radar_falls_back_to_no_distance():
+    law = TrackingControlLaw(make_config())  # kHold
+    snap = make_snapshot(1000)
+    snap.radar_distance_m = 30.0
+    snap.associated_to_target = True
+    snap.radar_time_ms = 400  # 600 ms 前 > 500 ms 阈值 → 距离不可用
+    out = law.update(snap, 1000)
+    assert out.mode is ControlMode.VELOCITY_HEADING
+    assert out.vx_mps == 0.0
+
+
+# ---- 首拍/限幅（§8、S6） ----
+
+def test_first_tick_vx_slews_from_zero():
+    # S6：首条速度指令必须经变化率限幅，从 0 渐变而非阶跃
+    cfg = make_config()
+    cfg.accel_limit.ax_mps2 = 2.0
+    law = TrackingControlLaw(cfg)
+    snap = make_snapshot(1000)
+    snap.radar_distance_m = 30.0  # 通道输出本应为 3.0
+    snap.associated_to_target = True
+    snap.radar_time_ms = 1000
+    out = law.update(snap, 1000)  # 首拍：dt 按 1/20 Hz 兜底
+    assert out.vx_mps == pytest.approx(2.0 * (1.0 / 20.0), abs=1e-9)  # 0.1
+
+
+# ---- 机体系 → NED 旋转（§2.3，A4） ----
+
+def test_output_rotated_to_ned_by_yaw():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.yaw_rad = math.pi / 2.0  # 机头朝东
+    snap.radar_distance_m = 30.0
+    snap.associated_to_target = True
+    snap.radar_time_ms = 1000
+    out = law.update(snap, 1000)
+    # 机体前向 3.0 → 东向 3.0，北向 ≈ 0
+    assert out.vx_mps == pytest.approx(0.0, abs=1e-9)
+    assert out.vy_mps == pytest.approx(3.0, abs=1e-9)
+
+
+def test_zero_yaw_keeps_forward_as_north():
+    law = TrackingControlLaw(make_config())
+    snap = make_snapshot(1000)
+    snap.yaw_rad = 0.0
+    snap.radar_distance_m = 30.0
+    snap.associated_to_target = True
+    snap.radar_time_ms = 1000
+    out = law.update(snap, 1000)
+    assert out.vx_mps == pytest.approx(3.0, abs=1e-9)
+    assert out.vy_mps == pytest.approx(0.0, abs=1e-9)
+
+
+# ---- Reset（§6.4） ----
+
+def test_reset_restores_slew_from_zero():
+    cfg = make_config()
+    cfg.accel_limit.ax_mps2 = 2.0
+    law = TrackingControlLaw(cfg)
+    snap = make_snapshot(1000)
+    snap.radar_distance_m = 30.0
+    snap.associated_to_target = True
+    snap.radar_time_ms = 1000
+    for t in range(1000, 3000, 50):
+        snap.track_time_ms = t
+        snap.radar_time_ms = t
+        snap.attitude_time_ms = t
+        law.update(snap, t)  # 若干拍后 vx 已爬升到限幅 3.0
+    law.reset()
+    snap.track_time_ms = 3000
+    snap.radar_time_ms = 3000
+    snap.attitude_time_ms = 3000
+    out = law.update(snap, 3000)
+    # Reset 后限幅器归零：又从 0.1 开始渐变
+    assert out.vx_mps == pytest.approx(0.1, abs=1e-9)

utils/tests/visual_track_control/vtc_config_test.py

@@ -0,0 +1,171 @@
+"""vtc_config_test.py —— TrackingConfig::load_from_json 单元测试。
+
+移植自 reference-c++/visual_track_control/tests/vtc_config_test.cpp。
+覆盖（物理追踪思路 §8、需求分析 NFR-007）：真实配置文件加载成功与取值
+正确（含 A1/A2 修改后的字段结构）、缺字段 / 缺分组 / 非法枚举 / 非法数值 /
+文件缺失均抛带字段名的 ValueError。
+
+C++ 版用字符串替换构造非法变体；Python 版改为 dict 变异（更稳健），
+临时文件经 pytest 的 tmp_path 提供。
+"""
+
+import json
+from pathlib import Path
+
+import pytest
+
+from visual_track_control.config import (HeadingMode, NoDistanceAction,
+                                         load_from_json)
+
+_REPO_ROOT = Path(__file__).resolve().parents[3]
+REAL_CONFIG = _REPO_ROOT / "reference-c++" / "visual_track_control" / "config" / "track_control.json"
+
+
+def full_config() -> dict:
+    """与真实配置同构的完整合法配置（用于派生各非法变体）。"""
+    return {
+        "control": {
+            "frequency_hz": 20.0,
+            "visual_stale_ms": 200,
+            "radar_stale_ms": 500,
+            "attitude_stale_ms": 300,
+        },
+        "camera": {
+            "image_width": 1920,
+            "image_height": 1080,
+            "fov_h_deg": 60.0,
+            "fov_v_deg": 35.0,
+        },
+        "heading": {
+            "mode": "rate",
+            "gain": 1.0,
+            "yaw_rate_limit_dps": 30.0,
+            "yaw_slew_limit_dps2": 60.0,
+        },
+        "vertical": {
+            "vz_gain_mps_per_deg": 0.1,
+            "vz_limit_mps": 2.0,
+        },
+        "distance": {
+            "d_exp_m": 10.0,
+            "kp": 0.5,
+            "ki": 0.05,
+            "kd": 0.1,
+            "integral_limit": 2.0,
+            "derivative_filter_coef": 0.2,
+            "approach_velocity_limit_mps": 3.0,
+            "retreat_velocity_limit_mps": 1.0,
+            "no_distance_action": "hold",
+            "no_distance_approach_limit_mps": 0.5,
+        },
+        "accel_limit": {
+            "ax_mps2": 2.0,
+            "ay_mps2": 2.0,
+            "az_mps2": 1.5,
+        },
+    }
+
+
+def write_temp_json(tmp_path: Path, config: dict, name: str) -> str:
+    path = tmp_path / name
+    path.write_text(json.dumps(config), encoding="utf-8")
+    return str(path)
+
+
+def test_load_real_config_succeeds():
+    cfg = load_from_json(str(REAL_CONFIG))
+    assert cfg.control.frequency_hz == 20.0
+    assert cfg.control.visual_stale_ms == 200
+    assert cfg.camera.image_width == 1920
+    assert cfg.camera.image_height == 1080
+    assert cfg.camera.fov_h_deg == 60.0
+    assert cfg.heading.mode is HeadingMode.RATE  # A2：默认速率式
+    assert cfg.heading.yaw_rate_limit_dps == 30.0
+    assert cfg.vertical.vz_gain_mps_per_deg == 0.1
+    assert cfg.vertical.vz_limit_mps == 2.0
+    assert cfg.distance.d_exp_m == 10.0
+    assert cfg.distance.no_distance_action is NoDistanceAction.HOLD
+    assert cfg.accel_limit.ax_mps2 == 2.0
+
+
+def test_full_json_round_trip_matches_expected(tmp_path):
+    # 内嵌合法配置与真实配置文件行为一致
+    path = write_temp_json(tmp_path, full_config(), "vtc_cfg_full.json")
+    cfg = load_from_json(path)
+    assert cfg.heading.mode is HeadingMode.RATE
+    assert cfg.distance.kp == 0.5
+
+
+def test_position_mode_accepted(tmp_path):
+    config = full_config()
+    config["heading"]["mode"] = "position"
+    path = write_temp_json(tmp_path, config, "vtc_cfg_position.json")
+    cfg = load_from_json(path)
+    assert cfg.heading.mode is HeadingMode.POSITION
+
+
+def test_missing_file_throws_with_path():
+    with pytest.raises(ValueError) as exc_info:
+        load_from_json("/nonexistent/vtc_no_such_file.json")
+    assert "vtc_no_such_file.json" in str(exc_info.value)
+
+
+def test_malformed_json_throws(tmp_path):
+    path = tmp_path / "vtc_cfg_bad.json"
+    path.write_text("{ this is not json", encoding="utf-8")
+    with pytest.raises(ValueError):
+        load_from_json(str(path))
+
+
+def test_missing_field_throws_with_field_name(tmp_path):
+    config = full_config()
+    del config["heading"]["gain"]
+    path = write_temp_json(tmp_path, config, "vtc_cfg_missing.json")
+    with pytest.raises(ValueError) as exc_info:
+        load_from_json(path)
+    assert "heading.gain" in str(exc_info.value)
+
+
+def test_missing_section_throws(tmp_path):
+    config = full_config()
+    del config["accel_limit"]
+    path = write_temp_json(tmp_path, config, "vtc_cfg_nosection.json")
+    with pytest.raises(ValueError) as exc_info:
+        load_from_json(path)
+    assert "accel_limit" in str(exc_info.value)
+
+
+def test_invalid_heading_mode_throws(tmp_path):
+    config = full_config()
+    config["heading"]["mode"] = "magic"
+    path = write_temp_json(tmp_path, config, "vtc_cfg_badmode.json")
+    with pytest.raises(ValueError) as exc_info:
+        load_from_json(path)
+    assert "heading.mode" in str(exc_info.value)
+
+
+def test_invalid_no_distance_action_throws(tmp_path):
+    config = full_config()
+    config["distance"]["no_distance_action"] = "zoom"
+    path = write_temp_json(tmp_path, config, "vtc_cfg_badaction.json")
+    with pytest.raises(ValueError) as exc_info:
+        load_from_json(path)
+    assert "no_distance_action" in str(exc_info.value)
+
+
+def test_negative_frequency_throws(tmp_path):
+    config = full_config()
+    config["control"]["frequency_hz"] = -1.0
+    path = write_temp_json(tmp_path, config, "vtc_cfg_negfreq.json")
+    with pytest.raises(ValueError) as exc_info:
+        load_from_json(path)
+    assert "control.frequency_hz" in str(exc_info.value)
+
+
+def test_fov_out_of_range_throws(tmp_path):
+    config = full_config()
+    config["camera"]["fov_h_deg"] = 200.0
+    path = write_temp_json(tmp_path, config, "vtc_cfg_badfov.json")
+    with pytest.raises(ValueError) as exc_info:
+        load_from_json(path)
+    assert "camera.fov_h_deg" in str(exc_info.value)