From cb00322a501c718fb50dc9db516ed9e711995fad Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E4=B8=80=E5=96=B5?= Date: Wed, 2 Sep 2026 15:13:13 +0800 Subject: [PATCH] first commit --- .idea/.gitignore | 5 + .idea/code_drone-test.iml | 8 + .idea/inspectionProfiles/Project_Default.xml | 27 + .../inspectionProfiles/profiles_settings.xml | 6 + .idea/modules.xml | 8 + ...ss_b7314a3c-d6f0-45f0-9768-58161c9cf637.md | 49 + docs/物理追踪思路.md | 371 ++++++ docs/视觉跟踪控制律技术路线.md | 1029 +++++++++++++++++ docs/视觉追踪技术路线.md | 685 +++++++++++ reference-c++/target_tracker/CMakeLists.txt | 85 ++ .../target_tracker/kalman_box_filter.h | 87 ++ .../include/target_tracker/target_tracker.h | 123 ++ .../include/target_tracker/tracklet.h | 93 ++ .../target_tracker/src/kalman_box_filter.cpp | 235 ++++ .../target_tracker/src/target_tracker.cpp | 312 +++++ reference-c++/target_tracker/src/tracklet.cpp | 101 ++ .../target_tracker/target_tracker.md | 217 ++++ .../tests/kalman_box_filter_test.cpp | 124 ++ .../tests/target_tracker_test.cpp | 402 +++++++ .../target_tracker/tests/tracklet_test.cpp | 177 +++ .../visual_track_control/CMakeLists.txt | 122 ++ .../config/track_control.json | 48 + .../examples/track_control_demo.cpp | 225 ++++ .../visual_track_control/frame_transform.h | 44 + .../i_command_transport.h | 31 + .../visual_track_control/line_of_sight.h | 38 + .../mavlink_command_gateway.h | 65 ++ .../visual_track_control/pid_controller.h | 59 + .../visual_track_control/rate_limiter.h | 56 + .../visual_track_control/staleness_checker.h | 38 + .../tracking_control_law.h | 63 + .../include/visual_track_control/vtc_config.h | 113 ++ .../include/visual_track_control/vtc_types.h | 102 ++ .../src/line_of_sight.cpp | 42 + .../src/mavlink_command_gateway.cpp | 119 ++ .../src/pid_controller.cpp | 81 ++ .../src/tracking_control_law.cpp | 239 ++++ .../visual_track_control/src/vtc_config.cpp | 225 ++++ .../tests/frame_transform_test.cpp | 56 + .../tests/line_of_sight_test.cpp | 69 ++ .../tests/mavlink_command_gateway_test.cpp | 181 +++ .../tests/pid_controller_test.cpp | 123 ++ .../tests/rate_limiter_test.cpp | 76 ++ .../tests/staleness_checker_test.cpp | 41 + .../tests/tracking_control_law_test.cpp | 349 ++++++ .../tests/vtc_config_test.cpp | 224 ++++ .../视觉跟踪控制律实现文档.md | 443 +++++++ utils/README.md | 58 + .../conftest.cpython-312-pytest-9.1.1.pyc | Bin 0 -> 739 bytes utils/config/track_control.json | 48 + utils/conftest.py | 8 + utils/demo_track_control.py | 207 ++++ utils/pytest.ini | 3 + utils/target_tracker/README.md | 29 + utils/target_tracker/__init__.py | 15 + .../__pycache__/__init__.cpython-312.pyc | Bin 0 -> 973 bytes .../kalman_box_filter.cpython-312.pyc | Bin 0 -> 6297 bytes .../target_tracker.cpython-312.pyc | Bin 0 -> 13603 bytes .../__pycache__/tracklet.cpython-312.pyc | Bin 0 -> 6268 bytes utils/target_tracker/kalman_box_filter.py | 111 ++ utils/target_tracker/target_tracker.py | 269 +++++ utils/target_tracker/tracklet.py | 110 ++ ...x_filter_test.cpython-312-pytest-9.1.1.pyc | Bin 0 -> 29617 bytes ..._tracker_test.cpython-312-pytest-9.1.1.pyc | Bin 0 -> 73797 bytes ...tracklet_test.cpython-312-pytest-9.1.1.pyc | Bin 0 -> 47576 bytes .../target_tracker/kalman_box_filter_test.py | 107 ++ .../target_tracker/target_tracker_test.py | 364 ++++++ utils/tests/target_tracker/tracklet_test.py | 159 +++ ...ransform_test.cpython-312-pytest-9.1.1.pyc | Bin 0 -> 11989 bytes ...of_sight_test.cpython-312-pytest-9.1.1.pyc | Bin 0 -> 14627 bytes ..._gateway_test.cpython-312-pytest-9.1.1.pyc | Bin 0 -> 37585 bytes ...ntroller_test.cpython-312-pytest-9.1.1.pyc | Bin 0 -> 20347 bytes ..._limiter_test.cpython-312-pytest-9.1.1.pyc | Bin 0 -> 17673 bytes ..._checker_test.cpython-312-pytest-9.1.1.pyc | Bin 0 -> 6043 bytes ...trol_law_test.cpython-312-pytest-9.1.1.pyc | Bin 0 -> 53159 bytes ...c_config_test.cpython-312-pytest-9.1.1.pyc | Bin 0 -> 25021 bytes .../frame_transform_test.py | 43 + .../line_of_sight_test.py | 63 + .../mavsdk_gateway_test.py | 176 +++ .../pid_controller_test.py | 103 ++ .../visual_track_control/rate_limiter_test.py | 68 ++ .../staleness_checker_test.py | 32 + .../tracking_control_law_test.py | 338 ++++++ .../visual_track_control/vtc_config_test.py | 171 +++ utils/visual_track_control/README.md | 41 + utils/visual_track_control/__init__.py | 55 + .../__pycache__/__init__.cpython-312.pyc | Bin 0 -> 1865 bytes .../command_sender.cpython-312.pyc | Bin 0 -> 2049 bytes .../__pycache__/config.cpython-312.pyc | Bin 0 -> 14488 bytes .../frame_transform.cpython-312.pyc | Bin 0 -> 1562 bytes .../__pycache__/line_of_sight.cpython-312.pyc | Bin 0 -> 1901 bytes .../mavsdk_gateway.cpython-312.pyc | Bin 0 -> 6610 bytes .../pid_controller.cpython-312.pyc | Bin 0 -> 4658 bytes .../__pycache__/rate_limiter.cpython-312.pyc | Bin 0 -> 2549 bytes .../staleness_checker.cpython-312.pyc | Bin 0 -> 1033 bytes .../tracking_control_law.cpython-312.pyc | Bin 0 -> 12996 bytes .../__pycache__/types.cpython-312.pyc | Bin 0 -> 4643 bytes utils/visual_track_control/command_sender.py | 32 + utils/visual_track_control/config.py | 278 +++++ utils/visual_track_control/frame_transform.py | 25 + utils/visual_track_control/line_of_sight.py | 25 + utils/visual_track_control/mavsdk_gateway.py | 98 ++ utils/visual_track_control/pid_controller.py | 95 ++ utils/visual_track_control/rate_limiter.py | 37 + .../visual_track_control/staleness_checker.py | 19 + .../tracking_control_law.py | 220 ++++ utils/visual_track_control/types.py | 95 ++ 107 files changed, 10918 insertions(+) create mode 100644 .idea/.gitignore create mode 100644 .idea/code_drone-test.iml create mode 100644 .idea/inspectionProfiles/Project_Default.xml create mode 100644 .idea/inspectionProfiles/profiles_settings.xml create mode 100644 .idea/modules.xml create mode 100644 .zcode/plans/plan-sess_b7314a3c-d6f0-45f0-9768-58161c9cf637.md create mode 100644 docs/物理追踪思路.md create mode 100644 docs/视觉跟踪控制律技术路线.md create mode 100644 docs/视觉追踪技术路线.md create mode 100644 reference-c++/target_tracker/CMakeLists.txt create mode 100644 reference-c++/target_tracker/include/target_tracker/kalman_box_filter.h create mode 100644 reference-c++/target_tracker/include/target_tracker/target_tracker.h create mode 100644 reference-c++/target_tracker/include/target_tracker/tracklet.h create mode 100644 reference-c++/target_tracker/src/kalman_box_filter.cpp create mode 100644 reference-c++/target_tracker/src/target_tracker.cpp create mode 100644 reference-c++/target_tracker/src/tracklet.cpp create mode 100644 reference-c++/target_tracker/target_tracker.md create mode 100644 reference-c++/target_tracker/tests/kalman_box_filter_test.cpp create mode 100644 reference-c++/target_tracker/tests/target_tracker_test.cpp create mode 100644 reference-c++/target_tracker/tests/tracklet_test.cpp create mode 100644 reference-c++/visual_track_control/CMakeLists.txt create mode 100644 reference-c++/visual_track_control/config/track_control.json create mode 100644 reference-c++/visual_track_control/examples/track_control_demo.cpp create mode 100644 reference-c++/visual_track_control/include/visual_track_control/frame_transform.h create mode 100644 reference-c++/visual_track_control/include/visual_track_control/i_command_transport.h create mode 100644 reference-c++/visual_track_control/include/visual_track_control/line_of_sight.h create mode 100644 reference-c++/visual_track_control/include/visual_track_control/mavlink_command_gateway.h create mode 100644 reference-c++/visual_track_control/include/visual_track_control/pid_controller.h create mode 100644 reference-c++/visual_track_control/include/visual_track_control/rate_limiter.h create mode 100644 reference-c++/visual_track_control/include/visual_track_control/staleness_checker.h create mode 100644 reference-c++/visual_track_control/include/visual_track_control/tracking_control_law.h create mode 100644 reference-c++/visual_track_control/include/visual_track_control/vtc_config.h create mode 100644 reference-c++/visual_track_control/include/visual_track_control/vtc_types.h create mode 100644 reference-c++/visual_track_control/src/line_of_sight.cpp create mode 100644 reference-c++/visual_track_control/src/mavlink_command_gateway.cpp create mode 100644 reference-c++/visual_track_control/src/pid_controller.cpp create mode 100644 reference-c++/visual_track_control/src/tracking_control_law.cpp create mode 100644 reference-c++/visual_track_control/src/vtc_config.cpp create mode 100644 reference-c++/visual_track_control/tests/frame_transform_test.cpp create mode 100644 reference-c++/visual_track_control/tests/line_of_sight_test.cpp create mode 100644 reference-c++/visual_track_control/tests/mavlink_command_gateway_test.cpp create mode 100644 reference-c++/visual_track_control/tests/pid_controller_test.cpp create mode 100644 reference-c++/visual_track_control/tests/rate_limiter_test.cpp create mode 100644 reference-c++/visual_track_control/tests/staleness_checker_test.cpp create mode 100644 reference-c++/visual_track_control/tests/tracking_control_law_test.cpp create mode 100644 reference-c++/visual_track_control/tests/vtc_config_test.cpp create mode 100644 reference-c++/visual_track_control/视觉跟踪控制律实现文档.md create mode 100644 utils/README.md create mode 100644 utils/__pycache__/conftest.cpython-312-pytest-9.1.1.pyc create mode 100644 utils/config/track_control.json create mode 100644 utils/conftest.py create mode 100644 utils/demo_track_control.py create mode 100644 utils/pytest.ini create mode 100644 utils/target_tracker/README.md create mode 100644 utils/target_tracker/__init__.py create mode 100644 utils/target_tracker/__pycache__/__init__.cpython-312.pyc create mode 100644 utils/target_tracker/__pycache__/kalman_box_filter.cpython-312.pyc create mode 100644 utils/target_tracker/__pycache__/target_tracker.cpython-312.pyc create mode 100644 utils/target_tracker/__pycache__/tracklet.cpython-312.pyc create mode 100644 utils/target_tracker/kalman_box_filter.py create mode 100644 utils/target_tracker/target_tracker.py create mode 100644 utils/target_tracker/tracklet.py create mode 100644 utils/tests/target_tracker/__pycache__/kalman_box_filter_test.cpython-312-pytest-9.1.1.pyc create mode 100644 utils/tests/target_tracker/__pycache__/target_tracker_test.cpython-312-pytest-9.1.1.pyc create mode 100644 utils/tests/target_tracker/__pycache__/tracklet_test.cpython-312-pytest-9.1.1.pyc create mode 100644 utils/tests/target_tracker/kalman_box_filter_test.py create mode 100644 utils/tests/target_tracker/target_tracker_test.py create mode 100644 utils/tests/target_tracker/tracklet_test.py create mode 100644 utils/tests/visual_track_control/__pycache__/frame_transform_test.cpython-312-pytest-9.1.1.pyc create mode 100644 utils/tests/visual_track_control/__pycache__/line_of_sight_test.cpython-312-pytest-9.1.1.pyc create mode 100644 utils/tests/visual_track_control/__pycache__/mavsdk_gateway_test.cpython-312-pytest-9.1.1.pyc create mode 100644 utils/tests/visual_track_control/__pycache__/pid_controller_test.cpython-312-pytest-9.1.1.pyc create mode 100644 utils/tests/visual_track_control/__pycache__/rate_limiter_test.cpython-312-pytest-9.1.1.pyc create mode 100644 utils/tests/visual_track_control/__pycache__/staleness_checker_test.cpython-312-pytest-9.1.1.pyc create mode 100644 utils/tests/visual_track_control/__pycache__/tracking_control_law_test.cpython-312-pytest-9.1.1.pyc create mode 100644 utils/tests/visual_track_control/__pycache__/vtc_config_test.cpython-312-pytest-9.1.1.pyc create mode 100644 utils/tests/visual_track_control/frame_transform_test.py create mode 100644 utils/tests/visual_track_control/line_of_sight_test.py create mode 100644 utils/tests/visual_track_control/mavsdk_gateway_test.py create mode 100644 utils/tests/visual_track_control/pid_controller_test.py create mode 100644 utils/tests/visual_track_control/rate_limiter_test.py create mode 100644 utils/tests/visual_track_control/staleness_checker_test.py create mode 100644 utils/tests/visual_track_control/tracking_control_law_test.py create mode 100644 utils/tests/visual_track_control/vtc_config_test.py create mode 100644 utils/visual_track_control/README.md create mode 100644 utils/visual_track_control/__init__.py create mode 100644 utils/visual_track_control/__pycache__/__init__.cpython-312.pyc create mode 100644 utils/visual_track_control/__pycache__/command_sender.cpython-312.pyc create mode 100644 utils/visual_track_control/__pycache__/config.cpython-312.pyc create mode 100644 utils/visual_track_control/__pycache__/frame_transform.cpython-312.pyc create mode 100644 utils/visual_track_control/__pycache__/line_of_sight.cpython-312.pyc create mode 100644 utils/visual_track_control/__pycache__/mavsdk_gateway.cpython-312.pyc create mode 100644 utils/visual_track_control/__pycache__/pid_controller.cpython-312.pyc create mode 100644 utils/visual_track_control/__pycache__/rate_limiter.cpython-312.pyc create mode 100644 utils/visual_track_control/__pycache__/staleness_checker.cpython-312.pyc create mode 100644 utils/visual_track_control/__pycache__/tracking_control_law.cpython-312.pyc create mode 100644 utils/visual_track_control/__pycache__/types.cpython-312.pyc create mode 100644 utils/visual_track_control/command_sender.py create mode 100644 utils/visual_track_control/config.py create mode 100644 utils/visual_track_control/frame_transform.py create mode 100644 utils/visual_track_control/line_of_sight.py create mode 100644 utils/visual_track_control/mavsdk_gateway.py create mode 100644 utils/visual_track_control/pid_controller.py create mode 100644 utils/visual_track_control/rate_limiter.py create mode 100644 utils/visual_track_control/staleness_checker.py create mode 100644 utils/visual_track_control/tracking_control_law.py create mode 100644 utils/visual_track_control/types.py diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..10b731c --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,5 @@ +# 默认忽略的文件 +/shelf/ +/workspace.xml +# 基于编辑器的 HTTP 客户端请求 +/httpRequests/ diff --git a/.idea/code_drone-test.iml b/.idea/code_drone-test.iml new file mode 100644 index 0000000..d0876a7 --- /dev/null +++ b/.idea/code_drone-test.iml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/inspectionProfiles/Project_Default.xml b/.idea/inspectionProfiles/Project_Default.xml new file mode 100644 index 0000000..130a849 --- /dev/null +++ b/.idea/inspectionProfiles/Project_Default.xml @@ -0,0 +1,27 @@ + + + + \ No newline at end of file diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000..49af811 --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.zcode/plans/plan-sess_b7314a3c-d6f0-45f0-9768-58161c9cf637.md b/.zcode/plans/plan-sess_b7314a3c-d6f0-45f0-9768-58161c9cf637.md new file mode 100644 index 0000000..2c6af37 --- /dev/null +++ b/.zcode/plans/plan-sess_b7314a3c-d6f0-45f0-9768-58161c9cf637.md @@ -0,0 +1,49 @@ +# C++ → Python 移植计划（reference-c++ → utils） + +## 范围与环境 +- 移植 `reference-c++/target_tracker` 与 `reference-c++/visual_track_control` 两个子项目为 Python，放入 `utils/`；**不改动 reference-c++ 任何文件**。 +- Python 解释器：`D:\program file\conda\envs\pytorch\python.exe`（3.12.13，mavsdk 3.15.3 / numpy 2.4.3 已装）。需向该环境 `pip install pytest`。 +- 行为保真优先：单位、符号约定、限幅顺序、超龄/判丢/Coast 语义、异常消息文本全部与 C++ 版及规格文档一致。 + +## 目录布局 +``` +utils/ +├── README.md # 唯一文档（结构、运行、MAVSDK 映射决策、实机接线片段） +├── conftest.py # pytest sys.path 注入，使两包可顶层导入 +├── target_tracker/ # 包1：追踪库（仅依赖 stdlib + numpy） +│ ├── __init__.py # 导出 TargetTracker/TrackResult/Detection/FrameShape 等 +│ ├── kalman_box_filter.py # 8 维恒速卡尔曼（numpy 矩阵；Q 用状态值、R 用 abs×2 的规格差异保留） +│ ├── tracklet.py # 轨道 + ToCenterSize/ToXYXY/ComputeIoU +│ ├── target_tracker.py # 六步主流程 + 贪婪关联 + 主目标锁定；LastResult 用 threading.Lock +│ └── tests/ # pytest 移植 27 条（kalman 5 / tracklet 12 / tracker 10，含 §9.4 六条验收） +├── visual_track_control/ # 包2：控制律（不 import target_tracker） +│ ├── __init__.py +│ ├── vtc_types.py # TrackObservation/ControlSnapshot/ControlOutput/枚举（frozen dataclass） +│ ├── vtc_config.py # stdlib json 加载，校验异常消息与 C++ 一致（含字段路径） +│ ├── line_of_sight.py / pid_controller.py / rate_limiter.py / staleness_checker.py / frame_transform.py +│ ├── tracking_control_law.py # 八步单周期主流程，logging 仅模式切换打点 +│ ├── offboard_sender.py # OffboardSender 协议 + YawIntegrator + LoopbackSender（演示/测试） +│ ├── mavsdk_sender.py # MAVSDK 实现（mavsdk 懒加载，其余模块零感知） +│ ├── config/track_control.json # 从 C++ 原样拷贝 +│ └── tests/ # pytest 移植 8 套 +└── examples/ + └── track_control_demo.py # 唯一同时 import 两包的胶水点 +``` + +## 关键设计 +1. **解耦边界**：C++ 版 ControlSnapshot 内嵌 tracker::TrackResult；Python 改为控制律自有的最小结构 `TrackObservation(tracked, is_predicted, center)`。visual_track_control 全程不出现 target_tracker 导入，demo 负责字段拷贝。两包内部一律相对导入、可独立搬移；mavsdk 只在 mavsdk_sender.py 内懒加载。 +2. **MAVSDK 映射（已确认方案）**：统一 `set_velocity_ned(VelocityNedYaw)`。 + - 速率式：`YawIntegrator` 把 yaw_rate 积分为绝对航向（wrap180，dt 取发送侧单调时钟差）→ VelocityNedYaw(vn, ve, vd, 积分航向)； + - 位置式：直接用 out.yaw_deg；刹车：零速 + 保持积分航向；降级：不发送。 + - 控制律输出仍与 C++ 一致（机体系计算限幅→输出端旋转为 NED），NED 分量直接对应 MAVSDK 字段；OffboardError 捕获后返回 False（对齐 C++ Send 布尔语义）。offboard.start()/stop() 生命周期不属于 sender，README 给接线片段。 +3. **数值**：全程 Python float（双精度），卡尔曼用 numpy（4×4 求逆 + 1e-9 正则化）；保留检测入口 NaN/Inf 过滤、置信度 NaN→−∞ 排序等防御逻辑；时间戳 `time.monotonic_ns()//10**6`。 +4. **MAVSDK sender 测试**：用 fake offboard 对象（鸭子类型）验证分发与积分行为，不 import mavsdk；gateway 相关 C++ 字节级用例改为验证 sender 收到的 VelocityNedYaw 内容。 + +## 验证与交付 +1. `pip install pytest`（pytorch 环境）→ 两包测试全绿； +2. 运行 demo（asyncio，20Hz，三阶段：正常 6s → Coast 4s → 判丢 3s，演示判丢阈值 60 帧），输出语义与 C++ 版一致； +3. mavsdk_sender 冒烟测试（fake offboard，不连实机）； +4. 撰写 utils/README.md。 + +## 实施顺序 +pip 安装 pytest → target_tracker 包+测试 → visual_track_control 包+测试 → sender 层+测试 → demo → README → 汇总报告。 \ No newline at end of file diff --git a/docs/物理追踪思路.md b/docs/物理追踪思路.md new file mode 100644 index 0000000..5dc77c0 --- /dev/null +++ b/docs/物理追踪思路.md @@ -0,0 +1,371 @@ +# 视觉跟踪控制律设计（FlightController 实现期规格） + +## 1. 文档状态与范围 + +### 1.1 文档状态 + +- 本文档为**实现期规格文档**，由早期草稿《物理追踪实现路线 v1.0》重写而来： + 原文档中与既有设计体系冲突的表述（摄像头型号、飞控直连、控制时机、 + 拦截设备绑定、雷达距离语义等）已全部修正，合理的控制思路 + （水平角控制、垂直控制、距离控制）保留并纳入本规格。 +- 当前相关代码均为骨架占位实现，与 `docs/开发进度.md`（第 6 步未开始）一致： + - `src/control/flight_controller.cpp`（IFlightController Stub） + - `src/state_machine/mission_state_machine.cpp`（IMissionStateMachine Stub） + - `src/perception/target_estimator.cpp`（ITargetEstimator Stub） +- 本文档中所有阈值、限幅值、PID 参数**只列占位，不填具体数值**， + 一律标注"待定/配置化"（需求分析.md NFR-007）。 + +### 1.2 范围与状态机对应 + +本文档定义视觉跟踪阶段（位置→速度控制切换之后）的控制律： + +| 状态机阶段 | 本文档覆盖内容 | +|------------|----------------| +| S5 GPS_APPROACH | 仅提供上下文（位置引导阶段，控制律不在本文档范围） | +| S6 VISUAL_HANDOVER | 切换后首条限幅指令的产生要求（切换条件本身见 状态机设计.md S6） | +| S7 VISUAL_TRACKING | **本文档核心**：水平航向、垂直/高度、距离三路控制律 | +| S8 OBSTACLE_HOLD | 控制律输出被障碍策略接管（限速/刹停），见第 9 章 | + +### 1.3 文档定位与引用关系 + +本文档定位为**视觉跟踪控制律设计（FlightController 实现期规格）**。 +与以下内容重叠的部分一律引用、不复述： + +| 引用文档 | 引用章节 | 本文档不重复的内容 | +|----------|----------|--------------------| +| `docs/状态机设计.md` | S6、S7、§4.2、§7 | 切换守卫条件、目标丢失转移、可替换策略接口 | +| `docs/感知与避障设计.md` | §3、§5、§6、§9 | 标定流程、稳定锁定、雷达关联条件、降级原则 | +| `docs/视觉追踪技术路线.md` | §1.2、§2 | TargetTracker 算法细节、Coast/判丢行为约定 | +| `docs/系统架构设计.md` | §6、§8 | 数据流、控制单出口、时间与控制周期 | +| `docs/数据接口文档.md` | §2.2、§3、§4.9~§4.12、§5 | 消息字段定义、接口签名、Topic 一览 | +| `docs/需求分析.md` | FR-033、FR-041、FR-051、NFR-007、NFR-008、§7 | 需求条文本身 | +| `include/common/types.h` | 全文 | 消息类型与 Topic 常量的权威定义 | +| `target_tracker/target_tracker.md` | §2、§7 | 追踪库接口与未来接线指引 | + +## 2. 输入输出与数据流 + +### 2.1 输入 + +控制律在状态机与控制线程中按固定周期消费以下快照（Topic 名称常量见 +`include/common/types.h` 的 `drone::common::topics` 命名空间）： + +| Topic 常量 | 消息类型 | 用途 | +|------------|----------|------| +| `kEstimatedTarget` | `TargetState` | 目标估计状态（位置/速度/置信度/最近观测时间），控制律主输入 | +| `kFlightState` | `FlightStateSnapshot` | 本机姿态、速度、局部位置，用于坐标转换与反馈 | +| `kLaserRange` | `LaserRangeSample` | 机头前向单点距离；**仅在相机—雷达标定完成且空间关联成立时**才可作为目标距离，否则仅为前向安全距离（需求分析.md FR-041） | +| `kGroundTarget` | `GroundStationTarget` | 地面站目标状态，用于丢失校验与降级回退（非视觉控制主输入） | + +### 2.2 输出与控制链路 + +控制律不直接写飞控。链路严格遵循 系统架构设计.md §6 的控制单出口约束： + +```text +状态机与控制线程（控制律，本文档范围） + -> ControlIntent（控制意图，kControlIntent） + -> IFlightController（意图 -> 设定值转换与限幅） + -> Px4Setpoint（kPx4Setpoint） + -> IPx4Link（PX4 通信线程） + -> 飞控串口 +``` + +- 只有 PX4 通信线程可以写飞控串口；控制律只提交意图 + （系统架构设计.md §6、数据接口文档.md §4.9）。 +- 视觉跟踪阶段主要使用： + - `ControlIntentType::kVelocityHeading`（速度与航向意图）： + `target_x/target_y/target_z` 为速度分量（坐标系由 `header.frame_id` 给出）， + `yaw_deg` 为目标航向，`max_speed_mps` 携带速度限制； + - `ControlIntentType::kBrakeHover`（刹停/悬停意图）：目标丢失判丢、 + 障碍制动等场景； + - `ControlIntentType::kManualOverride`（禁止自动输出/人工接管意图）： + 遥控器接管时停止任务控制输出。 +- `Px4Setpoint` 使用 `SetpointType::kVelocity`（速度设定值，分量写入 + `x`、`y`、`z`）或 `SetpointType::kAttitude`（姿态微调，仅限垂直小误差 + 支路，见第 5 章）；航向写入 `yaw_deg` 或 `yaw_rate_dps`； + 无效设定值（如刹停完成）也需发送，避免过期指令（`valid` 标志语义见 + `include/common/types.h`）。 + +### 2.3 坐标系编码 + +所有空间数据消息显式使用 `MessageHeader.frame_id` 编码 +（数据接口文档.md §2.2、`include/common/types.h` 文件头约定）： + +```text +0 = 未知 +1 = WGS84 经纬度 +2 = 机体（前右下） +3 = 局部NED +4 = 局部ENU +``` + +控制律内部计算统一在**机体坐标系（frame_id = 2）**下进行 +（视线角→机体前/右/下分量），输出的速度意图若采用局部系 +（frame_id = 3），必须随 `FlightStateSnapshot` 的姿态/航向完成旋转， +并在 `header.frame_id` 中显式标注；禁止混用未标注坐标系的数据。 + +## 3. 坐标与标定前提 + +### 3.1 转换链路 + +```text +目标像素位置 (u, v) + ->（相机内参 + 畸变校正）-> 归一化像平面方向 + ->（相机→机体固定外参）-> 机体视线方向 + ->（结合当前机体姿态）-> 水平偏航误差 / 垂直俯仰误差 + ->（雷达关联成立时叠加距离）-> 机体系相对位置 (Δ前, Δ右, Δ下) +``` + +视觉像素只有结合相机模型和对应时刻的机体姿态，才能转换为目标相对机体 +的视线方向（感知与避障设计.md §3）。 + +### 3.2 标定前提与待定项 + +实现视觉控制前必须完成（感知与避障设计.md §3、需求分析.md §7）： + +- 相机内参与畸变标定（**待定**：相机内参、畸变参数未标定）； +- 相机坐标系到机体坐标系的固定外参（**待定**：外参未标定）； +- 激光波束在相机图像中的投影位置或区域（**待定**：相机—雷达标定未完成）； +- 图像、检测、光流、雷达和 PX4 姿态的时间对齐； +- 目标和本机高度基准（**待定**，需求分析.md §7）。 + +摄像头为 SIYI A8mini，经网口以 RTSP 输出 H.265、1080p、25 FPS 视频 +（需求分析.md §3、开发思路.md）；画面宽度 `W`、高度 `H` 与帧率以实际 +解码配置为准。标定完成前，控制律只允许在 SITL/台架中以录制数据回放 +验证数学链路，不得上实飞。 + +## 4. 水平航向控制 + +### 4.1 目标与原理 + +目标：让机头在水平方向对准目标。由目标像素水平偏移反求偏航修正量， +经限幅后作为航向指令。 + +设目标中心像素横坐标偏移为 `x_offset`（目标中心相对画面中心，右正）， +图像宽度为 `W`，相机**水平视场角**为 `θ_h`（注意：`θ_h` 仅表示水平 +视场角，与俯仰姿态角无关，不得复用同一符号）。 + +针孔模型下的精确表述： + +``` +β = atan( x_offset * 2 * tan(θ_h / 2) / W ) +``` + +当目标靠近画面中心（|x_offset| 相对 W 较小）时，可用线性近似： + +``` +β ≈ x_offset * ( θ_h / W ) +``` + +线性近似的适用条件： + +- 偏移量处于画面中心附近（具体适用范围**待定**，由标定与实测确定）； +- 已完成畸变校正（畸变在画面边缘放大近似误差）； +- 大偏移场景应使用针孔模型表述，或直接采用归一化像平面方向。 + +机体姿态变化时，像素偏移同时包含本机转动与目标运动两部分， +控制律需结合 `FlightStateSnapshot` 的姿态与角速度做补偿 +（实现期细化，补偿结构**待定**）。 + +### 4.2 输出映射 + +- 航向指令映射到 `ControlIntentType::kVelocityHeading` 的 `yaw_deg`， + 或由 `Px4Setpoint` 的 `yaw_deg` / `yaw_rate_dps` 表达； +- 采用位置式（β→目标航向）还是速率式（β→偏航角速度）由实现期 + 按 PX4 1.17.0 Offboard 行为确定（**待定**）； +- 偏航角速度与方向变化率必须限幅（需求分析.md FR-033）， + 限幅值见第 8 章。 + +## 5. 垂直 / 高度控制 + +### 5.1 误差来源 + +垂直误差由两个来源合成： + +1. 像素垂直偏移：目标中心相对画面中心的纵坐标偏移 `y_offset`（下正）， + 图像高度 `H`，相机**垂直视场角** `θ_v`。与第 4 章同理，垂直视线角： + + ``` + α = atan( y_offset * 2 * tan(θ_v / 2) / H ) （针孔模型） + α ≈ y_offset * ( θ_v / H ) （中心附近线性近似） + ``` + +2. 距离信息：只有雷达与目标完成空间关联（见 3.2 与第 6.3 节）时， + 才可由测距距离 `d` 换算高度差： + + ``` + Δh = d * sin(α) + ``` + + （等价于早期草稿中的 `Δh = d × cos(90° − α)`；此处 α 为垂直视线角， + 与水平视场角 θ_h、垂直视场角 θ_v 明确区分命名。） + +雷达未关联时，垂直通道只能使用角度误差做姿态/速率级控制， +不得假设已知高度差。 + +### 5.2 控制策略 + +垂直控制按角度误差大小分两支（沿用原草稿思路，阈值待定）： + +- **角度误差较小**：直接微调姿态角（`SetpointType::kAttitude`）， + 仅作辅助，必须有严格姿态角与姿态变化率限幅（需求分析.md FR-033）； +- **角度误差较大**：调整高度，使用速度设定值（`SetpointType::kVelocity` + 的 `z` 分量，局部 NED 下为垂直速度）趋近目标高度。 + +角度误差切换阈值、姿态角限幅、垂直速度限幅均**待定**（第 8 章）。 + +## 6. 距离控制 + +### 6.1 目标 + +距离控制使目标进入拦截射程。**射程进入条件由拦截设备接口 +（`IInterceptionDevice`）与配置给出**，本文档不假设任何具体拦截方式； +具体捕网或其他拦截方式不得进入通用逻辑（需求分析.md FR-051、 +状态机设计.md §7）。射程 `d_exp` 为配置项，数值**待定**。 + +### 6.2 控制律 + +设目标距离为 `d`（来源见 6.3），射程为 `d_exp`： + +``` +距离误差：Δd = d - d_exp +前向速度指令 = PID(Δd) +``` + +- 对 `Δd` 做 PID 得到机体前向速度指令，采用速度指令实现 + （`ControlIntentType::kVelocityHeading` 的速度分量 → + `SetpointType::kVelocity`）； +- `d > d_exp` 时前向接近，`d < d_exp` 时减速/后退，接近末端的速度 + 必须按限幅约束收敛； +- PID 参数（Kp/Ki/Kd、积分限幅、微分滤波）全部**待定/配置化**。 + +### 6.3 距离来源与降级 + +- 雷达距离**不得直接当作目标距离**：`LaserRangeSample` 表达机头前向 + 单点距离，不得自动解释为任意视觉目标的距离 + （`include/common/types.h` 注释、通信与数据定义.md §7）。 + 仅当相机—雷达标定完成、视觉区域覆盖雷达投影点、时间足够接近且 + 测量质量有效时，才可关联为目标距离（需求分析.md FR-041、 + 感知与避障设计.md §3）； +- 关联失败时，雷达距离仅作为**前向安全距离**，供障碍策略 + （第 9 章）使用；距离通道此时按配置降级：保持当前速度、 + 限速接近或退出近距操作（具体降级动作**待定**）； +- 视觉估计的距离趋势（目标估计器输出）可作为辅助输入， + 融合权重**待定**。 + +### 6.4 与追踪库的行为衔接 + +沿用 视觉追踪技术路线.md §1.2 的上游行为约定： + +- 追踪库输出 `is_predicted == true`（Coast 预测中）时，距离与航向 + PID 的**积分项清空**，防止积分饱和； +- 追踪库输出 `tracked == false`（判丢）时，执行**零速刹车** + （`ControlIntentType::kBrakeHover`），并按 状态机设计.md §4.2 + 的目标丢失转移处理； +- 停止视觉闭环时调用追踪库 `Reset()` 清空状态。 + +## 7. 控制周期与时序 + +### 7.1 固定控制周期 + +- 控制线程按**固定周期持续运行**，不等待任意传感器消息后被动执行； + 视觉跟踪控制律在每个控制周期内持续执行，而非"检测完成后才进行" + （系统架构设计.md §8）； +- 每次控制计算使用同一时刻附近的 PX4、视觉、雷达和目标数据快照； +- 超龄数据只能触发降级或告警，不能继续生成正常控制指令 + （超龄判定使用 `MessageHeader.receive_time_ms` + `valid_for_ms`）； +- 控制周期频率与各输入超龄阈值**待定/配置化**（系统架构设计.md §8）。 + +### 7.2 与 target_tracker 的衔接 + +目标追踪库（`target_tracker/`，已实现，见 `target_tracker/target_tracker.md`） +承担像素级平滑、Coast 与主目标锁定： + +- 推理侧每帧调用 `TargetTracker::Update()`，控制线程（约 20Hz 速度闭环） + 读取 `TrackResult`（`LastResult()` 线程安全拷贝）； +- 控制线程消费 `TrackResult` 的平滑中心点计算第 4/5 章角度误差， + 并执行 6.4 的 Coast/判丢约定； +- `TrackResult` 属于追踪库自有类型，**尚未进入 + `include/common/types.h`**；`target_tracker/target_tracker.md` §7 + 提议新增 `TrackedTarget` 消息与 `kTrackedTarget` 主题， + 该提议当前**未实现**，接入主工程时需先在数据接口文档与 + types.h 中正式定义。 + +## 8. 限幅、阈值与配置化 + +所有阈值集中于此并进入配置文件，不得散落在代码中 +（需求分析.md NFR-007）。视觉跟踪的速度、加速度和方向变化 +必须限幅（需求分析.md FR-033）。 + +| 项目 | 说明 | 状态 | +|------|------|------| +| 水平/垂直角度误差切换阈值 | 姿态微调与高度/速率控制的分支条件 | 待定 | +| 射程 `d_exp` | 由拦截设备接口与配置给出 | 待定 | +| 距离通道 PID 参数 | Kp / Ki / Kd、积分限幅 | 待定 | +| 航向通道增益与限幅 | 偏航角速度上限、方向变化率上限 | 待定 | +| 垂直速度限幅 | 上升/下降速度上限 | 待定 | +| 姿态角限幅 | 姿态微调支路的角与角速率上限 | 待定 | +| 前向速度限幅 | 接近/后退速度上限，随距离收敛 | 待定 | +| 加速度限幅 | 各轴加速度上限 | 待定 | +| 控制周期频率 | 控制线程固定周期 | 待定 | +| 各输入超龄阈值 | 视觉/雷达/姿态/目标估计 | 待定 | +| 线性近似适用范围 | 画面中心偏移比例 | 待定 | + +首条视觉速度设定值必须经过限幅，避免位置→速度切换产生突变 +（状态机设计.md S6）。 + +## 9. 异常与降级 + +### 9.1 目标丢失 + +- 判丢后执行零速刹车并调用 `ITargetLostStrategy`：短时预测 + （预测时长与不确定度有上限）、最新有效 `kGroundTarget` 校验、 + 整机转向搜索或退回 `GPS_APPROACH`（状态机设计.md §4.2、 + 感知与避障设计.md §5）； +- 卡尔曼预测不得无限期生成控制指令（需求分析.md FR-034）。 + +### 9.2 传感器降级 + +按 感知与避障设计.md §9 的降级原则，与本文档控制律相关者： + +| 故障 | 视觉跟踪阶段动作 | +|------|------------------| +| 摄像头失效 | 退出视觉跟踪 | +| 光流失效 | 降低跟踪可信度和速度 | +| 雷达失效 | 降低速度或退出近距操作；距离通道仅保留角度级控制 | +| 地面站目标过期 | 可按策略短时视觉跟踪 | +| PX4 姿态过期 | 禁止空间转换和控制，立即退出正常控制 | + +### 9.3 障碍物规避 + +前向危险时控制律输出被障碍策略接管：清除视觉速度意图，限速、刹停并 +悬停（S8 OBSTACLE_HOLD，状态机设计.md S8、感知与避障设计.md §7）。 +雷达未关联时的前向安全距离即该策略的主输入。危险解除后必须重新 +验证入口条件才可恢复视觉控制（需求分析.md FR-044）。 + +## 10. 待定项与验证计划 + +### 10.1 待定项清单 + +| 待定项 | 依据 | +|--------|------| +| 相机内参、畸变参数、相机—机体—雷达外参 | 需求分析.md §7 | +| 目标和本机高度基准 | 需求分析.md §7 | +| 第 8 章全部阈值、限幅与 PID 参数 | 需求分析.md §7、NFR-007 | +| 雷达—目标空间关联的具体判据参数 | 感知与避障设计.md §3 | +| 航向控制位置式/速率式选择 | PX4 1.17.0 Offboard 实测 | +| 机体姿态补偿结构 | 实现期细化 | +| `TrackedTarget` / `kTrackedTarget` 消息定义 | target_tracker.md §7 提议，未入库 | + +### 10.2 验证路径 + +按 需求分析.md NFR-008 与 系统架构设计.md §11 逐级验证： + +1. **开发机单元测试**：控制律纯函数（角度换算、PID、限幅）用 + Google Test 覆盖；录制数据回放验证像素→视线角数学链路； +2. **PX4 SITL**：在仿真中验证设定值流、限幅行为、丢失/降级分支； +3. **串口台架**：真实飞控 + 模拟数据，验证时序、超龄判定与 + 控制权仲裁； +4. **受控实飞**：按状态机设计验证 S6→S7 切换与跟踪效果。 + +必测故障项包含旧目标位置、旧视频帧、雷达异常、遥控器接管、 +算力板控制输出中断（系统架构设计.md §11）。 diff --git a/docs/视觉跟踪控制律技术路线.md b/docs/视觉跟踪控制律技术路线.md new file mode 100644 index 0000000..3307769 --- /dev/null +++ b/docs/视觉跟踪控制律技术路线.md @@ -0,0 +1,1029 @@ +# 视觉跟踪控制律（visual_track_control）技术路线 + +> 本文档把 `visual_track_control/` 子项目已实现的控制律抽象为**与编程语言无关**的 +> 技术路线：只讲逻辑、数据结构、算法与实现方式，不依赖 C++ 语法特性。 +> 可作为任意语言（C++/Rust/Python 仿真/嵌入式 C）重新实现或移植的规格说明。 +> +> **文档关系**（三份文档各司其职，不重复抄录）： +> +> | 文档 | 定位 | 权威性 | +> |------|------|--------| +> | `docs/物理追踪思路.md` | 控制律**物理与行为规格**（为什么这样控） | 数学与行为的唯一权威 | +> | `docs/视觉跟踪控制律技术路线.md`（本文） | **语言无关实现路线**（逻辑怎么落地） | 实现结构与算法流程的权威 | +> | `visual_track_control/视觉跟踪控制律实现文档.md` | **C++ 实现说明**（构建、日志、排查） | 当前 C++ 落地的权威 | +> +> 文中出现的字段名（如 `track_time_ms`、`yaw_rate_dps`）取自当前实现，仅作为 +> **跨文档可追溯的标识符**，不构成语言绑定；重写时可自由改名，但语义与单位必须一致。 +> +> ⚠️ **全部数值均为占位值**：相机内参/畸变未标定、增益与 PID 未整定。标定完成前 +> 本路线**仅限回放/SITL 验证数学链路，不得用于实飞**（见第 11 章红线）。 + +--- + +## 目录 + +1. [概述与系统定位](#1-概述与系统定位) +2. [输入输出接口定义](#2-输入输出接口定义) +3. [核心构件与状态清单](#3-核心构件与状态清单) +4. [坐标系与符号约定](#4-坐标系与符号约定) +5. [单周期控制流程（主流程伪代码）](#5-单周期控制流程主流程伪代码) +6. [各通道算法细节](#6-各通道算法细节) +7. [安全语义：超龄、判丢、Coast、无距离](#7-安全语义超龄判丢coast无距离) +8. [输出打包与链路](#8-输出打包与链路) +9. [配置参数全量表](#9-配置参数全量表) +10. [设计取舍记录（为什么不是别的做法）](#10-设计取舍记录为什么不是别的做法) +11. [实现与重写指引](#11-实现与重写指引) +12. [验证路线与待定项](#12-验证路线与待定项) + +--- + +## 1. 概述与系统定位 + +### 1.1 模块职责 + +本模块是**视觉跟踪阶段（状态机 S7 VISUAL_TRACKING）的单周期控制律**，职责可归纳为 +"三算一限一降级一打包"： + +1. **三路控制律计算**（每拍一次，固定周期） + - **水平航向**：目标像素偏移 → 水平视线角 β → 增益 → 偏航指令； + - **垂直高度**：目标像素偏移 → 垂直视线角 α → 垂直速度指令； + - **距离**：雷达目标距离与期望射程之差 Δd → PID → 前向速度指令。 +2. **统一限幅**：每个通道内联完成"变化率限幅 + 幅值限幅"，任何输出都不允许 + 跳过限幅直达链路（含切换后的**首条**指令）。 +3. **安全降级**：输入超龄、目标判丢、距离不可用时，输出降级意图而不是继续 + 用旧数据算正常指令。 +4. **单出口打包**：控制意图 → MAVLink 字节流 → 注入的传输实现；控制律本身 + 不认识串口、电台或飞控。 + +### 1.2 在系统中的位置 + +```text +┌─────────────┐ 检测框 ┌──────────────┐ 追踪结果 ┌───────────────────┐ +│ YOLO 识别 │────────────▶│ TargetTracker│───────────▶│ 本模块：控制律 │ +│ （感知线程）│ 每帧 │ （滤波器） │ 主目标+ │ TrackingControlLaw│ +└─────────────┘ └──────────────┘ Coast标志 └─────────┬─────────┘ + │ +┌─────────────┐ 前向距离 │ 控制意图 +│ 激光测距雷达│──────────────────────┐ ▼ +└─────────────┘ │ ┌──────────────────────┐ +┌─────────────┐ 姿态/航向/速度 ├─────────────▶│ 打包网关 │ +│ PX4 链路 │──────────────────────┘ 控制快照 │ （控制意图→MAVLink） │ +└─────────────┘ （由调用方采集打戳） └─────────┬────────────┘ + ▲ │ 字节流 + │ MAVLink offboard 指令流 ▼ + └────────────────────────────────────────────── ┌──────────────────┐ + │ 传输抽象实现 │ + │ （串口/电台/回环）│ + └──────────────────┘ +``` + +**上游依赖**：追踪库提供主目标中心与 `tracked` / `is_predicted` 两个行为标志 +（追踪库技术路线见 `docs/视觉追踪技术路线.md`）；雷达提供机头前向单点距离与 +**关联标志**；飞控链路提供自机姿态（至少航向角）。 + +**下游依赖**：只有一个——传输抽象。打包与传输分离，控制律不直接写飞控。 + +**上层归属**：状态机（S6/S7/S8 切换、目标丢失策略、障碍接管）在**本模块之外**。 +本模块只按输入标志产生"当拍应该做什么"，不决定"要不要继续视觉闭环"。 + +### 1.3 设计特点 + +| 特点 | 含义 | 带来的约束 | +|------|------|-----------| +| **纯值输入 / 纯值输出** | 每拍传入一个不可变快照，返回一个输出值对象 | 无隐藏输入、无回调、可离线回放 | +| **单线程固定周期** | 由控制线程按配置频率调用，构件内部**无锁** | 不得从多线程并发调用同一实例 | +| **热路径零动态分配** | 所有系数、缓冲在构造期备妥；每拍只做算术 | 便于实时性分析与嵌入式移植 | +| **热路径零日志** | 仅在输出模式发生切换时打一条日志 | 日志量与状态变化次数成正比，不与频率成正比 | +| **配置全量外置** | 所有阈值/增益/限幅来自启动期加载的配置对象 | 代码内**不得**出现任何写死的控制参数 | +| **无状态纯函数构件** | 视线角转换、超龄判定、坐标旋转均为纯函数 | 可独立单元测试，无需夹具 | +| **单一 offboard 消息类型** | 全周期只发一种消息（ID 84） | 避免消息类型逐拍翻转账扰动飞控（见 §10.1） | + +### 1.4 明确不做的事（边界） + +- **不做感知**：不跑检测/光流/雷达驱动，只消费调用方填入的快照； +- **不做目标关联**：相机—雷达空间关联由感知融合层给出布尔标志，本模块只读取； +- **不做状态机仲裁**：不判断何时进入/退出视觉跟踪，不实现目标丢失策略； +- **不做链路 IO**：不打开串口、不管理重连、不解析任何入向 MAVLink 消息； +- **不做配置热更新**：启动期加载一次，调参重启生效； +- **不做多目标**：只处理追踪库选定的主目标。 + +--- + +## 2. 输入输出接口定义 + +### 2.1 输入：控制快照（每拍一个，由调用方采集并打戳） + +| 分组 | 字段 | 单位 / 类型 | 语义与约束 | +|------|------|------------|-----------| +| 视觉 | `track.center` | 像素 (x, y) | 主目标中心；原点为画面**左上角**，x 右正、y 下正 | +| 视觉 | `track.tracked` | 布尔 | 是否存在有效轨道；`false` → 判丢刹车 | +| 视觉 | `track.is_predicted` | 布尔 | 本拍是否为滤波器预测值（Coast）；`true` → 清距离积分 | +| 视觉 | `track_time_ms` | 单调时钟 ms | **取样瞬间**打戳；`<= 0` 视为从未取样，直接判超龄 | +| 雷达 | `radar_distance_m` | m | 机头前向单点距离；`<= 0` 视为不可用 | +| 雷达 | `associated_to_target` | 布尔 | 相机—雷达关联成立标志；**未关联不得当作目标距离** | +| 雷达 | `radar_time_ms` | 单调时钟 ms | 雷达取样瞬间打戳 | +| 姿态 | `attitude_present` | 布尔 | 姿态源当前是否可用；`false` → 禁止控制 | +| 姿态 | `attitude_time_ms` | 单调时钟 ms | 姿态取样瞬间打戳 | +| 姿态 | `yaw_rad` | rad (NED) | 自机航向；**参与计算**：位置式航向合成 + 输出端坐标旋转 | +| 姿态 | `roll_rad` / `pitch_rad` | rad (FRD) | 预留（机体补偿），当前不参与计算 | +| 姿态 | `yaw_rate_radps` | rad/s | 预留（机体补偿），当前不参与计算 | +| 自机速度 | `velocity_x/y/z_mps` | m/s | 预留，接入主工程后由飞控状态填充 | + +**关键约定：追踪结果自身不带时间戳。** 追踪库输出的是"最新一帧的结果"， +调用方必须在读取结果的**那一瞬间**用单调时钟打戳，写入 `track_time_ms`。 +所有超龄判定一律采用"取样时刻 + 有效期"模型（§7.1），不使用"消息序号"或 +"到达时刻"。 + +**时钟要求：单一单调时钟源。** 快照内三类时间戳与 `Update()` 的 `now_ms` +必须来自同一个不受墙钟调整影响的时钟（如系统启动后毫秒数）。混用墙钟会 +导致 `now < sample` 的不可核实情形，实现按"宁可降级"处理（§7.1）。 + +### 2.2 输出：控制意图（每拍一个，已限幅） + +| 字段 | 单位 | 语义 | +|------|------|------| +| `mode` | 枚举 | `kVelocityHeading` / `kBrakeHover` / `kDegradedHold` | +| `valid` | 布尔 | 是否可交付链路发送；`kDegradedHold` 恒为 `false` | +| `vx_mps` | m/s | **局部 NED 北向**速度（机体系前向经航向旋转得到） | +| `vy_mps` | m/s | **局部 NED 东向**速度（机体系侧向恒 0，旋转后可非零） | +| `vz_mps` | m/s | **NED 下正**垂直速度 | +| `yaw_deg` | deg (NED) | **绝对**目标航向，回绕到 [-180, 180)；仅位置式模式非零 | +| `yaw_rate_dps` | deg/s | 偏航角速度指令（相对量）；仅速率式模式非零 | +| `degraded_reason` | 枚举 | `kNone` / `kAttitudeStale` / `kVisionStale` / `kNoDistanceExit` | +| `coast_active` | 布尔 | 本拍处于 Coast 预测跟踪 | + +**输出坐标系已在库内完成旋转**：速度分量对外一律是局部 NED（北/东/地）， +与打包时声明的坐标框一致。调用方**不得**再做二次旋转。 + +### 2.3 输出模式与可发送性矩阵 + +| 模式 | `valid` | 速度分量 | 航向字段 | 链路动作 | +|------|---------|---------|---------|---------| +| `kVelocityHeading` | `true` | 三路限幅后旋转值 | 按配置的航向模式**二选一**非零 | 发送速度+航向指令 | +| `kBrakeHover` | `true` | 全零 | 全零（掩码忽略 yaw 与 yaw_rate） | 发送零速刹车 | +| `kDegradedHold` | `false` | 全零 | 全零 | **不发送**，仅告警/记录降级原因 | + +### 2.4 传输抽象契约（唯一出向依赖） + +```text +接口 CommandTransport: + Send(data: 字节序列, len: 长度) -> 布尔 + 返回 true = 已交付底层链路 + 返回 false = 发送失败（调用方可计数/告警，控制律不重试） +``` + +契约要点： + +- **只吃字节流**：串口、电台、回环打印、文件录制均可注入实现；换传输层 + 不改动打包与控制律； +- **同步交付**：实现方不得长期持有传入的字节指针（同步拷贝或同步写出）， + 因此打包侧可以复用同一块固定缓冲； +- **无反向通道**：本模块不接收任何入向消息，传输实现无需提供接收能力。 + +--- + +## 3. 核心构件与状态清单 + +控制律由 1 个主控律 + 6 个构件组成。下表列出**每个构件需要持久化的状态**， +这是重写/移植时最容易漏掉的部分——状态漏了，行为就变了。 + +| 构件 | 职责 | 构造期预计算 | 运行期持久状态 | 复位入口 | +|------|------|-------------|--------------|---------| +| 主控律 | 八步流程编排 | 各构件实例、配置值拷贝 | 上拍时刻 `last_now_ms`、上拍模式 `last_mode` | `Reset()` 清全部 | +| 视线角转换器 ×2 | 像素偏移 → β / α | 针孔系数 `2·tan(θ/2)/size` | 无（纯函数） | 不需要 | +| 距离 PID | Δd → 前向速度 | PID 参数、输出限幅 | 积分累计、上拍误差、低通后微分值、上次输出、"是否存在有效微分历史"标志 | `ResetIntegral()` 只清积分；`Reset()` 清全部 | +| 变化率限幅器 ×4 | 限制每拍变化量 | 变化率上限 | 上次输出值 | `Reset(value)` | +| 静态限幅 | 幅值钳制 | 无 | 无（纯函数） | 不需要 | +| 超龄判定器 | 输入是否过期 | 无 | 无（纯函数） | 不需要 | +| 坐标旋转 | 机体系 → NED | 无 | 无（纯函数） | 不需要 | + +**四个变化率限幅器分别守护什么**： + +| 限幅器 | 守护量 | 单位 | 生效通道 | +|--------|-------|------|---------| +| 航向角限幅器 | 位置式航向修正量的变化率 | deg/s（见 §12.2 量纲待澄清项） | 水平航向（位置式） | +| 航向角速率限幅器 | 偏航角速度指令的变化率 | deg/s² | 水平航向（速率式，默认） | +| 前向速度限幅器 | 前向加速度 | m/s² | 距离通道 | +| 垂直速度限幅器 | 垂直加速度 | m/s² | 垂直通道 | + +**为什么正好是 4 个**：机体系侧向速度恒为 0（本控制律不做侧移），因此 +`ay_mps2` 配置项当前为预留、无对应限幅器实例。 + +--- + +## 4. 坐标系与符号约定 + +### 4.1 三个坐标系 + +| 坐标系 | 轴定义 | 用途 | +|--------|-------|------| +| 像素系 | 原点左上角，x 右正、y 下正 | 追踪库输出的原始语义 | +| 机体系 (FRD) | x 前、y 右、z 下 | **控制律内部计算**（三路通道各自算出机体量） | +| 局部 NED | x 北、y 东、z 地 | **对外输出与打包声明** | + +### 4.2 像素偏移与视线角符号 + +```text +x_offset = center.x − W/2 # 目标偏右 → 正 +y_offset = center.y − H/2 # 目标偏下 → 正 + +β = 水平视线角（弧度）：与 x_offset 同号 # 目标偏右 → β 正 → 需右转 +α = 垂直视线角（弧度）：与 y_offset 同号 # 目标偏下 → α 正 +``` + +**垂直符号链**（NED 下正约定）：目标偏低 → α 为正 → 期望向下运动 → NED 垂直 +速度 `vz` 为正（下正）。因此垂直通道**不需要额外取反**，增益为正值即可。 + +**水平符号链**（速率式）：目标偏右 → β 为正 → 偏航角速度指令为正。按 NED +航向"自北顺时针（右转为正）"约定，正角速度即右转，趋近目标。因此水平通道 +同样不需要额外取反。 + +### 4.3 机体系 → NED 旋转（只随航向角） + +```text +给定机体速度 (v_fwd, v_right, v_down) 与自机航向 ψ（rad，自北顺时针为正）： + +v_north = v_fwd·cos ψ − v_right·sin ψ +v_east = v_fwd·sin ψ + v_right·cos ψ +v_down = v_down # 绕下轴旋转不影响垂直分量 +``` + +**为什么只随航向旋转、不含滚转/俯仰**：速度设定值的姿态耦合项由飞控内环 +处理，这是 offboard 速度设定值的标准做法；滚转/俯仰对水平速度指令是二阶小量。 + +**正方向自检**（实现时必须通过的两条断言）： + +| 输入 | ψ | 期望输出 | 说明 | +|------|---|---------|------| +| 机体前向 (1, 0, 0) | 90°（机头朝东） | (0, 1, 0) = 东 | 前向随航向转向东 ✓ | +| 机体右向 (0, 1, 0) | 0°（机头朝北） | (0, 1, 0) = 东 | 机体右侧即地理东 ✓ | + +### 4.4 限幅与旋转的先后顺序（正确性论证） + +**顺序：限幅在机体系完成 → 旋转在输出端执行。** + +论证：绕下轴旋转是**正交变换**，保持向量范数不变，即 +`√(v_north² + v_east²) = √(v_fwd² + v_right²)`。因此机体系内满足的幅值约束 +在旋转后仍然成立，先限幅后旋转**不破坏限幅语义**；反过来（先旋转后限幅） +则会让限幅作用在地理轴上，与"机体能力限制"的物理含义不符。 + +推论：输出的 `vy_mps` **可以非零**（机体系侧向恒 0，旋转后东向分量非零）， +这不是缺陷，调用方不得把它当作异常。 + +### 4.5 姿态不可用时的处理 + +旋转与位置式航向合成都依赖 `yaw_rad`。因此**姿态过期/缺失必须在任何坐标 +运算之前拦截**（流程第 ① 步），直接进入降级；不允许"用上一拍航向凑合旋转"， +因为那会产生坐标系标注与实际不符的输出（规格明令禁止混用未标注坐标系数据）。 + +--- + +## 5. 单周期控制流程（主流程伪代码） + +### 5.1 节拍推算 + +```text +函数 计算dt(now_ms) -> 秒: + dt ← 1.0 / 配置.control.frequency_hz # 默认：配置频率兜底 + 若 last_now_ms > 0 且 now_ms > last_now_ms: # 时钟正常前进才信任差值 + dt ← min( (now_ms − last_now_ms) / 1000.0, DT_MAX ) + last_now_ms ← now_ms + 返回 dt + +常量 DT_MAX ← 0.5 秒 # 异常长间隔上限 +``` + +**三条兜底规则的动机**： + +| 情形 | 处理 | 动机 | +|------|------|------| +| 首拍（`last_now_ms == 0`） | 用配置频率推算 dt | 无上拍参考，不允许 dt=0 触发除零 | +| 时钟回拨或同值（`now <= last`） | 用配置频率推算 dt | 差值不可信；不引入额外状态去纠错 | +| 间隔异常长（`> 0.5 s`） | 截断到 0.5 s | 防止限幅器一步放过巨大跳变 | + +### 5.2 八步主流程 + +```text +函数 Update(快照, now_ms) -> 控制意图: + dt ← 计算dt(now_ms) + 输出 ← 新建控制意图() + 输出.mode ← kVelocityHeading # 正常路径默认；分支可覆盖为降级/刹车 + + # ---- ① 超龄判定（最高优先级：数据不可信就不算） ---- + 若 (非 快照.attitude_present) 或 已超龄(快照.attitude_time_ms, now_ms, 配置.attitude_stale_ms): + 输出.mode ← kDegradedHold; 输出.degraded_reason ← kAttitudeStale + 返回 收尾并记录模式切换(输出, "姿态过期/缺失，禁止控制") + + 若 已超龄(快照.track_time_ms, now_ms, 配置.visual_stale_ms): + 输出.mode ← kDegradedHold; 输出.degraded_reason ← kVisionStale + 返回 收尾并记录模式切换(输出, "视觉追踪结果超龄") + + # ---- ② 判丢 → 零速刹车 ---- + 若 非 快照.track.tracked: + 若 last_mode ≠ kBrakeHover: # 仅在"进入沿"清状态 + 距离PID.全清(); 四个限幅器.复位(0) + 输出.mode ← kBrakeHover; 输出.valid ← true # 速度/航向字段保持全零 + 返回 收尾并记录模式切换(输出, "判丢，零速刹车") + + # ---- ③ Coast → 清距离积分 ---- + 若 快照.track.is_predicted: + 距离PID.清积分() # 只清积分，保留微分历史 + 输出.coast_active ← true + # 航向通道为纯比例映射，无积分项，无需清理 + + # ---- ④ 像素偏移 → 视线角 ---- + x_offset ← 快照.track.center.x − 配置.image_width / 2 + y_offset ← 快照.track.center.y − 配置.image_height / 2 + β_deg ← 水平视线角转换器(x_offset) × 弧度转度 # 针孔模型，见 §6.1 + α_deg ← 垂直视线角转换器(y_offset) × 弧度转度 + + # ---- ⑤ 水平航向通道 ---- + 若 配置.heading.mode == 速率式: + 目标角速率 ← 配置.heading.gain × β_deg + 限速后 ← 航向角速率限幅器.推进(目标角速率, dt) # 先变化率 + 输出.yaw_rate_dps ← 钳制(限速后, ±配置.yaw_rate_limit_dps) # 再幅值 + 否则: # 位置式 + 目标修正量 ← 配置.heading.gain × β_deg + 修正量 ← 航向角限幅器.推进(目标修正量, dt) + # ID 84 的 yaw 字段是"绝对航向"，必须与当前航向合成，不能直发修正量 + 输出.yaw_deg ← 回绕180( 快照.yaw_rad × 弧度转度 + 修正量 ) + + # ---- ⑥ 垂直速度通道（统一速度支路） ---- + 目标垂直速度 ← 配置.vertical.vz_gain_mps_per_deg × α_deg + 限速后 ← 垂直速度限幅器.推进(目标垂直速度, dt) # 加速度限幅 + 输出.vz_mps ← 钳制(限速后, ±配置.vertical.vz_limit_mps) + + # ---- ⑦ 距离通道 ---- + 距离可用 ← 快照.associated_to_target + 且 快照.radar_distance_m > 0 + 且 非 已超龄(快照.radar_time_ms, now_ms, 配置.radar_stale_ms) + + 若 距离可用: + Δd ← 快照.radar_distance_m − 配置.distance.d_exp_m # 正=偏远，需接近 + 前向原始 ← 距离PID.更新(Δd, dt) + 限速后 ← 前向速度限幅器.推进(前向原始, dt) # 加速度限幅 + # 非对称限幅：接近可以快，后退必须慢 + 输出.vx_mps ← 钳制(限速后, −配置.retreat_velocity_limit_mps, + +配置.approach_velocity_limit_mps) + 否则: + 距离PID.清积分() # 防饱和 + 按 配置.distance.no_distance_action 分支: + 保持: 输出.vx_mps ← 0; 前向速度限幅器.复位(0) + 限速接近: 输出.vx_mps ← 前向速度限幅器.推进(配置.no_distance_approach_limit_mps, dt) + 退出近距操作: 输出.mode ← kDegradedHold + 输出.degraded_reason ← kNoDistanceExit + 返回 收尾并记录模式切换(输出, "距离未关联，退出近距操作") + + # ---- ⑧ 机体系 → 局部 NED 旋转（输出端） ---- + 旋转机体到NED(输出.vx_mps, 输出.vy_mps, 输出.vz_mps, 快照.yaw_rad) + + 输出.valid ← true + 返回 收尾并记录模式切换(输出, 输出.coast_active ? "Coast 预测跟踪" : "正常跟踪") +``` + +### 5.3 提前返回的语义差异（易错点） + +三条提前返回路径**跳过第 ⑧ 步旋转**，这是安全的：这些路径的速度分量全为零， +零向量旋转后仍是零向量。但重写时必须注意： + +| 返回路径 | 是否清 PID | 是否清限幅器 | 速度是否旋转 | `valid` | +|---------|-----------|------------|------------|---------| +| 姿态超龄降级 | 否 | 否 | 否 | `false` | +| 视觉超龄降级 | 否 | 否 | 否 | `false` | +| 判丢刹车 | 是（进入沿） | 是（进入沿） | 否 | `true` | +| 无距离退出 | 是（仅积分） | 否 | 否 | `false` | +| 正常/Coast | 视情况清积分 | 否 | **是** | `true` | + +**降级路径不清状态的理由**：降级只是"这一拍不输出"，输入恢复后应当接着上次的 +积分/限幅历史平滑续接；若每次降级都清状态，输入抖动会导致指令反复从零起爬。 + +**刹车路径必须清状态的理由**：判丢是一次真实的控制中断，恢复跟踪时的首条指令 +必须从零渐变（避免位置→速度切换突变），因此积分与全部限幅器历史归零。 + +### 5.4 模式切换日志（唯一日志点） + +```text +函数 收尾并记录模式切换(输出, 原因文本) -> 控制意图: + 若 输出.mode ≠ last_mode: + 记录 INFO 日志: "[ControlLaw] 状态切换: {旧模式名} -> {新模式名} ({原因文本})" + last_mode ← 输出.mode + 返回 输出 +``` + +同一模式连续多拍**不重复打印**。这是本模块唯一的运行期日志；每拍的三路通道 +数学计算不打任何日志。 + +--- + +## 6. 各通道算法细节 + +### 6.1 像素 → 视线角：全程针孔模型 + +```text +构造期（水平、垂直各一个实例）: + 校验 size_px > 0，否则报参数错误 + 校验 0 < fov_rad < π，否则报参数错误 + 系数 ← 2 · tan(fov_rad / 2) / size_px # 预计算，热路径不再算 tan + +每拍: + 视线角(弧度) ← atan( offset_px × 系数 ) +``` + +| 通道 | size | fov | 含义 | +|------|------|-----|------| +| 水平 | `image_width` (W) | `fov_h_deg` (θ_h) | 偏移 → β | +| 垂直 | `image_height` (H) | `fov_v_deg` (θ_v) | 偏移 → α | + +**为什么不用线性近似**：线性式 `angle ≈ offset · θ/size` 只在小偏移成立。 +若做"小偏移走线性、大偏移走针孔"的双路切换，两式在切换边界**函数值不相等** +（θ=60°、边界比例 0.2 处约 8% 角度台阶），目标跨越边界的那一拍会向控制环路 +注入一次角度突变 → 限幅器被迫吸收 → 机体抽动。`atan` 的计算代价在此频率下 +可忽略，故全程使用精确针孔式，配置项也随之少一个（原 `linear_ratio` 已移除）。 + +**未建模项（红线相关）**：镜头畸变、相机安装外参（俯仰/偏航安装角）、 +像素非正方形均未补偿。标定完成前，视线角存在系统性偏差。 + +### 6.2 水平航向通道 + +**两种输出映射，二选一，由配置决定**： + +| 模式 | 输出字段 | 量纲 | 优点 | 风险 | +|------|---------|------|------|------| +| **速率式（默认）** | `yaw_rate_dps` | (deg/s)/deg = 1/s | 指令天然是**相对量**，无坐标系歧义；不依赖自机航向精度 | 稳态需 β→0 才停转，存在小幅极限环可能 | +| 位置式 | `yaw_deg` | deg/deg（无量纲） | 稳态无累积漂移 | 输出是**绝对 NED 航向**，必须与当前航向合成；依赖新鲜姿态 | + +**限幅顺序：先变化率、后幅值**（不可颠倒）： + +```text +目标 ← 增益 × β_deg +限速 ← 变化率限幅器.推进(目标, dt) # 限制"每拍能变多少" +输出 ← 钳制(限速, ±幅值上限) # 限制"最大能到多少" +``` + +颠倒顺序会导致：幅值限幅先切掉超限部分，变化率限幅器记录的"上次输出"变成 +被切过的值，下一步以此为基准继续推进，实际变化率可超过配置上限。 + +**位置式必须合成绝对航向**：链路消息的 `yaw` 字段语义是绝对 NED 航向。 +若直接把修正量填进去，等于命令机体"转向修正角那个方向"（例如修正量 5° 会被 +理解为"转向北偏东 5°"），与当前航向无关——这是典型的坐标系语义误用。正确做法： + +```text +输出.yaw_deg ← 回绕180( 当前航向(度) + 限幅后修正量 ) + +函数 回绕180(deg): + d ← fmod(deg + 180, 360) + 若 d < 0: d ← d + 360 + 返回 d − 180 # 结果落在 [-180, 180) +``` + +### 6.3 垂直通道：统一速度支路 + +```text +目标垂直速度 ← vz_gain_mps_per_deg × α_deg +限速 ← 垂直速度限幅器.推进(目标垂直速度, dt) # 即垂直加速度限幅 +输出.vz_mps ← 钳制(限速, ±vz_limit_mps) +``` + +**小误差自然映射为小垂直速度**：α = 1° 且增益 0.1 → vz = 0.1 m/s，行为连续， +不存在"小误差走一条支路、大误差走另一条支路"的模式翻转。原设计的双支路 +（小误差发姿态微调消息）已取消，理由见 §10.1。 + +### 6.4 距离通道 + +```text +Δd ← 实测距离 − 期望射程 d_exp + Δd > 0：目标比期望远 → 前向接近（vx 为正） + Δd < 0：目标比期望近 → 减速/后退（vx 为负） + +vx ← 非对称钳制( 加速度限幅( PID(Δd) ), −后退限速, +接近限速 ) +``` + +**非对称限幅的物理动机**：接近方向允许较快（追击需要），后退方向必须保守 +（后退意味着放弃射程优势，且后方通常未观测）。默认占位值 3 m/s vs 1 m/s。 + +**PID 输出限幅取两者较大值**：PID 内部用对称限幅 `±max(接近, 后退)` 做 +anti-windup 判据，外部再做非对称钳制。这样内部限幅不会先于外部约束生效， +保证 anti-windup 判据基于"真实饱和边界"。 + +**可用性三重门**（缺一不可）：关联标志成立 **且** 距离为正 **且** 未超龄。 +"未关联的雷达距离"只是机头前方某个障碍物的距离，把它当作目标距离会导致 +朝着错误目标调整射程。 + +### 6.5 PID 内部算法 + +```text +函数 更新(error, dt) -> 输出: + 若 dt <= 0: 返回 上次输出 # 异常节拍保护 + + # ---- 积分：候选值先钳位 ---- + 积分候选 ← 积分累计 + error × dt + 若 积分限幅 > 0: 积分候选 ← 钳制(积分候选, ±积分限幅) + + # ---- 微分：一阶低通 ---- + 若 存在有效微分历史: + 微分原始 ← (error − 上拍误差) / dt + 微分 ← 滤波系数 × 上次滤波微分 + (1 − 滤波系数) × 微分原始 + 否则: + 微分 ← 0 # 首拍不做微分（避免冲击） + 上次滤波微分 ← 微分; 上拍误差 ← error; 存在有效微分历史 ← true + + # ---- 位置式合成 ---- + 原始输出 ← kp·error + ki·积分候选 + kd·微分 + + # ---- anti-windup：条件积分 ---- + 已饱和 ← (输出限幅 > 0) 且 |原始输出| > 输出限幅 + 增量推深饱和 ← (error × dt) × 原始输出 > 0 # 本拍积分增量方向与饱和同向 + 若 非 (已饱和 且 增量推深饱和): + 积分累计 ← 积分候选 # 否则冻结本拍累加 + + # ---- 输出限幅 ---- + 上次输出 ← (输出限幅 > 0) ? 钳制(原始输出, ±输出限幅) : 原始输出 + 返回 上次输出 +``` + +**条件积分判据的三种候选与取舍**（这是 anti-windup 最容易写错的地方）： + +| 判据 | 饱和时行为 | 误差反号后 | 结论 | +|------|-----------|-----------|------| +| `积分项与输出同向` | 冻结 | **仍冻结**（积分残留同号）→ 无法回卷 | ✗ 会留下饱和残留 | +| `原始输出 × error > 0` | 冻结 | 可回卷，但大 kp 项会掩盖饱和方向 → 回卷被拖延 | △ 不够干脆 | +| **`(error × dt) × 原始输出 > 0`** | 冻结 | 增量立即反向 → **首拍即回卷** | ✓ 采用 | + +**两种复位的语义差别**（必须分开实现）： + +| 方法 | 清空内容 | 保留内容 | 使用场景 | +|------|---------|---------|---------| +| 清积分 | 积分累计 | 微分历史、上次输出 | Coast 每拍、距离不可用每拍 | +| 全清 | 积分、上拍误差、滤波微分、上次输出、微分历史标志 | 参数 | 判丢进入刹车、停止视觉闭环 | + +Coast 只清积分而不清微分：预测值仍在连续演化，微分历史有效，清掉会让微分项 +在恢复跟踪瞬间失去参考、产生一次冲击。 + +### 6.6 变化率限幅器 + +```text +函数 推进(target, dt) -> 输出: + 若 dt <= 0 或 变化率上限 <= 0: 返回 当前输出 # 异常节拍/冻结保护 + 最大步长 ← 变化率上限 × dt + 当前输出 ← 钳制(target, 当前输出 − 最大步长, 当前输出 + 最大步长) + 返回 当前输出 +``` + +**两个必须知道的性质**： + +1. **不可能超调**：输出被钳制在以"上次输出"为中心、`±最大步长` 为半径的区间内， + 因此单步变化严格不超过配置上限。调试时若观察到"限幅后数值超过目标"， + 一定是别处的问题，不是限幅器超调。 +2. **初值为 0 即天然实现首条指令渐变**：构造时输出置 0，切换进入视觉跟踪后的 + 第一条指令会从 0 按加速度限幅爬升，**不需要专门写切换分支**。这也是"判丢 + 刹车时把限幅器复位到 0"的原因——复位后恢复跟踪同样从零爬升。 + +--- + +## 7. 安全语义：超龄、判丢、Coast、无距离 + +### 7.1 超龄判定（纯函数，三条判据） + +```text +函数 已超龄(取样时刻, 当前时刻, 有效期) -> 布尔: + 若 取样时刻 <= 0: 返回 true # 从未取样 + 若 当前时刻 < 取样时刻: 返回 true # 时钟源错配/打戳错误，年龄不可信 + 返回 (当前时刻 − 取样时刻) > 有效期 +``` + +**第二条判据是"宁可降级"的显式表达**：单一单调时钟下不应出现 `now < sample`。 +一旦出现，说明取样与判定用了不同时钟源（或打戳位置错误），此时年龄**不可核实**， +不能因为"看起来没超龄"就继续用它算正常指令。 + +**三个有效期独立配置**，反映各输入的更新特性： + +| 输入 | 有效期占位值 | 依据 | +|------|-------------|------| +| 视觉追踪结果 | 200 ms | 与推理帧率/追踪输出节拍匹配，约 4 拍（20 Hz） | +| 自机姿态 | 300 ms | 姿态链路更新较快，但缺失即禁止坐标运算 | +| 雷达距离 | 500 ms | 雷达更新较慢，且距离不可用有独立降级动作 | + +### 7.2 降级优先级（顺序即语义） + +```text +姿态不可用 > 视觉超龄 > 判丢 > 无距离（退出档） +``` + +| 优先级 | 检查 | 理由 | +|-------|------|------| +| 1 | 姿态过期/缺失 | 连坐标旋转与绝对航向合成都做不了，任何空间量都不可信（规格：禁止空间转换和控制） | +| 2 | 视觉超龄 | 视觉是"是否判丢"的唯一来源；数据过期时连 `tracked` 标志本身都不可信，因此**必须先于判丢**处理 | +| 3 | 判丢 | 视觉可信且明确报告无轨道 → 零速刹车 | +| 4 | 无距离 | 只有在"视觉可信且正在跟踪"前提下才谈得上距离降级 | + +### 7.3 判丢刹车与状态清理 + +```text +触发: tracked == false（且视觉未超龄、姿态可用） +动作: 输出 kBrakeHover（速度全零、航向全零）、valid = true + 进入沿清：距离 PID 全清 + 四个限幅器复位到 0 +``` + +**为什么用"进入沿"而不是每拍清**：每拍清会把限幅器输出反复钉在 0，虽然结果 +相同，但会掩盖"进入刹车"这一事件；用进入沿判定还能保证只清一次，语义清晰。 +判定依据是"上拍模式 ≠ 刹车"，因此不需要额外的布尔标志。 + +**为什么刹车要 `valid = true`**：刹车是一条**真实要发出去的指令**（零速）， +与降级"什么都不发"有本质区别。 + +### 7.4 Coast（预测跟踪） + +```text +触发: is_predicted == true（追踪库用滤波器预测值补帧） +动作: 输出照常计算（目标中心为预测值），仅清距离 PID 积分；coast_active = true +``` + +**为什么 Coast 期间仍照常算航向与垂直**：Coast 是短暂补帧（数帧量级），预测中心 +连续可用，此时停止控制反而会造成机体停顿—恢复的抖动。清积分的理由是：预测期间 +误差不反映真实世界，积分累加会把预测偏差固化成饱和量。 + +**Coast 期间距离通常不可用**：目标短暂消失往往伴随雷达关联失效，因此第 ⑦ 步会 +走无距离分支（默认"保持"→ 前向清零）。这是设计上的自然降级，不需要额外分支。 + +### 7.5 无距离三档动作 + +| 配置值 | 行为 | 适用场景 | +|--------|------|---------| +| `hold`（默认） | 前向速度清零，限幅器复位到 0 | 安全占位语义：不知道距离就不前冲 | +| `slow_approach` | 以配置的低速限幅缓速接近（仍经加速度限幅） | 需要持续压迫、且低速可接受风险 | +| `exit` | 整体降级保持（`kDegradedHold` / `kNoDistanceExit`），不发送 | 距离是安全前提（如避撞），无距离必须退出近距操作 | + +三档中只有 `exit` 会中断整拍输出；`hold` 与 `slow_approach` 仍输出航向与垂直 +通道（继续把目标保持在画面中心）。 + +--- + +## 8. 输出打包与链路 + +### 8.1 控制意图 → 消息映射 + +| 模式 | 消息 | 关键填法 | +|------|------|---------| +| `kVelocityHeading` | ID 84 `SET_POSITION_TARGET_LOCAL_NED`（速度模式） | 位置三字段填 0 并被掩码忽略；加速度三字段填 0 并被掩码忽略；速度填 NED 三分量；航向按掩码三分支二选一 | +| `kBrakeHover` | ID 84（同上，全零速度） | 速度全零，位置/加速度/航向/航向角速率字段**全部**被掩码忽略 | +| `kDegradedHold` | —— | **不打包、不发送** | + +当前实现已收敛为**只使用一种消息类型**（ID 84）。原设计中的姿态目标消息 +（ID 82）随姿态微调支路一并取消（§10.1）。 + +### 8.2 忽略掩码语义与位定义 + +掩码语义：**置位 = 忽略该字段**。基础位组合（速度模式必备）： + +```text +基础掩码 = 忽略位置 x/y/z + 忽略加速度 afx/afy/afz + = 1 + 2 + 4 + 64 + 128 + 256 = 455 = 0x01C7 +``` + +航向两位取自**本仓库随附的 MAVLink 定义**（v3，`common.xml` / `common.h`）： + +| 枚举 | 值 | 位序 | +|------|---|------| +| `YAW_IGNORE` | 1024 = `0x0400` | bit 10 | +| `YAW_RATE_IGNORE` | 2048 = `0x0800` | bit 11 | + +> ⚠️ **版本偏移风险（必须实测确认）**：这两个值之所以是 1024/2048，是因为该 +> 版本的枚举在 `AZ_IGNORE=256` 之后插入了 `FORCE_SET=512`。**较旧的 MAVLink +> 版本没有 `FORCE_SET`，其 `YAW_IGNORE=512`、`YAW_RATE_IGNORE=1024`**， +> 位序整体低一位。若飞控端（PX4 1.11.3）随附的是旧枚举，本模块发出的掩码会被 +> 解读为"使用 yaw、忽略 yaw_rate"，即把速率式指令误解为绝对航向 0（指向正北）。 +> **上线前必须在飞控端核对枚举值，或在链路侧抓包比对行为。** + +### 8.3 三个掩码与三分支选择 + +| 掩码 | 组成 | 语义 | +|------|------|------| +| 速度+航向角 | 基础 \| `0x0800` | 使用 `yaw`，忽略 `yaw_rate`（位置式） | +| 速度+角速率 | 基础 \| `0x0400` | 使用 `yaw_rate`，忽略 `yaw`（速率式，默认） | +| 刹车 | 基础 \| `0x0C00` | `yaw` 与 `yaw_rate` 均忽略 | + +**分支选择规则（含"两者皆零"的处理）**： + +```text +有角速率 ← |yaw_rate_dps| > 极小阈值 +有航向角 ← |yaw_deg| > 极小阈值 + +若 (非 有角速率) 且 有航向角: 用 速度+航向角掩码 +否则: 用 速度+角速率掩码 +``` + +**为什么"两者皆零"必须落到角速率掩码**：此时若选航向角掩码，`yaw = 0` 会被 +飞控当作**有效绝对航向 0（正北）**执行——机体无故转向正北。落到角速率掩码配 +零角速率，语义是"保持当前航向"，才是期望行为。这是速率式默认配置下的常态 +路径（β≈0 时角速率自然为 0），影响面很大。 + +### 8.4 单位换算清单（打包边界上的全部换算） + +| 量 | 控制律内部 | 链路上 | 换算 | +|----|-----------|-------|------| +| 航向角 | deg | rad | × π/180 | +| 偏航角速率 | deg/s | rad/s | × π/180 | +| 速度 | m/s | m/s | 无 | +| 时间戳 | 单调 ms | 系统启动后 ms | 直接传入（由调用方保证语义一致） | + +**内部统一用度、边界统一转弧度**的理由：控制参数（视场角、增益、限幅）在 +度单位下便于人工整定与阅读；弧度只是链路协议要求。换算点集中在打包层， +控制律内部不出现弧度制航向。 + +### 8.5 打包与传输分离 + +```text +控制律 ──控制意图──▶ 打包网关 ──字节流──▶ 传输抽象 ──▶ 物理链路 + （懂协议） （只懂字节） +``` + +分离带来的三个好处： + +1. **可回放/可录制**：把传输实现换成"写文件"即可离线录制全量指令流， + 换成"回环打印"即可台架观测，控制律零改动； +2. **链路可替换**：串口 ↔ 电台 ↔ 网络透明切换； +3. **协议知识集中**：所有 MAVLink 细节只存在于打包层一处，控制律与配置层 + 完全不感知协议。 + +**与实现语言相关的工程约束**（重写时可忽略，但当前 C++ 落地必须遵守）： +随附的 MAVLink C 库把辅助函数以外部链接直接定义在头文件中（`MAVLINK_HELPER` +默认为空宏），**多个编译单元包含同一协议头会产生重复定义链接错误**。因此 +协议头只允许被打包层那**一个**编译单元包含，对外头文件不得出现任何协议类型， +打包/发送缓冲以固定尺寸字节存储承载并在编译期断言尺寸足够。 + +### 8.6 发送节奏 + +- 控制频率由配置给定（占位 20 Hz），**每拍一条**指令； +- offboard 模式要求指令流**持续**：断流超过飞控侧超时阈值会触发 offboard + 失效保护。因此 `kDegradedHold`（不发送）会让飞控自行进入失效保护—— + 这是有意的"交还控制权"语义，但见 §12.2 的待确认项； +- 本模块不做重传、不做发送失败重试；发送失败仅返回布尔由调用方计数告警。 + +--- + +## 9. 配置参数全量表 + +配置在**启动期一次性加载**，运行期只读。缺字段、类型错误、非法取值一律 +在加载期报错并**携带完整字段路径**（如 `heading.gain`），不允许带着半套 +默认值进入运行期。 + +### 9.1 control 组：周期与超龄 + +| 键 | 单位 | 占位值 | 作用 | +|----|------|-------|------| +| `frequency_hz` | Hz | 20.0 | 控制频率；同时作为 dt 兜底值 | +| `visual_stale_ms` | ms | 200 | 视觉结果超龄阈值 | +| `radar_stale_ms` | ms | 500 | 雷达距离超龄阈值 | +| `attitude_stale_ms` | ms | 300 | 自机姿态超龄阈值 | + +### 9.2 camera 组：画面与视场 + +| 键 | 单位 | 占位值 | 作用 | +|----|------|-------|------| +| `image_width` | px | 1920 | 画面宽 W（β 计算与 x 偏移基准） | +| `image_height` | px | 1080 | 画面高 H（α 计算与 y 偏移基准） | +| `fov_h_deg` | deg | 60.0 | 水平视场角 θ_h | +| `fov_v_deg` | deg | 35.0 | 垂直视场角 θ_v | + +> 画面尺寸必须与追踪库实际使用的画面尺寸一致；若推理在缩放图上运行， +> 这里填**缩放后**的尺寸，否则视线角会系统性偏差。 + +### 9.3 heading 组：水平航向 + +| 键 | 单位 | 占位值 | 作用 | +|----|------|-------|------| +| `mode` | 枚举 | `rate` | `rate` = 速率式（默认）/ `position` = 位置式 | +| `gain` | 见 §6.2 | 1.0 | 视线角 → 航向指令增益 | +| `yaw_rate_limit_dps` | deg/s | 30.0 | 偏航角速度幅值上限 | +| `yaw_slew_limit_dps2` | deg/s² | 60.0 | 航向指令变化率上限（两种模式共用，量纲待澄清） | + +### 9.4 vertical 组：垂直 + +| 键 | 单位 | 占位值 | 作用 | +|----|------|-------|------| +| `vz_gain_mps_per_deg` | (m/s)/deg | 0.1 | 视线角 → 垂直速度增益 | +| `vz_limit_mps` | m/s | 2.0 | 垂直速度幅值上限 | + +> 原双支路所需的"切换阈值"与"姿态微调增益/限幅"三项已随支路取消而移除。 + +### 9.5 distance 组：距离 + +| 键 | 单位 | 占位值 | 作用 | +|----|------|-------|------| +| `d_exp_m` | m | 10.0 | 期望射程（由拦截设备接口给定） | +| `kp` / `ki` / `kd` | —— | 0.5 / 0.05 / 0.1 | PID 三增益 | +| `integral_limit` | m·s | 2.0 | 积分项限幅（<=0 表示不限幅） | +| `derivative_filter_coef` | ∈[0,1) | 0.2 | 微分一阶低通系数（0 = 不滤波） | +| `approach_velocity_limit_mps` | m/s | 3.0 | 接近速度上限 | +| `retreat_velocity_limit_mps` | m/s | 1.0 | 后退速度上限 | +| `no_distance_action` | 枚举 | `hold` | `hold` / `slow_approach` / `exit` | +| `no_distance_approach_limit_mps` | m/s | 0.5 | `slow_approach` 档的限速值 | + +### 9.6 accel_limit 组：加速度 + +| 键 | 单位 | 占位值 | 作用 | +|----|------|-------|------| +| `ax_mps2` | m/s² | 2.0 | 前向加速度上限（距离通道限幅器） | +| `ay_mps2` | m/s² | 2.0 | 侧向（**预留**：本控制律不做侧移，无对应限幅器） | +| `az_mps2` | m/s² | 1.5 | 垂直加速度上限（垂直通道限幅器） | + +--- + +## 10. 设计取舍记录（为什么不是别的做法） + +### 10.1 取消垂直"姿态微调"双支路，统一走速度支路 + +**原设计**：垂直误差小于切换阈值时发**姿态目标消息**做微调，大于阈值时发 +**速度设定值**。 + +**取消理由（三条，任一条都足以否决）**： + +1. **丢弃同周期的距离通道输出**：姿态消息里没有前向速度字段。一旦本拍切成 + 姿态消息，距离通道算出的 `vx` 就被整拍丢弃 → 目标在画面中心附近时逼近 + **停滞**（而"目标居中"恰恰是最常见的稳态）。 +2. **消息类型逐拍翻转扰动飞控**：目标在切换阈值附近抖动时，offboard 会收到 + 交替的两种消息类型，飞控侧设定值来源反复切换，行为难以预测。 +3. **需要额外整定一套参数**：切换阈值、微调增益、姿态角限幅、姿态角速率限幅 + 四项全部需要标定，而收益仅是"小误差时更细腻"。 + +**替代方案的效果**：小误差经增益线性映射为小垂直速度（1° → 0.1 m/s），行为 +连续、无模式翻转、全周期单一消息类型，且少 4 个待标定参数。 + +### 10.2 航向默认改为速率式 + +**理由**：速率式指令是**相对量**，不存在"这是绝对航向还是修正量"的坐标系歧义； +位置式的 `yaw` 字段是绝对 NED 航向，一旦合成环节出错（漏加当前航向、姿态过期 +用了旧航向），后果是机体转向一个完全无关的地理方向——**故障模式危险且不直观**。 +速率式最坏情况是转速不对，不会指向错误方位。 + +**代价**：稳态需 β→0 才停转，可能有小幅极限环（可通过死区或降低增益抑制）。 +位置式保留为可配置选项，但**姿态接线未验证前不得用于实飞**。 + +### 10.3 输出端补齐机体系 → NED 旋转 + +**原状态**：速度分量以机体系语义直发，而打包声明的坐标框是局部 NED—— +**声明与实际不符**。机头不朝北时，"前向 3 m/s"会被飞控理解为"北向 3 m/s"， +机体航向一变，速度方向就错。 + +**修正**：在控制律输出端（第 ⑧ 步）随快照航向旋转为 NED，使对外语义与打包 +声明一致。限幅仍在机体系完成（正交变换保范，见 §4.4）。 + +### 10.4 视线角全程针孔、取消线性双路 + +见 §6.1：双路切换边界处约 8% 角度台阶，会向控制环注入突变；`atan` 代价可忽略。 +同时消除了一个待标定参数。 + +### 10.5 PID anti-windup 采用"增量方向"条件积分 + +见 §6.5 判据对比表：另两种判据要么导致误差反号后无法回卷（积分残留）， +要么被大比例项掩盖饱和方向、拖延回卷。 + +### 10.6 限幅器初值为 0 兼作首条指令突变抑制 + +见 §6.6：不需要为"位置模式 → 速度模式切换"写专门分支，限幅器从零起步天然 +满足"首条视觉速度设定值必须经过限幅"的要求；判丢刹车时复位到 0，恢复跟踪 +同样从零爬升。 + +### 10.7 降级路径不清状态、刹车路径清状态 + +见 §5.3：降级是"这一拍不输出"，恢复后应平滑续接；刹车是真实控制中断， +恢复后必须从零渐变。两者语义不同，清理策略必须不同。 + +--- + +## 11. 实现与重写指引 + +### 11.1 必须持久化的状态（漏一个就变行为） + +| 状态 | 所属 | 初值 | 何时改变 | +|------|------|------|---------| +| 上拍时刻 | 主控律 | 0（表示首拍） | 每拍写入当前时刻 | +| 上拍输出模式 | 主控律 | 降级保持 | 仅模式切换时写入（并触发日志） | +| 积分累计 | PID | 0 | 每拍（除冻结）；清积分/全清时归零 | +| 上拍误差 | PID | 0 | 每拍 | +| 低通后微分 | PID | 0 | 每拍 | +| 上次输出 | PID | 0 | 每拍 | +| 微分历史有效标志 | PID | false | 首次更新后置 true；全清时回 false | +| 上次输出 ×4 | 四个限幅器 | 0 | 每拍；复位时归零 | + +**构造期预计算量**（不应在热路径重复计算）：针孔系数 ×2、PID 参数拷贝、 +限幅器变化率上限 ×4、配置值拷贝。 + +### 11.2 时序与线程模型 + +```text +推理侧（每帧，独立线程）: + 追踪库.更新(检测框, 画面尺寸) # 内部维护轨道与主目标 + +控制线程（固定周期，配置频率）: + 结果 ← 追踪库.取最新结果() # 线程安全拷贝 + 快照 ← 采集(结果, 雷达, 姿态) + 快照.track_time_ms ← 当前单调时钟() # 就在取结果的那一刻打戳 + 意图 ← 控制律.更新(快照, 当前单调时钟()) + 按 意图.mode 分发: + 速度航向 → 网关.发送速度航向(意图, 启动后毫秒) + 零速刹车 → 网关.发送刹车(启动后毫秒) + 降级保持 → 不发送（记录降级原因/告警） + +停止视觉闭环（状态机决定）: + 控制律.复位() # 清积分、清限幅器、清节拍与模式历史 +``` + +**约束**： + +- 控制律实例**只由控制线程调用**，构件无锁； +- 采集快照与打戳必须尽量贴近（打戳晚于采集会把采集耗时算进"新鲜度"）； +- 追踪库取结果必须是**拷贝**而非引用，避免推理线程并发改写； +- 复位与更新不得并发。 + +### 11.3 数值与精度注意事项 + +| 事项 | 处理方式 | 理由 | +|------|---------|------| +| 角度回绕 | 位置式合成后回绕到 [-180, 180) | 链路侧与人工读数都按此区间 | +| 浮点"是否为零" | 用极小阈值（如 1e-9）比较，不用 `== 0` | 掩码分支选择依赖"航向/角速率是否有效" | +| dt 异常 | `dt <= 0` → 保持上次输出；`dt > 0.5 s` → 截断 | 除零保护与限幅器保护 | +| 首拍微分 | 不做微分（无历史） | 避免首拍微分冲击 | +| 除法基准 | 画面尺寸、`dt` 均在构造期/入口校验为正 | 防除零 | +| 配置校验 | 加载期全量校验并抛错（含字段路径） | 不允许带非法参数进入运行期 | + +### 11.4 资源与日志纪律 + +- **热路径零动态分配**：所有缓冲与系数在构造期备妥（若实现语言有 GC， + 也应避免每拍新建对象造成抖动）； +- **热路径零日志**：仅模式切换打一条 INFO； +- **异常日志节流**：若实现中加入异常日志，必须按"第 1 次与每满 N 次"节流， + 20 Hz 下不节流会在数秒内刷满日志； +- **RAII / 确定性释放**：传输实现、配置源在构造期注入，生命周期由调用方保证。 + +### 11.5 可测试性设计（构件边界即测试边界） + +| 构件 | 是否纯函数 | 测试要点 | +|------|-----------|---------| +| 超龄判定 | 是 | 三条判据各一例；边界（恰好等于有效期不超龄） | +| 坐标旋转 | 是 | §4.3 两条正方向自检；旋转保范；垂直分量不变 | +| 视线角转换 | 是 | 中心偏移为 0；针孔结果与线性近似明显偏离（证明未走线性）；非法参数报错 | +| 静态限幅 | 是 | 上/下界、界内不变 | +| 变化率限幅器 | 否（有状态） | 单步不超限；**不可超调**；复位后从零爬升；`dt<=0` 保持 | +| PID | 否（有状态） | 各项独立；积分钳位；**误差反号后能回卷**（旧错误判据在此会留在同侧饱和值）；两种复位语义 | +| 主控律 | 否（有状态） | 八步流程各分支；降级优先级；模式切换日志只打一次 | +| 打包网关 | 否 | 字节头（起始字节、长度、消息 ID）；掩码三分支；单位换算；**两者皆零走角速率掩码** | + +**测试策略**：构件级用纯函数/小状态测试覆盖数学与边界；主控律级用构造快照 +驱动分支覆盖；打包级**直接解析字节**验证线格式（不依赖协议库的解码函数， +避免"用同一份错误理解自我验证"）。 + +--- + +## 12. 验证路线与待定项 + +### 12.1 分阶段验证路线 + +| 阶段 | 目标 | 方法 | 通过判据 | +|------|------|------|---------| +| 1. 构件级 | 数学正确 | 单元测试（无硬件） | 全部用例通过 | +| 2. 控制律级 | 分支与时序正确 | 构造快照驱动 + 端到端合成场景回放 | 三分支（正常/Coast/判丢）行为符合 §7 | +| 3. 链路级 | 线格式正确 | 注入"回环打印/写文件"传输实现，解析字节 | 帧头、消息 ID、掩码、单位换算符合 §8 | +| 4. SITL | 闭环行为 | 仿真飞控 + 回放/合成目标 | 目标居中收敛、无抖动、无积分饱和 | +| 5. 台架 | 实链路 | 真实串口/电台 + 飞控上电不转桨 | offboard 建立、指令被接受、无失效保护 | +| 6. 实飞 | 标定整定 | 逐项标定增益与限幅 | **仅在 1–5 全部通过后** | + +当前已完成阶段 1–3（构建零告警、单元测试全绿、端到端示例三阶段完整运行）。 +阶段 4 起需要飞控与标定数据，属后续工作。 + +### 12.2 待定项清单 + +| 项 | 现状 | 需要谁确认 | 风险 | +|----|------|-----------|------| +| **全部数值参数** | 占位值 | 标定 + 台架整定 | 高：未标定不得实飞 | +| **忽略掩码位序** | 按随附 MAVLink v3 枚举（`YAW_IGNORE=1024`、`YAW_RATE_IGNORE=2048`） | 核对飞控端 PX4 1.11.3 随附枚举 | **高**：旧枚举无 `FORCE_SET=512`，位序低一位 → 速率指令被误解为绝对航向 0（§8.2） | +| **坐标框取值** | 局部 NED | 台架实测 | 中：与旋转语义必须一致 | +| **降级时是否应主动刹车** | 降级**不发送**，依赖飞控 offboard 断流失效保护 | 与安全策略负责人确认 | 中：断流后飞控行为（悬停/返航）取决于其配置，不由本模块决定 | +| **`yaw_slew_limit_dps2` 量纲** | 两种航向模式共用一个键；速率式下含义为 deg/s²，位置式下实际含义为 deg/s | 启用位置式前拆分为两个键或重命名 | 低（默认速率式，位置式当前休眠） | +| **侧向通道** | `ay_mps2` 预留、机体侧向恒 0 | 若需要侧移则新增通道 | 低 | +| **机体补偿** | 快照的滚转/俯仰/角速度字段预留未用 | 接入主工程姿态源后评估 | 低 | +| **相机标定** | 内参/畸变/安装外参未标定，针孔模型无畸变补偿 | 标定 | 中：视线角系统性偏差 | +| **雷达关联判据** | 由感知融合层给出标志，本模块不实现 | 融合层设计 | 中：关联错误 = 距离通道朝错误目标调射程 | + +### 12.3 红线 + +1. **未标定不得实飞**：配置与代码默认值中所有数值均为占位，标定与实测整定 + 完成前，仅限 SITL / 台架 / 录制数据回放验证数学链路。 +2. **不得混用未标注坐标系的数据**：对外输出的速度分量已声明为局部 NED， + 任何改动都必须同步更新声明与打包侧坐标框。 +3. **不得用超龄数据生成正常指令**：超龄只能触发降级或告警。 +4. **不得绕过限幅**：任何新增通道都必须内联"变化率 + 幅值"双重限幅， + 包括切换后的首条指令。 +5. **未关联的雷达距离不得当作目标距离**。 + +--- + +## 附：与追踪库的行为衔接速查 + +| 追踪库输出 | 本模块响应 | 位置 | +|-----------|-----------|------| +| `tracked == true`、`is_predicted == false` | 三路正常计算 | §5.2 | +| `tracked == true`、`is_predicted == true` | 三路照常计算 + 清距离积分 + `coast_active` | §7.4 | +| `tracked == false` | 零速刹车 + 清积分与限幅器历史 | §7.3 | +| 结果超过 `visual_stale_ms` 未更新 | 降级保持（视觉超龄），**不刹车**（标志不可信） | §7.2 | +| 主目标切换（追踪库内部选举） | 本模块无感知：像素偏移自然跳变，由限幅器吸收 | §6.6 | diff --git a/docs/视觉追踪技术路线.md b/docs/视觉追踪技术路线.md new file mode 100644 index 0000000..5374bd2 --- /dev/null +++ b/docs/视觉追踪技术路线.md @@ -0,0 +1,685 @@ +# 多目标追踪过滤器（TargetTracker）技术路线 + +> 本文档将 `drone_controller/target_tracker.py` 的实现抽象为与编程语言无关的技术路线， +> 可作为 C++（或任意语言）重新实现的规格说明。所有算法细节、参数默认值均以现有代码 +> 实际实现为准，未包含代码中不存在的功能。 + +--- + +## 目录 + +1. [概述与系统定位](#1-概述与系统定位) +2. [输入输出接口定义](#2-输入输出接口定义) +3. [核心数据结构](#3-核心数据结构) +4. [卡尔曼滤波算法细节](#4-卡尔曼滤波算法细节) +5. [检测-轨迹关联流程](#5-检测-轨迹关联流程) +6. [轨迹生命周期与 ID 管理](#6-轨迹生命周期与-id-管理) +7. [主流程伪代码（每帧处理）](#7-主流程伪代码每帧处理) +8. [关键参数表](#8-关键参数表) +9. [C++ 移植要点与模块划分建议](#9-c-移植要点与模块划分建议) + +--- + +## 1. 概述与系统定位 + +### 1.1 模块职责 + +本模块是一个**轻量级多目标追踪过滤器**，位于无人机视觉闭环控制链路中 +"目标检测"与"飞控闭环"之间，承担三类职责： + +1. **平滑**：检测器（YOLO 推理）输出的边界框逐帧抖动，通过恒定速度卡尔曼滤波 + 输出平滑后的目标中心点与边界框，供下游速度闭环（PID）消费。 +2. **抗短暂丢失（Coast 预测）**：目标短暂消失（如 2~5 帧）时，不立即判丢， + 而是用滤波器预测值继续输出目标位置（`is_predicted = true`）， + 避免控制输出突变导致机体抽搐。 +3. **ID 稳定与主目标锁定**：多目标场景下通过"中心距离 + IoU 加权代价"的贪婪 + 关联维持稳定 ID；选定主目标（primary）后持续锁定，直到该轨道真正死亡， + 避免被新出现的高置信度目标抢走导致"甩头"。 + +### 1.2 在系统中的位置 + +``` + 摄像头/RTSP ──► NPU 推理(YOLO) ──► 后处理检测框 + │ + ▼ + ┌──────────────────┐ + │ TargetTracker │ ◄── 每帧调用 update() + │ (本模块) │ + └──────────────────┘ + │ 平滑中心点 / 框 / 状态标志 + ┌────────────────┼────────────────┐ + ▼ ▼ ▼ + 20Hz 控制线程 Coast 期间 tracked=false + PID → 速度指令 清空 PID 积分 触发零速刹车 +``` + +- **调用时机**：由推理主循环在每一帧检测完成后调用一次 `update()`； + 结果由独立的控制线程（约 20Hz 速度闭环）读取消费。 +- **上游行为约定**（来自调用方，供移植时对齐）： + - 输出 `is_predicted == true`（Coast 中）时，调用方会清空 PID 积分项， + 防止积分饱和（windup）； + - 输出 `tracked == false`（彻底丢失）时，调用方执行零速刹车； + - 停止视觉闭环时调用方会调用 `reset()` 清空全部追踪状态。 + +### 1.3 设计特点 + +- 恒定速度模型，8 维状态向量，纯线性卡尔曼滤波（无 UKF/EKF）。 +- 关联采用**贪婪最邻近**（按轨道置信度降序逐一匹配），不使用匈牙利算法； + 代价函数为中心距离与 (1−IoU) 的加权和。 +- 所有坐标使用**原始像素坐标**（与检测后处理输出空间一致），不做归一化。 +- 时间步长以"帧"为单位（dt = 1），与帧率无关，参数按约 30fps、640×480 画面调优。 + +--- + +## 2. 输入输出接口定义 + +### 2.1 主要接口 + +| 接口 | 说明 | +|------|------| +| `构造(max_lost_frames, max_association_dist, min_hits, dist_weight, iou_weight)` | 创建追踪器，参数见[第 8 节](#8-关键参数表) | +| `update(detections, frame_shape) → TrackResult` | 每帧调用一次，输入检测列表，输出主目标追踪结果 | +| `get_last_result() → TrackResult` | 获取上一帧结果快照（线程安全读取场景使用） | +| `reset()` | 清空所有轨道、ID 计数器、锁定状态与缓存结果 | +| `active_count() → int` | 当前存活轨道数量 | +| `active_tracks() → {track_id: Tracklet}` | 当前所有轨道的只读快照（浅复制） | + +### 2.2 输入：detections + +每帧检测列表，每个元素为 6 元组： + +``` +[x1, y1, x2, y2, conf, cls_id] +``` + +| 字段 | 类型 | 含义 | +|------|------|------| +| x1, y1 | 浮点 | 边界框左上角（像素坐标） | +| x2, y2 | 浮点 | 边界框右下角（像素坐标） | +| conf | 浮点 | 检测置信度 ∈ [0,1] | +| cls_id | 整数 | 类别编号 | + +- 允许传入空列表（表示本帧无任何检测），此时所有轨道进入 Coast 计数。 +- `frame_shape = (height, width)`，可选。仅在"无实测轨道、只有 Coast 轨道" + 时用于选取主目标的兜底排序（选最接近画面中心的轨道）；未提供时按 + 640×480 的画面中心 (320, 240) 计算。 + +### 2.3 输出：TrackResult + +`update()` 每帧返回一个结构体（代码中以字典表示），字段如下： + +| 字段 | 类型 | 含义 | +|------|------|------| +| tracked | 布尔 | 是否存在有效主目标。false 时调用方应刹车 | +| primary_id | 整数或空 | 主目标轨道 ID；无目标时为空 | +| center | (cx, cy) 或空 | 平滑后的目标中心点（像素坐标） | +| box | [x1,y1,x2,y2] 或空 | 平滑后的边界框 | +| confidence | 浮点 | 当前置信度（Coast 期间继承最近一次实测的置信度） | +| is_predicted | 布尔 | true = 当前帧处于 Coast（仅有预测、无实测） | +| lost_frames | 整数 | 主目标连续未命中帧数 | +| n_active | 整数 | 当前活跃轨道总数 | +| raw | [x1,y1,x2,y2] 或空 | 关联到的原始检测框；Coast 时为空 | + +丢失（无主目标）时的返回值：`tracked=false`，其余定位字段为空，`lost_frames=0`。 + +--- + +## 3. 核心数据结构 + +模块由三层结构组成： + +``` +TargetTracker（多目标管理器） + └── Tracklet × N（单条轨道：生命周期 + 辅助状态） + └── KalmanBoxFilter（单个 8 维恒定速度卡尔曼滤波器） +``` + +### 3.1 KalmanBoxFilter（单目标滤波器） + +| 成员 | 类型 | 含义 | +|------|------|------| +| x | 数组[8] | 状态向量 [cx, cy, w, h, vx, vy, vw, vh] | +| P | 矩阵 8×8 | 状态协方差 | +| F | 矩阵 8×8 | 状态转移矩阵（常量） | +| H | 矩阵 4×8 | 观测矩阵（常量） | +| initialized | 布尔 | 是否已用首次观测初始化 | + +### 3.2 Tracklet（轨道） + +| 成员 | 类型 | 含义 | +|------|------|------| +| track_id | 整数 | 全局唯一轨道 ID | +| kf | KalmanBoxFilter | 本轨道的滤波器 | +| lost_count | 整数 | 当前连续未匹配帧数 | +| max_lost | 整数 | 最大允许连续丢失帧数（构造时注入） | +| confidence | 浮点 | 最近一次匹配的置信度 | +| cls_id | 整数 | 最近一次匹配的类别 ID | +| age | 整数 | 轨道已存在的总帧数（创建时为 1） | +| hits | 整数 | 累计成功匹配次数（创建时为 1） | +| last_center | (cx, cy) | 最近一次中心点（实测或预测） | +| last_box | [x1,y1,x2,y2] 或空 | 最近一次边界框（实测或预测） | + +派生状态（只读属性）： + +- `is_dead := lost_count > max_lost`（可回收） +- `is_coasting := lost_count > 0`（处于纯预测状态） + +### 3.3 TargetTracker（管理器） + +| 成员 | 类型 | 含义 | +|------|------|------| +| max_lost_frames 等 5 项 | 浮点/整数 | 配置参数，见第 8 节 | +| next_id | 整数 | 下一个可分配的轨道 ID（从 0 递增，不复用） | +| tracks | 哈希表 {track_id → Tracklet} | 所有存活轨道 | +| frame_count | 整数 | 已处理帧数（仅统计用） | +| locked_primary_id | 整数或空 | 当前锁定的主目标轨道 ID | +| last_result | TrackResult | 上一帧结果缓存（供跨线程读取） | + +--- + +## 4. 卡尔曼滤波算法细节 + +### 4.1 状态空间模型 + +状态向量（8 维）与观测向量（4 维）： + +\[ +\mathbf{x} = [c_x,\ c_y,\ w,\ h,\ v_x,\ v_y,\ v_w,\ v_h]^T, +\qquad +\mathbf{z} = [c_x,\ c_y,\ w,\ h]^T +\] + +其中 \(c_x, c_y\) 为框中心像素坐标，\(w, h\) 为框宽高，后四项为对应速度 +（单位：像素/帧）。 + +**状态转移**（恒定速度模型，dt = 1 帧）： + +\[ +\mathbf{x}_k = F\,\mathbf{x}_{k-1},\quad +F = \begin{bmatrix} I_4 & I_4 \\ 0 & I_4 \end{bmatrix} +\] + +即前 4 维（位置/尺寸）各自加上对应速度：`F[i][i+4] = 1 (i=0..3)`，其余为单位阵。 + +**观测模型**（直接观测位置与尺寸，不观测速度）： + +\[ +\mathbf{z}_k = H\,\mathbf{x}_k,\quad +H = [\,I_4 \mid 0\,] \in \mathbb{R}^{4\times 8} +\] + +### 4.2 噪声参数（经验值，针对 640×480 画面调优） + +| 常量 | 值 | 用途 | +|------|----|------| +| STD_POS | 1/50 = 0.02 | 位置过程噪声比例系数 | +| STD_VEL | 1/200 = 0.005 | 速度过程噪声比例系数 | +| INI_POS | 2.0 | 初始位置协方差（对角） | +| INI_VEL | 100.0 | 初始速度协方差（对角） | + +**关键设计：噪声与目标尺寸成比例（动态 Q/R）。** +预测时过程噪声 Q 按当前状态中的目标宽高缩放： + +\[ +\sigma_{pos} = [\tfrac{w}{50},\ \tfrac{h}{50},\ \tfrac{w}{50},\ \tfrac{h}{50}], +\quad +\sigma_{vel} = [\tfrac{w}{200},\ \tfrac{h}{200},\ \tfrac{w}{200},\ \tfrac{h}{200}] +\] + +\[ +Q = \mathrm{diag}\big(\sigma_{pos}^2,\ \sigma_{vel}^2\big) +\] + +（Q 为 8×8 对角阵，前 4 个对角元来自 \(\sigma_{pos}\)，后 4 个来自 \(\sigma_{vel}\)； +每个分量先平方再上对角。） + +更新时观测噪声 R 同样按当前状态的宽高缩放，并额外乘 2 增强平滑： + +\[ +R = \mathrm{diag}\big(2\,(\tfrac{|w|}{50})^2,\ 2\,(\tfrac{|h|}{50})^2,\ 2\,(\tfrac{|w|}{50})^2,\ 2\,(\tfrac{|h|}{50})^2\big) +\] + +> 实现注意：R 中使用 `abs(w)`、`abs(h)` 防御负尺寸；Q 中直接使用状态值。 +> 该差异与代码保持一致即可，正常情形下尺寸为正，二者等价。 + +### 4.3 初始化（initiate） + +用首次观测 \([c_x, c_y, w, h]\) 初始化： + +- 状态：\(\mathbf{x} = [c_x, c_y, w, h, 0, 0, 0, 0]^T\)（速度置零） +- 协方差：\(P = \mathrm{diag}(2, 2, 2, 2, 100, 100, 100, 100)\) + （位置置信度中等，速度置信度低，允许前几帧快速收敛速度估计） + +### 4.4 预测（predict） + +未初始化时返回空。否则： + +\[ +\mathbf{x} \leftarrow F\,\mathbf{x}, \qquad +P \leftarrow F\,P\,F^T + Q +\] + +返回预测的位置/尺寸 \([c_x, c_y, w, h]\)（状态向量前 4 维的副本）。 + +### 4.5 更新（update） + +未初始化时直接返回。给定观测 \(\mathbf{z} = [c_x, c_y, w, h]\)： + +\[ +S = H\,P\,H^T + R +\] + +\[ +K = P\,H^T\,S^{-1} +\] + +\[ +\mathbf{y} = \mathbf{z} - H\,\mathbf{x} \quad \text{（残差 / innovation）} +\] + +\[ +\mathbf{x} \leftarrow \mathbf{x} + K\,\mathbf{y}, \qquad +P \leftarrow (I_8 - K\,H)\,P +\] + +> 实现注意：S 为 4×4 矩阵，代码中使用通用矩阵求逆。C++ 中可用 +> 4×4 专用求逆（如高斯消元或 LDL 分解），无需引入大型线性代数库。 +> 代码未对位置/尺寸做画面边界钳制，也未做负尺寸约束，移植时保持一致 +> （若需增强鲁棒性可另行添加，但属于行为变更）。 + +--- + +## 5. 检测-轨迹关联流程 + +### 5.1 代价函数 + +对每条轨道（预测后的中心 \((p_x, p_y)\)、预测框 \(B_t\)）与每个检测 +（中心 \((d_x, d_y)\)、检测框 \(B_d\)），定义加权代价： + +\[ +\mathrm{cost} = w_{dist}\cdot \frac{\lVert (p_x,p_y)-(d_x,d_y) \rVert_2}{D_{max}} +\;+\; w_{iou}\cdot (1 - \mathrm{IoU}(B_t, B_d)) +\] + +- \(\lVert\cdot\rVert_2\) 为中心点欧氏距离（像素）； +- \(D_{max}\) 为距离软阈值（`max_association_dist`，默认 200 px）， + 用于把距离项归一化到与 IoU 项同量级；实际使用时取 + \(\max(D_{max},\ 10^{-3})\) 防止除零； +- 默认 \(w_{dist} = w_{iou} = 0.5\)。 + +**匹配有效性判据**：仅当 `cost < 1.0` 时视为有效匹配 +（实现上：每条轨道的最优代价初始化为 1.0，只有严格小于 1.0 的候选才会被记录）。 + +### 5.2 IoU 计算 + +轴对齐框 \([x_1,y_1,x_2,y_2]\) 的标准交并比： + +\[ +\mathrm{IoU}(A,B) = \frac{\mathrm{area}(A \cap B)}{\mathrm{area}(A) + \mathrm{area}(B) - \mathrm{area}(A \cap B)} +\] + +交集宽高需与 0 取最大值；任一框为空、交集面积 ≤ 0 或并集 ≤ 0 时返回 0。 + +### 5.3 贪婪关联算法（非匈牙利） + +按**轨道置信度从高到低**排序后逐一贪心匹配（高置信轨道优先抢占检测）： + +``` +ASSOCIATE(tracks, detections): + 若 tracks 或 detections 为空： + 返回 matches=[], unmatched_tracks=全部轨道, unmatched_dets=全部检测 + + 预计算每个检测的中心与框 + + sorted_tracks ← 按 track.confidence 降序排序的轨道列表 + assigned_dets ← 空集合; assigned_tracks ← 空集合; matches ← [] + + 对 sorted_tracks 中每条轨道 t： + 若 t.id ∈ assigned_tracks：跳过 + best_cost ← 1.0; best_det ← 空 + 对每个未分配的检测 d： + dist ← 欧氏距离(t.last_center, d.center) + norm_dist ← dist / max(Dmax, 1e-3) + iou ← IoU(t.last_box, d.box) + cost ← w_dist × norm_dist + w_iou × (1 − iou) + 若 cost < best_cost： + best_cost ← cost; best_det ← d + 若 best_det 非空： + matches.append((t.id, best_det.idx)) + assigned_dets.add(best_det.idx); assigned_tracks.add(t.id) + + unmatched_tracks ← tracks 中不在 assigned_tracks 的轨道 + unmatched_dets ← detections 中不在 assigned_dets 的检测 + 返回 (matches, unmatched_tracks, unmatched_dets) +``` + +复杂度：O(M×N)（M 条轨道 × N 个检测），另有 O(M log M) 排序。 +在机载小目标数量（通常个位数）下开销可忽略。 + +> 说明：代码明确选择贪心而非匈牙利最优匹配——贪心实现简单、无第三方依赖， +> 配合"置信度优先 + 距离/IoU 加权"在无人机跟随单主目标的场景下已足够稳定。 + +--- + +## 6. 轨迹生命周期与 ID 管理 + +### 6.1 生命周期状态 + +代码中没有显式状态枚举，但由字段组合可归纳出四个阶段： + +| 阶段 | 判定条件 | 行为 | +|------|----------|------| +| 新建（未确认） | `hits < min_hits` | 正常参与关联与预测，但主目标选举时优先级低于已确认轨道 | +| 已确认 | `hits ≥ min_hits` | 主目标选举的优先候选池 | +| Coast（滑行预测） | `0 < lost_count ≤ max_lost` | 无实测，输出滤波器预测值，`is_predicted=true` | +| 死亡 | `lost_count > max_lost` | 当帧即从轨道表中删除 | + +注意两点（与代码一致）： + +1. **min_hits 不阻止轨道参与关联或对外输出**，它只影响"重新选择主目标"时 + 的候选池优先级（见 6.3）。这与 SORT 中 tentative 轨道不输出的做法不同。 +2. **死亡判定使用严格大于**：`lost_count > max_lost`，即默认配置下允许 + 连续 8 帧 Coast，第 9 帧未命中才回收。 + +### 6.2 ID 分配策略 + +- 全局计数器 `next_id` 从 0 开始，每创建一个新轨道自增 1； +- **ID 永不复用**；`reset()` 时计数器归零； +- 未匹配到任何现有轨道的检测，每个都会创建一个新轨道（无预过滤， + 不要求检测置信度下限——上游检测器已完成阈值过滤）。 + +### 6.3 主目标（primary）锁定与选举 + +对外每帧只输出**一个**主目标，供控制回路使用。选择规则： + +``` +SELECT_PRIMARY: + 若轨道表为空：清除锁定，返回 tracked=false + + ① 锁定延续：若 locked_primary_id 非空且该轨道仍存活： + 直接以该轨道输出（Coast 中照样输出，is_predicted=true） + —— 不被其他更高置信度目标抢占 + + ② 否则（无锁定 / 锁定目标已死亡被回收）→ 重新选举并锁定： + measured ← 所有 lost_count == 0 的轨道（本帧有实测） + coasting ← 其余存活轨道 + + 若 measured 非空： + confirmed ← measured 中 hits ≥ min_hits 者 + pool ← confirmed（若为空则退回 measured） + chosen ← pool 中 confidence 最大者 + 否则若 coasting 非空： + chosen ← coasting 中离画面中心最近者（欧氏距离平方排序） + is_predicted ← true + 否则： + 清除锁定，返回 tracked=false + + locked_primary_id ← chosen.id + 以 chosen 输出结果 +``` + +锁定的**唯一解除途径**：锁定轨道死亡被回收时清除 `locked_primary_id` +（在轨道删除的同一处处理），或调用 `reset()`。 + +### 6.4 生命周期状态图 + +```mermaid +graph TB + A[未匹配检测] --> B[新建轨道 hits=1 lost=0] + B -->|每帧匹配成功| C{hits >= min_hits?} + C -->|否, 选举低优先级| B + C -->|是, 已确认| D[正常跟踪 lost=0] + D -->|某帧未匹配| E[Coast lost_count+1 输出预测值] + E -->|重新匹配| D + E -->|lost_count > max_lost| F[死亡回收 删除轨道] + B -->|连续未匹配| E + F -->|若是锁定主目标| G[清除锁定 下帧重新选举] +``` + +--- + +## 7. 主流程伪代码（每帧处理） + +`update(detections, frame_shape)` 的完整单帧流程： + +``` +UPDATE(detections, frame_shape): + frame_count += 1 + + # ---- 步骤 1：所有存活轨道先行预测（推进一帧）---- + 对 tracks 中每条轨道 t： + t.predict() # 卡尔曼预测；更新 t.last_center / t.last_box；age += 1 + + # ---- 步骤 2：构造检测中心表并关联 ---- + 若 detections 非空： + det_centers ← [] + 对每个检测 i = [x1,y1,x2,y2,conf,cls]： + cx ← (x1+x2)/2; cy ← (y1+y2)/2 + w ← x2−x1; h ← y2−y1 + det_centers.append((i, cx, cy, w, h, conf, cls)) + (matches, unmatched_tracks, unmatched_dets) ← ASSOCIATE(tracks, det_centers) + 否则： + matches ← []; unmatched_tracks ← 全部轨道; unmatched_dets ← [] + + # ---- 步骤 3：用匹配到的检测更新轨道 ---- + 对每个匹配 (tid, did)： + 由 det_centers[did] 重建原始框 raw_box = [cx−w/2, cy−h/2, cx+w/2, cy+h/2] + tracks[tid].update(raw_box, conf, cls) + # 内部：框→[cx,cy,w,h] → kf.update；lost_count←0；hits+1；age+1 + # 更新 last_center / last_box / confidence / cls_id + + # ---- 步骤 4：未匹配轨道计丢失，死亡者回收 ---- + 对每个 tid ∈ unmatched_tracks： + tracks[tid].mark_miss() # lost_count+1, age+1 + 若 tracks[tid].is_dead： # lost_count > max_lost + 若 tid == locked_primary_id：locked_primary_id ← 空 + 从 tracks 中删除 tid + + # ---- 步骤 5：未匹配检测 → 创建新轨道 ---- + 对每个 did ∈ unmatched_dets： + 以 (next_id, [cx,cy,w,h], conf, cls, max_lost=max_lost_frames) 创建 Tracklet + new_track.last_box ← 由中心/宽高重建的框 # 保证创建当帧即可参与输出 + tracks[next_id] ← new_track + next_id += 1 + + # ---- 步骤 6：选取/延续主目标，组装输出 ---- + result ← SELECT_PRIMARY(frame_shape) + last_result ← result # 缓存供跨线程读取 + 返回 result +``` + +其中 `Tracklet.predict()` 与 `Tracklet.update()`： + +``` +TRACKLET_PREDICT(): + pred ← kf.predict() + 若 pred 非空： + last_center ← (pred.cx, pred.cy) + last_box ← center_size → [x1,y1,x2,y2] + age += 1 + 返回 (last_center, last_box) + +TRACKLET_UPDATE(box=[x1,y1,x2,y2], conf, cls): + cx ← (x1+x2)/2; cy ← (y1+y2)/2 + w ← x2−x1; h ← y2−y1 + kf.update([cx, cy, w, h]) + last_center ← (cx, cy); last_box ← box + confidence ← conf; cls_id ← cls + lost_count ← 0; hits += 1; age += 1 +``` + +每帧处理的时序图： + +```mermaid +graph TB + A[接收本帧检测] --> B[全部轨道卡尔曼预测] + B --> C{有检测?} + C -->|是| D[中心距离+IoU 贪婪关联] + C -->|否| E[全部轨道标记 miss] + D --> F[匹配轨道: 卡尔曼更新] + D --> G[未匹配轨道: miss 计数] + D --> H[未匹配检测: 新建轨道] + G --> I{lost > max_lost?} + I -->|是| J[回收轨道 必要时解除锁定] + I -->|否| K[主目标选择/锁定延续] + E --> I + F --> K + H --> K + J --> K + K --> L[输出 TrackResult] +``` + +--- + +## 8. 关键参数表 + +以下默认值全部取自代码实际实现。 + +### 8.1 追踪器配置参数 + +| 参数 | 默认值 | 含义与调优建议 | +|------|--------|----------------| +| max_lost_frames | 8 | 最大连续丢失帧数（约 250~300ms @ 30fps）。越大越能容忍遮挡，但 Coast 漂移时间越长；判丢后下游刹车 | +| max_association_dist | 200.0（像素） | 中心距离软阈值，用于归一化距离代价。目标运动快/帧率低时调大 | +| min_hits | 3 | 轨道"确认"所需累计匹配次数，仅影响主目标选举优先级 | +| dist_weight | 0.5 | 关联代价中归一化距离项权重 | +| iou_weight | 0.5 | 关联代价中 (1−IoU) 项权重。两项建议之和保持 1.0，使 cost<1.0 判据语义稳定 | + +> 工程部署时以上参数可由外部配置文件注入（项目中即通过配置文件的 +> `target_tracker` 段传入，未配置项回落到代码默认值）。 + +### 8.2 卡尔曼滤波内部常量 + +| 常量 | 值 | 含义 | +|------|----|------| +| dt | 1.0 | 状态转移步长，以"帧"为单位 | +| STD_POS | 1/50 | 位置过程/观测噪声比例系数（乘以目标宽高） | +| STD_VEL | 1/200 | 速度过程噪声比例系数（乘以目标宽高） | +| INI_POS | 2.0 | 初始位置协方差 | +| INI_VEL | 100.0 | 初始速度协方差 | +| R 缩放因子 | 2.0 | 观测噪声额外加倍，增强输出平滑 | +| 匹配代价上限 | 1.0 | cost ≥ 1.0 判为不可匹配 | +| Coast 兜底画面中心 | (320, 240) | frame_shape 缺省时按 640×480 计算 | + +--- + +## 9. C++ 移植要点与模块划分建议 + +### 9.1 模块 / 类划分草案 + +与代码三层结构一一对应： + +``` +target_tracker/ + ├── kalman_box_filter.h/.cpp // class KalmanBoxFilter + ├── tracklet.h/.cpp // struct/class Tracklet + └── target_tracker.h/.cpp // class TargetTracker + struct TrackResult +``` + +```cpp +// ---- 基础几何量 ---- +struct BoxXYXY { double x1, y1, x2, y2; }; +struct BoxCXCYWH{ double cx, cy, w, h; }; + +// ---- 单目标滤波器 ---- +class KalmanBoxFilter { + double x[8]; // 状态向量 + double P[8][8]; // 协方差 + static constexpr double F[8][8] = /* I4 右上 I4 */; + static constexpr double H[4][8] = /* [I4 | 0] */; + bool initialized = false; + + void initiate(const BoxCXCYWH& z); + bool predict(BoxCXCYWH& out); // 未初始化返回 false + void update(const BoxCXCYWH& z); +}; + +// ---- 轨道 ---- +struct Tracklet { + int track_id; + KalmanBoxFilter kf; + int lost_count = 0; + int max_lost; + double confidence; + int cls_id; + int age = 1; + int hits = 1; + std::pair last_center; + std::optional last_box; // 首帧预测前可能为空（创建时即赋实测框） + + bool is_dead() const { return lost_count > max_lost; } + bool is_coasting() const { return lost_count > 0; } +}; + +// ---- 输出结构 ---- +struct TrackResult { + bool tracked = false; + std::optional primary_id; + std::pair center; // tracked=false 时无效 + std::optional box; + double confidence = 0.0; + bool is_predicted = false; + int lost_frames = 0; + int n_active = 0; + std::optional raw; +}; + +// ---- 管理器 ---- +class TargetTracker { +public: + struct Config { + int max_lost_frames = 8; + double max_association_dist = 200.0; + int min_hits = 3; + double dist_weight = 0.5; + double iou_weight = 0.5; + }; + explicit TargetTracker(const Config& cfg); + TrackResult update(const std::vector& dets, + std::optional frame_shape); + TrackResult last_result() const; // 需要跨线程时加锁/原子快照 + void reset(); +}; +``` + +### 9.2 STL 容器选择 + +| 代码中的结构 | C++ 建议 | +|--------------|----------| +| 轨道哈希表 `{id: Tracklet}` | `std::unordered_map`；若需按 ID 有序遍历可用 `std::map` | +| 关联中的已分配集合 | `std::unordered_set`，或目标数量少时直接用 `std::vector` 位标记 | +| 按置信度排序的轨道视图 | 复制 ID/指针到 `std::vector` 后 `std::sort`（不直接排哈希表） | +| 检测列表 | `std::vector`，`Detection` 为 POD 结构体 | +| 结果缓存 | `TrackResult` 值类型；跨线程用互斥锁保护的拷贝，或双缓冲原子交换 | + +### 9.3 数值计算 + +- 矩阵规模极小（8×8、4×8、4×4），**无需引入 Eigen/OpenCV**： + 用固定大小二维数组或 `std::array` 手写矩阵乘法即可。 +- 唯一需要通用实现的是 4×4 矩阵求逆（S 矩阵）。可用高斯-约当消元或 + 利用 S 接近对称正定的特性做 Cholesky/LDL 分解；建议加极小量 + 正则化（对角加 1e-9）防奇异。 +- 距离计算用 `std::hypot`；IoU 为纯标量运算。 +- 全程 `double` 精度，与代码一致（代码为 float64）。 + +### 9.4 行为一致性检查清单 + +移植后建议用以下用例对齐行为： + +1. **空检测帧序列**：连续输入空检测，主目标应输出 `is_predicted=true` + 共 `max_lost_frames` 帧，第 `max_lost_frames + 1` 帧 `tracked=false` + （因死亡判定为 `lost_count > max_lost`）。 +2. **ID 单调性**：多次进出画面的目标应获得严格递增、不复用的 ID。 +3. **锁定不被抢占**：锁定目标 Coast 期间出现更高置信度新目标， + `primary_id` 不变；锁定目标死亡后切换到新目标。 +4. **min_hits 语义**：新轨道（hits 未达 min_hits）在无其他已确认实测轨道时 + 也可被选为主目标（候选池回退逻辑）。 +5. **cost 边界**：恰好 `cost = 1.0` 不构成匹配（严格小于）。 +6. **置信度优先**：两轨道竞争同一检测时，置信度高者获胜。 + +### 9.5 线程模型注意 + +原系统中追踪器在推理主线程更新，结果由 20Hz 控制线程读取。C++ 移植若 +保持该模型，仅需保护 `last_result` 的读写（互斥锁或无锁双缓冲）， +追踪器内部状态不必加锁（单线程写）。若改为多线程调用 `update()`， +则需整体加锁。 diff --git a/reference-c++/target_tracker/CMakeLists.txt b/reference-c++/target_tracker/CMakeLists.txt new file mode 100644 index 0000000..ab5c384 --- /dev/null +++ b/reference-c++/target_tracker/CMakeLists.txt @@ -0,0 +1,85 @@ +cmake_minimum_required(VERSION 3.16) + +# ============================================================================= +# target_tracker —— 视觉追踪库（独立动态库工程） +# +# 轻量级多目标追踪过滤器：恒定速度卡尔曼滤波 + 贪婪关联 + 主目标锁定。 +# 规格见 docs/视觉追踪技术路线.md；实现文档见 target_tracker.md。 +# 零第三方依赖（不引入 spdlog / Eigen / OpenCV），仅链接 C++ 标准库。 +# ============================================================================= + +project(target_tracker VERSION 1.0.0 LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +# ---- 动态库目标 ---- +add_library(target_tracker SHARED + src/kalman_box_filter.cpp + src/tracklet.cpp + src/target_tracker.cpp) +target_include_directories(target_tracker PUBLIC + $) +target_compile_features(target_tracker PUBLIC cxx_std_17) + +# 版本与符号：VERSION 1.0.0 / SOVERSION 1；默认符号可见性（不加 +# visibility=hidden 与导出宏），全部公共类直接对外导出。 +set_target_properties(target_tracker PROPERTIES + VERSION 1.0.0 + SOVERSION 1) + +# 编译告警：与主工程一致（非 MSVC：-Wall -Wextra -Wpedantic） +if(MSVC) + target_compile_options(target_tracker PRIVATE /W4 /permissive-) +else() + target_compile_options(target_tracker PRIVATE -Wall -Wextra -Wpedantic) +endif() + +# ---- 单元测试（GTest；缺失时仅提示不失败，与主工程策略一致） ---- +include(CTest) +if(BUILD_TESTING) + find_package(GTest QUIET) + if(GTest_FOUND) + include(GoogleTest) + + # 测试可执行文件直接链接共享库 target_tracker； + # BUILD_RPATH 指向库输出目录，保证 ctest 无需 LD_LIBRARY_PATH 即可运行 + set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR};$ORIGIN") + + # 卡尔曼滤波器测试：初始化状态、静止收敛、匀速速度收敛与预测超前、未初始化行为 + add_executable(kalman_box_filter_test tests/kalman_box_filter_test.cpp) + target_link_libraries(kalman_box_filter_test PRIVATE + target_tracker GTest::gtest_main) + if(MSVC) + target_compile_options(kalman_box_filter_test PRIVATE /W4 /permissive-) + else() + target_compile_options(kalman_box_filter_test PRIVATE -Wall -Wextra -Wpedantic) + endif() + gtest_discover_tests(kalman_box_filter_test) + + # 轨道与几何工具测试：IoU 边界、IsDead/IsCoasting、Predict/Update/MarkMiss 计数 + add_executable(tracklet_test tests/tracklet_test.cpp) + target_link_libraries(tracklet_test PRIVATE + target_tracker GTest::gtest_main) + if(MSVC) + target_compile_options(tracklet_test PRIVATE /W4 /permissive-) + else() + target_compile_options(tracklet_test PRIVATE -Wall -Wextra -Wpedantic) + endif() + gtest_discover_tests(tracklet_test) + + # 追踪器测试：逐条覆盖文档 §9.4 六条行为一致性验收用例 + add_executable(target_tracker_test tests/target_tracker_test.cpp) + target_link_libraries(target_tracker_test PRIVATE + target_tracker GTest::gtest_main) + if(MSVC) + target_compile_options(target_tracker_test PRIVATE /W4 /permissive-) + else() + target_compile_options(target_tracker_test PRIVATE -Wall -Wextra -Wpedantic) + endif() + gtest_discover_tests(target_tracker_test) + else() + message(STATUS "GoogleTest not found; target_tracker tests will not be built") + endif() +endif() diff --git a/reference-c++/target_tracker/include/target_tracker/kalman_box_filter.h b/reference-c++/target_tracker/include/target_tracker/kalman_box_filter.h new file mode 100644 index 0000000..d5998a9 --- /dev/null +++ b/reference-c++/target_tracker/include/target_tracker/kalman_box_filter.h @@ -0,0 +1,87 @@ +#pragma once + +// ============================================================================= +// kalman_box_filter.h —— 单目标 8 维恒定速度卡尔曼滤波器 +// +// 对应规格：docs/视觉追踪技术路线.md §4（卡尔曼滤波算法细节）、§8.2（内部常量） +// 状态向量 x[8] = [cx, cy, w, h, vx, vy, vw, vh]（§4.1） +// 观测向量 z[4] = [cx, cy, w, h]（§4.1） +// 时间步长 dt = 1 帧，与帧率无关（§1.3） +// +// 实现约束（§9.3）：矩阵规模极小（8×8 / 4×8 / 4×4），手写定长矩阵运算， +// 全程 double 精度，不引入 Eigen/OpenCV 等线性代数库。 +// ============================================================================= + +#include +#include + +namespace drone { +namespace tracker { + +/// 轴对齐边界框（中心点 + 宽高表示，卡尔曼观测空间） +struct BoxCXCYWH { + double cx = 0.0; // 框中心 x（像素） + double cy = 0.0; // 框中心 y（像素） + double w = 0.0; // 框宽（像素） + double h = 0.0; // 框高（像素） +}; + +/// 单目标恒定速度卡尔曼滤波器（§3.1 / §4） +class KalmanBoxFilter { +public: + KalmanBoxFilter() = default; + + /// 是否已用首次观测初始化（§3.1 initialized 字段） + bool Initialized() const { return initialized_; } + + /// 用首次观测 [cx, cy, w, h] 初始化（§4.3）： + /// 状态速度置零；P = diag(2,2,2,2,100,100,100,100)。 + void Initiate(const BoxCXCYWH& z); + + /// 预测推进一帧（§4.4）：未初始化返回 std::nullopt； + /// 否则 x ← F·x，P ← F·P·Fᵀ + Q，返回状态前 4 维 [cx,cy,w,h] 副本。 + std::optional Predict(); + + /// 观测更新（§4.5）：未初始化时直接返回； + /// S = H·P·Hᵀ + R，K = P·Hᵀ·S⁻¹，x ← x + K(z−Hx)，P ← (I−KH)P。 + void Update(const BoxCXCYWH& z); + + /// 状态向量只读访问（[cx, cy, w, h, vx, vy, vw, vh]，测试与排查用） + const std::array& State() const { return x_; } + + // ---- 内部常量（§4.2 / §8.2，经验值，针对 640×480 画面调优） ---- + static constexpr double kStdPos = 1.0 / 50.0; // STD_POS：位置噪声比例系数 + static constexpr double kStdVel = 1.0 / 200.0; // STD_VEL：速度噪声比例系数 + static constexpr double kIniPos = 2.0; // INI_POS：初始位置协方差 + static constexpr double kIniVel = 100.0; // INI_VEL：初始速度协方差 + static constexpr double kRScale = 2.0; // R 额外缩放因子（增强平滑，§8.2） + +private: + std::array x_{}; // 状态向量（§3.1） + std::array, 8> P_{}; // 状态协方差 8×8（§3.1） + bool initialized_ = false; // 是否已初始化（§3.1） + + // 状态转移矩阵 F（常量，§4.1）：I4 + 右上 I4，即 F[i][i]=1、F[i][i+4]=1 + static constexpr std::array, 8> kF = {{ + {{1, 0, 0, 0, 1, 0, 0, 0}}, + {{0, 1, 0, 0, 0, 1, 0, 0}}, + {{0, 0, 1, 0, 0, 0, 1, 0}}, + {{0, 0, 0, 1, 0, 0, 0, 1}}, + {{0, 0, 0, 0, 1, 0, 0, 0}}, + {{0, 0, 0, 0, 0, 1, 0, 0}}, + {{0, 0, 0, 0, 0, 0, 1, 0}}, + {{0, 0, 0, 0, 0, 0, 0, 1}}, + }}; + + // 观测矩阵 H（常量，§4.1）：[I4 | 0]（4 行 × 8 列）， + // 直接观测位置与尺寸、不观测速度 + static constexpr std::array, 4> kH = {{ + {{1, 0, 0, 0, 0, 0, 0, 0}}, + {{0, 1, 0, 0, 0, 0, 0, 0}}, + {{0, 0, 1, 0, 0, 0, 0, 0}}, + {{0, 0, 0, 1, 0, 0, 0, 0}}, + }}; +}; + +} // namespace tracker +} // namespace drone diff --git a/reference-c++/target_tracker/include/target_tracker/target_tracker.h b/reference-c++/target_tracker/include/target_tracker/target_tracker.h new file mode 100644 index 0000000..856796e --- /dev/null +++ b/reference-c++/target_tracker/include/target_tracker/target_tracker.h @@ -0,0 +1,123 @@ +#pragma once + +// ============================================================================= +// target_tracker.h —— 多目标追踪过滤器（TargetTracker）对外接口 +// +// 对应规格：docs/视觉追踪技术路线.md +// §2 输入输出接口定义（Detection / TrackResult） +// §3.3 TargetTracker（管理器） +// §5 检测-轨迹关联（贪婪最邻近，§5.3） +// §6 轨迹生命周期与 ID 管理（主目标锁定与选举，§6.3） +// §7 主流程（每帧 Update 六步） +// +// 线程模型（§9.5）：Update() 由单线程（推理主循环）调用；库内仅对 +// last_result 缓存加一把 std::mutex，LastResult() 返回受保护拷贝。 +// 本库零第三方依赖（不依赖 spdlog / Eigen / OpenCV）。 +// ============================================================================= + +#include +#include +#include +#include +#include + +#include "tracklet.h" + +namespace drone { +namespace tracker { + +/// 单个检测框（§2.2 输入 detections 元素：[x1,y1,x2,y2,conf,cls_id]） +struct Detection { + double x1 = 0.0; // 边界框左上角 x（像素坐标） + double y1 = 0.0; // 边界框左上角 y + double x2 = 0.0; // 边界框右下角 x + double y2 = 0.0; // 边界框右下角 y + double conf = 0.0; // 检测置信度 ∈ [0,1] + int cls_id = 0; // 类别编号 +}; + +/// 画面尺寸（height, width，§2.2）：仅在"无实测轨道、只有 Coast 轨道" +/// 时用于选取离画面中心最近的轨道；缺省按 640×480 中心 (320,240)。 +struct FrameShape { + int height = 0; + int width = 0; +}; + +/// 每帧追踪输出（§2.3） +struct TrackResult { + bool tracked = false; // 是否存在有效主目标；false 时调用方应刹车 + std::optional primary_id; // 主目标轨道 ID；无目标时为空 + std::pair center{0.0, 0.0}; // 平滑后的目标中心点（tracked=false 时无效） + std::optional box; // 平滑后的边界框 + double confidence = 0.0; // 当前置信度（Coast 期间继承最近实测值） + bool is_predicted = false; // true = Coast（仅预测、无实测） + int lost_frames = 0; // 主目标连续未命中帧数 + int n_active = 0; // 当前活跃轨道总数 + std::optional raw; // 关联到的原始检测框；Coast 时为空（§2.3） +}; + +/// 多目标追踪过滤器（§1.1 / §3.3） +class TargetTracker { +public: + /// 追踪器配置参数（§8.1，默认值取自规格） + struct Config { + int max_lost_frames = 8; // 最大连续丢失帧数（严格大于才判死，§6.1） + double max_association_dist = 200.0; // 中心距离软阈值 Dmax（像素，§5.1） + int min_hits = 3; // 轨道"确认"所需累计匹配次数（§6.1） + double dist_weight = 0.5; // 关联代价距离项权重（§5.1） + double iou_weight = 0.5; // 关联代价 (1−IoU) 项权重（§5.1） + }; + + /// 默认配置构造（等价于 Config{}，§8.1 默认值） + TargetTracker(); + + /// 指定配置构造 + explicit TargetTracker(const Config& cfg); + + /// 每帧调用一次（§7 六步主流程）：输入检测列表与可选画面尺寸， + /// 输出主目标追踪结果并缓存（供 LastResult 跨线程读取）。 + TrackResult Update(const std::vector& detections, + std::optional frame_shape = std::nullopt); + + /// 上一帧结果快照（线程安全：返回受互斥锁保护的拷贝，§9.5） + TrackResult LastResult() const; + + /// 清空所有轨道、ID 计数器、锁定状态与缓存结果（§2.1）；next_id 归零（§6.2） + void Reset(); + + /// 当前存活轨道数量（§2.1） + int ActiveCount() const; + + /// 当前所有轨道的只读快照（§2.1；值拷贝） + std::unordered_map ActiveTracks() const; + + /// 配置只读访问 + const Config& GetConfig() const { return config_; } + +private: + /// 贪婪关联结果（§5.3）：matches 元素为 (track_id, 检测索引) + struct AssociationResult { + std::vector> matches; + std::vector unmatched_track_ids; + std::vector unmatched_det_indices; + }; + + /// 贪婪最邻近关联（§5.3）：按轨道 confidence 降序逐一匹配， + /// cost = w_dist·dist/max(Dmax,1e-3) + w_iou·(1−IoU)，仅 cost 严格小于 + /// best_cost（初始 1.0）才匹配，即 cost ≥ 1.0 判为不可匹配（§5.1）。 + AssociationResult AssociateDetections(const std::vector& detections); + + /// 主目标选择/锁定延续（§6.3 SELECT_PRIMARY） + TrackResult SelectPrimary(std::optional frame_shape); + + Config config_; // 配置参数（§3.3） + int next_id_ = 0; // 下一个可分配 ID，从 0 递增不复用（§6.2） + std::unordered_map tracks_; // 所有存活轨道（§3.3） + long long frame_count_ = 0; // 已处理帧数（仅统计用，§3.3） + std::optional locked_primary_id_; // 当前锁定的主目标 ID（§3.3） + TrackResult last_result_; // 上一帧结果缓存（§3.3） + mutable std::mutex last_result_mutex_; // 仅保护 last_result_（§9.5） +}; + +} // namespace tracker +} // namespace drone diff --git a/reference-c++/target_tracker/include/target_tracker/tracklet.h b/reference-c++/target_tracker/include/target_tracker/tracklet.h new file mode 100644 index 0000000..2a147f8 --- /dev/null +++ b/reference-c++/target_tracker/include/target_tracker/tracklet.h @@ -0,0 +1,93 @@ +#pragma once + +// ============================================================================= +// tracklet.h —— 轨道（Tracklet）与基础几何工具 +// +// 对应规格：docs/视觉追踪技术路线.md +// §3.2 Tracklet（轨道）字段定义与派生状态 +// §5.2 IoU 计算 +// §7 主流程伪代码末尾 TRACKLET_PREDICT / TRACKLET_UPDATE +// +// 几何量说明：BoxCXCYWH（卡尔曼观测空间 [cx,cy,w,h]）定义于 +// kalman_box_filter.h，本文件通过 include 复用并同时提供 BoxXYXY +// （像素坐标 [x1,y1,x2,y2]）与两种表示的互转函数。 +// 本库只依赖 C++ 标准库，无任何第三方依赖。 +// ============================================================================= + +#include +#include + +#include "kalman_box_filter.h" + +namespace drone { +namespace tracker { + +// ---- 基础几何量（§9.1 模块/类划分草案） ---- + +/// 轴对齐边界框（像素坐标，左上/右下角表示） +struct BoxXYXY { + double x1 = 0.0; + double y1 = 0.0; + double x2 = 0.0; + double y2 = 0.0; +}; + +/// [x1,y1,x2,y2] → [cx,cy,w,h]（§7 TRACKLET_UPDATE 中的转换） +BoxCXCYWH ToCenterSize(const BoxXYXY& box); + +/// [cx,cy,w,h] → [x1,y1,x2,y2]（§7 TRACKLET_PREDICT 中的 center_size 逆变换） +BoxXYXY ToXYXY(const BoxCXCYWH& box); + +/// 标准轴对齐 IoU（§5.2）： +/// 任一框为空、交集面积 ≤ 0 或并集 ≤ 0 时返回 0。 +double ComputeIoU(const BoxXYXY& a, const BoxXYXY& b); + +// ============================================================================= +// Tracklet —— 单条轨道：生命周期计数 + 辅助状态 + 卡尔曼滤波器（§3.2） +// ============================================================================= +struct Tracklet { + /// 构造即创建轨道（§7 步骤 5 / §6.1 新建阶段）： + /// 用首次观测初始化滤波器，age=1、hits=1、lost_count=0， + /// last_center / last_box 立即由观测重建（保证创建当帧即可参与输出）。 + Tracklet(int id, const BoxCXCYWH& measurement, double conf, int cls, + int max_lost_frames); + + int track_id = 0; // 全局唯一轨道 ID（§3.2） + KalmanBoxFilter kf; // 本轨道的滤波器（§3.2） + int lost_count = 0; // 当前连续未匹配帧数（§3.2） + int max_lost = 0; // 最大允许连续丢失帧数（构造时注入，§3.2） + double confidence = 0.0; // 最近一次匹配的置信度（§3.2） + int cls_id = 0; // 最近一次匹配的类别 ID（§3.2） + int age = 1; // 轨道已存在的总帧数（创建时为 1，§3.2） + int hits = 1; // 累计成功匹配次数（创建时为 1，§3.2） + std::pair last_center{0.0, 0.0}; // 最近一次中心点（实测或预测） + std::optional last_box; // 最近一次边界框（实测或预测，§3.2） + std::optional last_raw_box; // 最近一次实测关联到的原始检测框； + // Coast 期间保留不更新，仅内部参考； + // 对外输出语义按 §2.3：Coast 时 raw 置空 + + // ---- 派生状态（§3.2 只读属性） ---- + + /// is_dead := lost_count > max_lost（严格大于，§6.1 注记 2） + bool IsDead() const { return lost_count > max_lost; } + + /// is_coasting := lost_count > 0（处于纯预测状态，§3.2） + bool IsCoasting() const { return lost_count > 0; } + + // ---- 生命周期方法（§7 末尾伪代码） ---- + + /// TRACKLET_PREDICT：卡尔曼预测推进一帧。 + /// 预测成功时更新 last_center / last_box 且 age+1。 + void Predict(); + + /// TRACKLET_UPDATE：框 [x1,y1,x2,y2] → [cx,cy,w,h] → kf.update； + /// lost_count 清零、hits+1、age+1，更新 + /// last_center / last_box / last_raw_box / confidence / cls_id。 + void Update(const BoxXYXY& box, double conf, int cls); + + /// 未匹配计丢失：lost_count+1、age+1（§7 步骤 4）。 + void MarkMiss(); +}; + +} // namespace tracker +} // namespace drone diff --git a/reference-c++/target_tracker/src/kalman_box_filter.cpp b/reference-c++/target_tracker/src/kalman_box_filter.cpp new file mode 100644 index 0000000..66e040a --- /dev/null +++ b/reference-c++/target_tracker/src/kalman_box_filter.cpp @@ -0,0 +1,235 @@ +// ============================================================================= +// kalman_box_filter.cpp —— 单目标 8 维恒定速度卡尔曼滤波器实现 +// +// 对应规格：docs/视觉追踪技术路线.md §4（算法细节）、§8.2（内部常量）、 +// §9.3（数值计算：定长数组手写矩阵乘法，4×4 高斯-约当求逆 + 1e-9 正则化）。 +// 关键公式逐行注释并标注对照文档小节号。 +// ============================================================================= + +#include "target_tracker/kalman_box_filter.h" + +#include +#include + +namespace drone { +namespace tracker { +namespace { + +// ---- 定长矩阵基础运算（§9.3：无需引入 Eigen/OpenCV） ---- + +/// 8×8 矩阵乘法：C = A·B +std::array, 8> MatMul8(const std::array, 8>& a, + const std::array, 8>& b) { + std::array, 8> c{}; + for (int i = 0; i < 8; ++i) { + for (int k = 0; k < 8; ++k) { + if (a[i][k] == 0.0) continue; // 稀疏常量矩阵加速，结果不变 + for (int j = 0; j < 8; ++j) { + c[i][j] += a[i][k] * b[k][j]; + } + } + } + return c; +} + +/// 8×8 矩阵转置：R = Aᵀ +std::array, 8> MatTranspose8( + const std::array, 8>& a) { + std::array, 8> r{}; + for (int i = 0; i < 8; ++i) { + for (int j = 0; j < 8; ++j) { + r[i][j] = a[j][i]; + } + } + return r; +} + +/// 4×4 矩阵求逆：高斯-约当消元 + 部分主元（§9.3）。 +/// 输入矩阵先对角加 1e-9 正则化防奇异（§9.3 建议）。 +/// 返回的 bool 表示是否求逆成功（主元退化时返回 false）。 +bool Invert4x4(const std::array, 4>& m, + std::array, 4>& out) { + constexpr double kRegularization = 1e-9; // 正则化极小量（§9.3） + + // 增广矩阵 [M + εI | I]，消元后左半变为单位阵、右半即为逆矩阵 + std::array, 4> aug{}; + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + aug[i][j] = m[i][j] + (i == j ? kRegularization : 0.0); + aug[i][j + 4] = (i == j) ? 1.0 : 0.0; + } + } + + for (int col = 0; col < 4; ++col) { + // 部分主元：选取当前列绝对值最大行交换，提高数值稳定性 + int pivot = col; + for (int r = col + 1; r < 4; ++r) { + if (std::abs(aug[r][col]) > std::abs(aug[pivot][col])) { + pivot = r; + } + } + if (std::abs(aug[pivot][col]) < 1e-12) { + return false; // 主元退化（正则化后理论上不可达） + } + if (pivot != col) { + std::swap(aug[pivot], aug[col]); + } + // 主元行归一化 + const double d = aug[col][col]; + for (int j = 0; j < 8; ++j) { + aug[col][j] /= d; + } + // 消去其余行（高斯-约当：一次消成对角单位阵） + for (int r = 0; r < 4; ++r) { + if (r == col) continue; + const double f = aug[r][col]; + if (f == 0.0) continue; + for (int j = 0; j < 8; ++j) { + aug[r][j] -= f * aug[col][j]; + } + } + } + + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + out[i][j] = aug[i][j + 4]; + } + } + return true; +} + +} // namespace + +void KalmanBoxFilter::Initiate(const BoxCXCYWH& z) { + // §4.3 初始化：状态 = [cx, cy, w, h, 0, 0, 0, 0]（速度置零） + x_ = {{z.cx, z.cy, z.w, z.h, 0.0, 0.0, 0.0, 0.0}}; + + // §4.3 协方差：P = diag(2,2,2,2,100,100,100,100) + // （位置置信度中等，速度置信度低，允许前几帧快速收敛速度估计） + for (auto& row : P_) row.fill(0.0); + for (int i = 0; i < 4; ++i) { + P_[i][i] = kIniPos; // INI_POS = 2.0（§4.2） + P_[i + 4][i + 4] = kIniVel; // INI_VEL = 100.0（§4.2） + } + initialized_ = true; +} + +std::optional KalmanBoxFilter::Predict() { + // §4.4：未初始化返回空 + if (!initialized_) { + return std::nullopt; + } + + // §4.2 动态过程噪声：Q = diag(σ_pos², σ_vel²)，σ 按状态中 w/h 缩放 + // σ_pos = [w/50, h/50, w/50, h/50] + // σ_vel = [w/200, h/200, w/200, h/200] + // （Q 直接使用状态值，不做 abs，与规格 §4.2 注记保持一致） + const double w = x_[2]; + const double h = x_[3]; + const double std_pos[4] = {w * kStdPos, h * kStdPos, w * kStdPos, h * kStdPos}; + const double std_vel[4] = {w * kStdVel, h * kStdVel, w * kStdVel, h * kStdVel}; + std::array, 8> q{}; + for (int i = 0; i < 4; ++i) { + q[i][i] = std_pos[i] * std_pos[i]; // 前 4 对角元来自 σ_pos（各分量先平方） + q[i + 4][i + 4] = std_vel[i] * std_vel[i]; // 后 4 对角元来自 σ_vel + } + + // §4.4 状态转移：x ← F·x（恒定速度模型：位置/尺寸各自加上对应速度） + std::array x_new{}; + for (int i = 0; i < 8; ++i) { + double sum = 0.0; + for (int j = 0; j < 8; ++j) { + sum += kF[i][j] * x_[j]; + } + x_new[i] = sum; + } + x_ = x_new; + + // §4.4 协方差传播：P ← F·P·Fᵀ + Q + P_ = MatMul8(MatMul8(kF, P_), MatTranspose8(kF)); + for (int i = 0; i < 8; ++i) { + for (int j = 0; j < 8; ++j) { + P_[i][j] += q[i][j]; + } + } + + // §4.4 返回预测的位置/尺寸（状态向量前 4 维的副本） + return BoxCXCYWH{x_[0], x_[1], x_[2], x_[3]}; +} + +void KalmanBoxFilter::Update(const BoxCXCYWH& z) { + // §4.5：未初始化直接返回 + if (!initialized_) { + return; + } + + // §4.2 动态观测噪声：R = diag(2(w/50)², 2(h/50)², 2(w/50)², 2(h/50)²) + // （R 使用 abs(w)/abs(h) 防御负尺寸，并额外乘 2 增强平滑； + // 该差异与规格 §4.2 注记保持一致） + const double w = std::abs(x_[2]); + const double h = std::abs(x_[3]); + std::array, 4> r{}; + const double rw = kRScale * (w * kStdPos) * (w * kStdPos); + const double rh = kRScale * (h * kStdPos) * (h * kStdPos); + r[0][0] = rw; + r[1][1] = rh; + r[2][2] = rw; + r[3][3] = rh; + + // §4.5 新息协方差：S = H·P·Hᵀ + R（4×4） + // H = [I4 | 0]，故 H·P·Hᵀ 即 P 的左上 4×4 子块 + std::array, 4> s{}; + for (int i = 0; i < 4; ++i) { + for (int j = 0; j < 4; ++j) { + s[i][j] = P_[i][j] + r[i][j]; + } + } + + // §4.5 / §9.3：S⁻¹（4×4 高斯-约当求逆 + 对角加 1e-9 正则化防奇异） + std::array, 4> s_inv{}; + if (!Invert4x4(s, s_inv)) { + return; // 正则化后理论不可达；防御性跳过本次更新 + } + + // §4.5 卡尔曼增益：K = P·Hᵀ·S⁻¹（8×4） + // H = [I4 | 0]，故 P·Hᵀ 即 P 的前 4 列（8×4） + std::array, 8> k{}; + for (int i = 0; i < 8; ++i) { + for (int j = 0; j < 4; ++j) { + double sum = 0.0; + for (int l = 0; l < 4; ++l) { + sum += P_[i][l] * s_inv[l][j]; + } + k[i][j] = sum; + } + } + + // §4.5 残差（innovation）：y = z − H·x（H·x 即状态前 4 维） + const double z_vec[4] = {z.cx, z.cy, z.w, z.h}; + double y[4]; + for (int i = 0; i < 4; ++i) { + y[i] = z_vec[i] - x_[i]; + } + + // §4.5 状态更新：x ← x + K·y + for (int i = 0; i < 8; ++i) { + double sum = 0.0; + for (int j = 0; j < 4; ++j) { + sum += k[i][j] * y[j]; + } + x_[i] += sum; + } + + // §4.5 协方差更新：P ← (I8 − K·H)·P + // K·H（8×8）= K 的前 4 列铺到前 4 列上（H 选通前 4 维） + std::array, 8> ikh{}; + for (int i = 0; i < 8; ++i) { + for (int j = 0; j < 8; ++j) { + ikh[i][j] = (i == j ? 1.0 : 0.0) - (j < 4 ? k[i][j] : 0.0); + } + } + P_ = MatMul8(ikh, P_); +} + +} // namespace tracker +} // namespace drone diff --git a/reference-c++/target_tracker/src/target_tracker.cpp b/reference-c++/target_tracker/src/target_tracker.cpp new file mode 100644 index 0000000..91881cd --- /dev/null +++ b/reference-c++/target_tracker/src/target_tracker.cpp @@ -0,0 +1,312 @@ +// ============================================================================= +// target_tracker.cpp —— 多目标追踪过滤器（TargetTracker）实现 +// +// 对应规格：docs/视觉追踪技术路线.md +// §5 检测-轨迹关联（贪婪最邻近，§5.3） +// §6 轨迹生命周期与 ID 管理（主目标锁定与选举，§6.3） +// §7 主流程六步（UPDATE 伪代码） +// §9.5 线程模型（单线程写 Update，仅 last_result 缓存加锁） +// ============================================================================= + +#include "target_tracker/target_tracker.h" + +#include +#include +#include + +namespace drone { +namespace tracker { + +TargetTracker::TargetTracker() : TargetTracker(Config{}) {} + +TargetTracker::TargetTracker(const Config& cfg) : config_(cfg) {} + +TrackResult TargetTracker::Update(const std::vector& detections, + std::optional frame_shape) { + // §7：frame_count += 1（仅统计用） + ++frame_count_; + + // ---- 入口防御：过滤坐标或置信度含 NaN/Inf 的检测 ---- + // 防御性增强：上游检测器理论上已过滤，但库自身必须免疫—— + // 非有限值一旦进入卡尔曼状态将永久污染轨道并传导到主目标输出 + std::vector valid_detections; + valid_detections.reserve(detections.size()); + for (const Detection& det : detections) { + if (std::isfinite(det.x1) && std::isfinite(det.y1) && + std::isfinite(det.x2) && std::isfinite(det.y2) && + std::isfinite(det.conf)) { + valid_detections.push_back(det); + } + } + + // ---- 步骤 1：所有存活轨道先行预测（推进一帧）---- + // （§7 步骤 1：t.predict() 更新 last_center / last_box、age += 1） + for (auto& kv : tracks_) { + kv.second.Predict(); + } + + // ---- 步骤 2：构造检测中心表并贪婪关联（§7 步骤 2 / §5.3） ---- + AssociationResult assoc; + if (!valid_detections.empty()) { + assoc = AssociateDetections(valid_detections); + } else { + // 无检测：全部轨道视为未匹配，全部进入 Coast 计数（§7 步骤 2 否则分支） + for (const auto& kv : tracks_) { + assoc.unmatched_track_ids.push_back(kv.first); + } + } + + // ---- 步骤 3：用匹配到的检测更新轨道（§7 步骤 3） ---- + for (const auto& match : assoc.matches) { + const int track_id = match.first; + const Detection& det = valid_detections[static_cast(match.second)]; + // §7 步骤 3：由检测重建原始框 raw_box = [cx−w/2, cy−h/2, cx+w/2, cy+h/2] + // （等价于检测框本身：cx=(x1+x2)/2、w=x2−x1 的逆运算，浮点上精确还原） + const BoxXYXY raw_box{det.x1, det.y1, det.x2, det.y2}; + tracks_.at(track_id).Update(raw_box, det.conf, det.cls_id); + } + + // ---- 步骤 4：未匹配轨道计丢失，死亡者回收（§7 步骤 4） ---- + for (const int track_id : assoc.unmatched_track_ids) { + auto it = tracks_.find(track_id); + if (it == tracks_.end()) { + continue; + } + it->second.MarkMiss(); // lost_count+1、age+1 + if (it->second.IsDead()) { // lost_count > max_lost（严格大于，§6.1） + // 锁定的唯一解除途径之一：锁定轨道死亡被回收时清锁（§6.3） + if (locked_primary_id_.has_value() && *locked_primary_id_ == track_id) { + locked_primary_id_.reset(); + } + tracks_.erase(it); + } + } + + // ---- 步骤 5：未匹配检测 → 创建新轨道（§7 步骤 5 / §6.2） ---- + for (const int det_idx : assoc.unmatched_det_indices) { + const Detection& det = valid_detections[static_cast(det_idx)]; + // §7 步骤 5：以 [cx,cy,w,h] 创建；构造函数内重建 last_box， + // 保证创建当帧即可参与输出；next_id 从 0 递增永不复用（§6.2） + tracks_.emplace(next_id_, + Tracklet(next_id_, ToCenterSize({det.x1, det.y1, det.x2, det.y2}), + det.conf, det.cls_id, config_.max_lost_frames)); + ++next_id_; + } + + // ---- 步骤 6：选取/延续主目标，组装输出并缓存（§7 步骤 6 / §6.3） ---- + TrackResult result = SelectPrimary(frame_shape); + { + std::lock_guard lock(last_result_mutex_); + last_result_ = result; // 缓存供跨线程读取（§9.5） + } + return result; +} + +TrackResult TargetTracker::LastResult() const { + // §9.5：LastResult() 返回受互斥锁保护的拷贝 + std::lock_guard lock(last_result_mutex_); + return last_result_; +} + +void TargetTracker::Reset() { + // §2.1：清空所有轨道、ID 计数器、锁定状态与缓存结果 + tracks_.clear(); + next_id_ = 0; // §6.2：reset() 时计数器归零 + frame_count_ = 0; + locked_primary_id_.reset(); // 锁定的唯一解除途径之二（§6.3） + std::lock_guard lock(last_result_mutex_); + last_result_ = TrackResult{}; +} + +int TargetTracker::ActiveCount() const { + return static_cast(tracks_.size()); +} + +std::unordered_map TargetTracker::ActiveTracks() const { + return tracks_; // 值拷贝只读快照（§2.1） +} + +TargetTracker::AssociationResult TargetTracker::AssociateDetections( + const std::vector& detections) { + AssociationResult result; + + // §5.3：按轨道 confidence 降序排序（复制 ID 到 vector 后 sort， + // 不直接排哈希表，§9.2） + std::vector sorted_ids; + sorted_ids.reserve(tracks_.size()); + for (const auto& kv : tracks_) { + sorted_ids.push_back(kv.first); + } + std::sort(sorted_ids.begin(), sorted_ids.end(), [this](int a, int b) { + const double raw_a = tracks_.at(a).confidence; + const double raw_b = tracks_.at(b).confidence; + // NaN 安全：非有限置信度按 −∞ 处理（排最后），避免 NaN 参与比较 + // 破坏 std::sort 要求的严格弱序（未定义行为）；映射后为全序， + // 等价传递性恒成立 + const double ca = std::isfinite(raw_a) + ? raw_a + : -std::numeric_limits::infinity(); + const double cb = std::isfinite(raw_b) + ? raw_b + : -std::numeric_limits::infinity(); + if (ca != cb) return ca > cb; // 置信度从高到低 + return a < b; // 平局按 ID 升序作次级键，保证确定性 + }); + + std::vector det_assigned(detections.size(), false); + + for (const int track_id : sorted_ids) { + const Tracklet& track = tracks_.at(track_id); + // §5.3：每条轨道最优代价初始化为 1.0（仅 cost 严格小于 1.0 才记录， + // 即 cost ≥ 1.0 判为不可匹配，§5.1 匹配有效性判据） + double best_cost = 1.0; + int best_det = -1; + for (size_t d = 0; d < detections.size(); ++d) { + if (det_assigned[d]) continue; // 跳过已分配检测 + const Detection& det = detections[d]; + // §5.1：中心点欧氏距离（像素） + const double det_cx = (det.x1 + det.x2) / 2.0; + const double det_cy = (det.y1 + det.y2) / 2.0; + const double dist = + std::hypot(track.last_center.first - det_cx, + track.last_center.second - det_cy); + // §5.1：归一化距离，取 max(Dmax, 1e-3) 防除零 + const double norm_dist = + dist / std::max(config_.max_association_dist, 1e-3); + // §5.2：预测框与检测框的 IoU + const double iou = track.last_box.has_value() + ? ComputeIoU(*track.last_box, + {det.x1, det.y1, det.x2, det.y2}) + : 0.0; + // §5.1：cost = w_dist·norm_dist + w_iou·(1−IoU) + const double cost = config_.dist_weight * norm_dist + + config_.iou_weight * (1.0 - iou); + if (cost < best_cost) { // 严格小于才更新（§5.1 / §5.3） + best_cost = cost; + best_det = static_cast(d); + } + } + if (best_det >= 0) { + result.matches.emplace_back(track_id, best_det); + det_assigned[static_cast(best_det)] = true; + } else { + result.unmatched_track_ids.push_back(track_id); + } + } + + // §5.3：未分配检测收集 + for (size_t d = 0; d < detections.size(); ++d) { + if (!det_assigned[d]) { + result.unmatched_det_indices.push_back(static_cast(d)); + } + } + return result; +} + +TrackResult TargetTracker::SelectPrimary(std::optional frame_shape) { + TrackResult result; + result.n_active = static_cast(tracks_.size()); + + // §6.3：轨道表为空 → 清除锁定，返回 tracked=false + if (tracks_.empty()) { + locked_primary_id_.reset(); + return result; + } + + // §6.3 ① 锁定延续：锁定轨道仍存活则直接输出（Coast 中照样输出， + // is_predicted=true；不被其他更高置信度目标抢占） + Tracklet* chosen = nullptr; + bool is_predicted = false; + if (locked_primary_id_.has_value()) { + auto it = tracks_.find(*locked_primary_id_); + if (it != tracks_.end()) { + chosen = &it->second; + is_predicted = it->second.IsCoasting(); + } + } + + // §6.3 ② 重新选举（无锁定 / 锁定目标已死亡被回收） + if (chosen == nullptr) { + // measured ← 所有 lost_count == 0 的轨道；coasting ← 其余存活轨道 + std::vector measured; + std::vector coasting; + for (auto& kv : tracks_) { + if (kv.second.IsCoasting()) { + coasting.push_back(&kv.second); + } else { + measured.push_back(&kv.second); + } + } + + if (!measured.empty()) { + // confirmed ← measured 中 hits ≥ min_hits 者；为空则退回 measured + // （min_hits 只影响候选池优先级，不阻止输出，§6.1 注记 1） + std::vector pool; + for (Tracklet* t : measured) { + if (t->hits >= config_.min_hits) { + pool.push_back(t); + } + } + if (pool.empty()) { + pool = measured; + } + // chosen ← pool 中 confidence 最大者；平局取 unordered_map 遍历序 + // 中首个最大者，不做确定性承诺（规格 §6.3 未规定平局规则） + chosen = pool.front(); + for (Tracklet* t : pool) { + if (t->confidence > chosen->confidence) { + chosen = t; + } + } + is_predicted = false; + } else if (!coasting.empty()) { + // 无实测轨道：选 coasting 中离画面中心最近者（欧氏距离平方排序） + // §2.2：frame_shape 缺省按 640×480 画面中心 (320, 240) + const double center_x = + (frame_shape.has_value() && frame_shape->width > 0) + ? frame_shape->width / 2.0 + : 320.0; + const double center_y = + (frame_shape.has_value() && frame_shape->height > 0) + ? frame_shape->height / 2.0 + : 240.0; + double best_dist_sq = std::numeric_limits::infinity(); + for (Tracklet* t : coasting) { + const double dx = t->last_center.first - center_x; + const double dy = t->last_center.second - center_y; + const double dist_sq = dx * dx + dy * dy; + if (dist_sq < best_dist_sq) { + best_dist_sq = dist_sq; + chosen = t; + } + } + if (chosen == nullptr) { + // NaN 残余的防御兜底：若所有 coasting 轨道中心均为 NaN， + // dist_sq 恒为 NaN 无人当选，取首个避免空指针解引用； + // 正常输入已在 Update 入口经有限性过滤，本分支理论上不可达 + chosen = coasting.front(); + } + is_predicted = true; // Coast 兜底：仅预测、无实测 + } else { + // 理论上不可达（轨道非空则 measured/coasting 必居其一）；防御清锁 + locked_primary_id_.reset(); + return result; + } + locked_primary_id_ = chosen->track_id; // 选举后锁定（§6.3） + } + + // ---- 组装输出（§2.3 九字段） ---- + result.tracked = true; + result.primary_id = chosen->track_id; + result.center = chosen->last_center; + result.box = chosen->last_box; + result.confidence = chosen->confidence; // Coast 期间继承最近实测置信度 + result.is_predicted = is_predicted; + result.lost_frames = chosen->lost_count; + // §2.3：raw = 关联到的原始检测框；Coast 时为空（锁定延续分支同样遵守） + result.raw = is_predicted ? std::nullopt : chosen->last_raw_box; + return result; +} + +} // namespace tracker +} // namespace drone diff --git a/reference-c++/target_tracker/src/tracklet.cpp b/reference-c++/target_tracker/src/tracklet.cpp new file mode 100644 index 0000000..1936b65 --- /dev/null +++ b/reference-c++/target_tracker/src/tracklet.cpp @@ -0,0 +1,101 @@ +// ============================================================================= +// tracklet.cpp —— 轨道（Tracklet）与基础几何工具实现 +// +// 对应规格：docs/视觉追踪技术路线.md +// §5.2 IoU 计算 +// §7 TRACKLET_PREDICT / TRACKLET_UPDATE 伪代码 +// §3.2 Tracklet 字段语义 +// ============================================================================= + +#include "target_tracker/tracklet.h" + +#include + +namespace drone { +namespace tracker { + +BoxCXCYWH ToCenterSize(const BoxXYXY& box) { + // §7 TRACKLET_UPDATE：cx=(x1+x2)/2；cy=(y1+y2)/2；w=x2−x1；h=y2−y1 + return BoxCXCYWH{(box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0, + box.x2 - box.x1, box.y2 - box.y1}; +} + +BoxXYXY ToXYXY(const BoxCXCYWH& box) { + // §7 TRACKLET_PREDICT：center_size → [cx−w/2, cy−h/2, cx+w/2, cy+h/2] + return BoxXYXY{box.cx - box.w / 2.0, box.cy - box.h / 2.0, + box.cx + box.w / 2.0, box.cy + box.h / 2.0}; +} + +double ComputeIoU(const BoxXYXY& a, const BoxXYXY& b) { + // §5.2：任一框为空（宽或高 ≤ 0）返回 0 + const double area_a = (a.x2 - a.x1) * (a.y2 - a.y1); + const double area_b = (b.x2 - b.x1) * (b.y2 - b.y1); + if (area_a <= 0.0 || area_b <= 0.0) { + return 0.0; + } + + // 交集宽高与 0 取最大值（§5.2） + const double inter_w = std::min(a.x2, b.x2) - std::max(a.x1, b.x1); + const double inter_h = std::min(a.y2, b.y2) - std::max(a.y1, b.y1); + if (inter_w <= 0.0 || inter_h <= 0.0) { + return 0.0; // 交集面积 ≤ 0 返回 0（§5.2） + } + + const double inter_area = inter_w * inter_h; + const double union_area = area_a + area_b - inter_area; + if (union_area <= 0.0) { + return 0.0; // 并集 ≤ 0 返回 0（§5.2） + } + // IoU = area(A∩B) / (area(A)+area(B)−area(A∩B))（§5.2） + return inter_area / union_area; +} + +Tracklet::Tracklet(int id, const BoxCXCYWH& measurement, double conf, int cls, + int max_lost_frames) + : track_id(id), + max_lost(max_lost_frames), + confidence(conf), + cls_id(cls) { + // §4.3：用首次观测初始化滤波器（速度置零、P 取默认对角） + kf.Initiate(measurement); + // §7 步骤 5：创建当帧即赋 last_center / last_box（保证当帧可参与输出） + last_center = {measurement.cx, measurement.cy}; + last_box = ToXYXY(measurement); + // 创建即源于实测检测：同步记录原始检测框（Coast 期间保留， + // 对外输出按 §2.3 在 Coast 时置空） + last_raw_box = last_box; + // age / hits / lost_count 由类内默认值保证（1 / 1 / 0，§3.2） +} + +void Tracklet::Predict() { + // §7 TRACKLET_PREDICT：卡尔曼预测推进一帧 + const auto pred = kf.Predict(); + if (pred.has_value()) { + // 预测成功：更新 last_center / last_box，age += 1 + last_center = {pred->cx, pred->cy}; + last_box = ToXYXY(*pred); + ++age; + } +} + +void Tracklet::Update(const BoxXYXY& box, double conf, int cls) { + // §7 TRACKLET_UPDATE：框 → [cx,cy,w,h] → kf.update + kf.Update(ToCenterSize(box)); + last_center = {(box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0}; + last_box = box; + last_raw_box = box; // 实测关联到的原始检测框（Coast 期间保留不更新） + confidence = conf; + cls_id = cls; + lost_count = 0; // 匹配成功：连续丢失计数清零 + ++hits; // 累计匹配次数 +1 + ++age; // 帧龄 +1 +} + +void Tracklet::MarkMiss() { + // §7 步骤 4：未匹配计丢失 lost_count+1、age+1 + ++lost_count; + ++age; +} + +} // namespace tracker +} // namespace drone diff --git a/reference-c++/target_tracker/target_tracker.md b/reference-c++/target_tracker/target_tracker.md new file mode 100644 index 0000000..2b87406 --- /dev/null +++ b/reference-c++/target_tracker/target_tracker.md @@ -0,0 +1,217 @@ +# target_tracker 视觉追踪库实现文档 + +> 独立动态库（`libtarget_tracker.so`），实现轻量级多目标追踪过滤器： +> 恒定速度卡尔曼滤波平滑 + Coast 抗短暂丢失 + ID 稳定与主目标锁定。 +> 算法行为的唯一权威规格为 `docs/视觉追踪技术路线.md`（下称"技术路线"）， +> 本文档不重复抄录算法全文，仅描述实现结构与对照关系。 + +--- + +## 一、功能职责与边界 + +**做什么**： + +- 每帧接收 YOLO 检测框列表，输出**一个主目标**的平滑中心点/边界框/状态标志 + （`TrackResult`），供下游速度闭环（PID）消费。 +- 平滑：8 维恒定速度卡尔曼滤波（位置/尺寸 + 速度），抑制检测框逐帧抖动。 +- Coast 预测：目标短暂消失时用滤波器预测值继续输出（`is_predicted=true`）， + 避免控制输出突变；连续丢失超过 `max_lost_frames`（严格大于）才判丢。 +- ID 稳定：贪婪最邻近关联（中心距离 + IoU 加权代价）维持轨道 ID； + 主目标锁定后不被高置信度新目标抢占，仅在轨道死亡或 `Reset()` 时解除。 + +**不做什么**（边界）： + +- 不做光流估计、不做激光/惯导等多源融合（属 perception 模块职责）； +- 不做 Topic 订阅/发布接线、不依赖 spdlog/Eigen/OpenCV，零第三方依赖； +- 不做画面边界钳制与负尺寸约束（与技术路线 §4.5 注记保持一致）； +- 不要求检测置信度下限过滤（上游检测器已完成阈值过滤）。 + +## 二、接口与数据流 + +命名空间 `drone::tracker`，头文件保护 `#pragma once`。 + +| 头文件 | 内容 | +|--------|------| +| `include/target_tracker/kalman_box_filter.h` | `BoxCXCYWH`、`KalmanBoxFilter` | +| `include/target_tracker/tracklet.h` | `BoxXYXY`、`ToCenterSize`/`ToXYXY`/`ComputeIoU`、`Tracklet` | +| `include/target_tracker/target_tracker.h` | `Detection`、`FrameShape`、`TrackResult`、`TargetTracker` | + +主要接口签名： + +```cpp +class KalmanBoxFilter { + void Initiate(const BoxCXCYWH& z); // 首次观测初始化（§4.3） + std::optional Predict(); // 未初始化返回 nullopt（§4.4） + void Update(const BoxCXCYWH& z); // 观测更新（§4.5） + bool Initialized() const; + const std::array& State() const; // [cx,cy,w,h,vx,vy,vw,vh] +}; + +struct Tracklet { + Tracklet(int id, const BoxCXCYWH& measurement, double conf, int cls, + int max_lost_frames); + bool IsDead() const; // lost_count > max_lost（严格大于） + bool IsCoasting() const; // lost_count > 0 + void Predict(); // kf.predict 非空时更新 last_center/last_box、age+1 + void Update(const BoxXYXY& box, double conf, int cls); // 匹配更新 + void MarkMiss(); // lost+1、age+1 + // 字段：track_id / kf / lost_count / max_lost / confidence / cls_id / + // age / hits / last_center / last_box / last_raw_box +}; + +class TargetTracker { + struct Config { // §8.1 默认值 + int max_lost_frames = 8; + double max_association_dist = 200.0; + int min_hits = 3; + double dist_weight = 0.5; + double iou_weight = 0.5; + }; + TargetTracker(); // 默认配置 + explicit TargetTracker(const Config& cfg); + TrackResult Update(const std::vector& detections, + std::optional frame_shape = std::nullopt); + TrackResult LastResult() const; // 线程安全拷贝 + void Reset(); // 清全部状态且 next_id 归零 + int ActiveCount() const; + std::unordered_map ActiveTracks() const; // 只读快照 +}; +``` + +数据流：调用方每帧将检测列表（像素坐标 `[x1,y1,x2,y2,conf,cls_id]`）传入 +`Update()`，得到 `TrackResult`（九字段：tracked / primary_id / center / box / +confidence / is_predicted / lost_frames / n_active / raw）。丢失时 +`tracked=false`、定位字段空、`lost_frames=0`（技术路线 §2.3）。 + +## 三、关键实现点 + +### 3.1 卡尔曼滤波（对照技术路线 §4） + +- 状态 `x[8]=[cx,cy,w,h,vx,vy,vw,vh]`，观测 `z[4]=[cx,cy,w,h]`，dt=1 帧。 +- 常量矩阵：`F`（8×8，I4 + 右上 I4）、`H`（4×8，[I4|0]）为 `constexpr`； + 矩阵乘法/转置用 `std::array` 定长手写，全程 `double`（§9.3）。 +- 常量：`kStdPos=1/50`、`kStdVel=1/200`、`kIniPos=2.0`、`kIniVel=100.0`、 + `kRScale=2.0`（§4.2/§8.2）。 +- **动态噪声**：`Predict()` 时 Q=diag(σ_pos²,σ_vel²)，σ 直接用状态中的 w/h + 缩放；`Update()` 时 R 用 `abs(w)/abs(h)` 缩放并乘 2——两处差异与规格 + §4.2 注记严格保持一致。 +- `Initiate()`：速度置零，P=diag(2,2,2,2,100,100,100,100)（§4.3）。 +- `Update()` 中 S=HPHᵀ+R 为 4×4，用**高斯-约当消元 + 部分主元**求逆， + 消元前对角加 1e-9 正则化防奇异（§9.3）；随后 K=PHᵀS⁻¹、 + x←x+K(z−Hx)、P←(I−KH)P（§4.5）。 + +### 3.2 贪婪关联（对照 §5） + +- `cost = dist_weight·dist/max(Dmax,1e-3) + iou_weight·(1−IoU)`， + 距离用 `std::hypot`；每条轨道 `best_cost` 初始化为 1.0，**仅 cost 严格 + 小于才匹配**（cost=1.0 恰好不匹配，§5.1）。 +- 按轨道 confidence 降序（平局按 ID 升序保证确定性）逐一贪心匹配； + 排序时复制 ID 到 `std::vector` 后 `std::sort`，不直接排哈希表（§9.2）。 + +### 3.3 主目标选举三级回退（对照 §6.3） + +1. **锁定延续优先**：`locked_primary_id` 存活则直接输出（Coast 中照样输出 + `is_predicted=true`），不被高置信度目标抢占； +2. **重新选举**（无锁定/锁定已死亡）：measured（lost==0）中 hits≥min_hits + 的已确认池取 confidence 最大；池空则退回全部 measured； +3. 无 measured 时从 coasting 中选离画面中心最近者（欧氏距离平方， + `frame_shape` 缺省按 640×480 中心 (320,240)），`is_predicted=true`； + 都没有则 `tracked=false`。 + +锁定仅在轨道死亡删除处或 `Reset()` 时解除。 + +### 3.4 主流程六步（对照 §7） + +`Update()` 严格按：①全体轨道 predict ②检测中心表 + 贪婪关联 ③匹配轨道 +update（raw_box 由检测重建）④未匹配轨道 mark_miss、死亡即删、若为锁定 +目标同时清锁 ⑤未匹配检测新建轨道（创建当帧赋 last_box/last_raw_box， +保证当帧可参与输出；next_id 从 0 递增永不复用）⑥主目标选举组装输出并 +缓存 last_result。 + +### 3.5 线程模型（对照 §9.5） + +单线程写 `Update()`（推理主循环）；库内仅对 `last_result` 缓存加一把 +`std::mutex`，`LastResult()` 返回受保护拷贝。内部轨道状态不加锁； +若未来改为多线程调用 `Update()` 需整体加锁。 + +### 3.6 符号导出说明 + +当前不加 `-fvisibility=hidden`、不使用导出宏，共享库默认导出全部公共符号。 +日后若启用隐藏可见性（如 `CXX_VISIBILITY_PRESET hidden`），需补 +`TARGET_TRACKER_API` 宏（导出/导入双态）并标注所有公共类与函数， +否则下游链接将缺失符号。 + +## 四、日志行为 + +本库为**纯算法层，不打任何日志**（不依赖 spdlog）。若未来接入主工程， +接线层（订阅/发布适配部件）按主工程日志纪律负责关键路径日志与节流， +本库内部不新增日志。 + +## 五、测试方式 + +构建环境：WSL2 Ubuntu（AGENTS.md 规定的开发机构建环境）。 + +```bash +cd /mnt/d/ProgramData/drone_test/drone_test +cmake -S target_tracker -B target_tracker/build +cmake --build target_tracker/build -j$(nproc) +ctest --test-dir target_tracker/build --output-on-failure +``` + +测试共 27 条（kalman_box_filter_test 5 条 / tracklet_test 12 条 / +target_tracker_test 10 条），其中 `target_tracker_test.cpp` 逐条覆盖 +技术路线 §9.4 六条验收用例： + +1. `EmptyFramesCoastThenLost`：空检测帧 Coast 持续 max_lost_frames 帧 + （is_predicted=true），第 max_lost_frames+1 帧 tracked=false； +2. `IdStrictlyIncreasingNoReuse`：ID 严格递增不复用，Reset 后归零； +3. `LockNotStolenAndSwitchAfterDeath`：Coast 期间高置信度新目标不抢占锁定， + 锁定目标死亡后切换； +4. `MinHitsFallbackAllowsNewTrack`：无已确认实测轨道时新轨道（hits max_lost`（严格大于）， + 默认允许 Coast 8 帧、第 9 帧判丢；改成 `>=` 属于行为变更。 +- **关联不上 / 乱匹配**：检查 `max_association_dist` 与目标帧间位移是否 + 匹配；cost 判据为严格小于 1.0；dist/iou 权重之和建议保持 1.0。 +- **滤波发散或跳变**：检查 Q/R 动态噪声是否按状态 w/h 缩放（R 用 abs 且 + 乘 2、Q 不用 abs，该差异是规格要求）；4×4 求逆已加 1e-9 正则化。 +- **测试链接失败（找不到 so）**：测试可执行文件通过 `CMAKE_BUILD_RPATH` + 定位库；若把测试挪到别处运行需设置 `LD_LIBRARY_PATH`。 +- 修改任何行为后必须同步更新本文档与 `docs/视觉追踪技术路线.md` 的对照注释。 + +## 七、未来接线指引（接入主工程视频链路） + +本库以独立动态库交付，主工程接入建议采用： + +1. **订阅解码帧主题作为每帧节拍**：接线部件订阅解码帧 Topic，以解码帧 + 到达为帧节拍驱动追踪（保证与推理节奏一致）； +2. **按 frame_sequence 聚合 DetectionResult**：YOLO 每目标发布一条 + DetectionResult（同帧共享 frame_sequence），接线部件按 frame_sequence + 聚合成 `std::vector` 后调用 `Update()`； +3. **主工程 `include/common/types.h` 新增 `TrackedTarget` 消息与 + `kTrackedTarget` 主题**：由接线部件把 `TrackResult` 转换为 + `TrackedTarget` 发布，供控制线程（20Hz PID）订阅消费；Coast + （is_predicted=true）时调用方清 PID 积分、tracked=false 时零速刹车 + （技术路线 §1.2 上游行为约定）。 + +主工程链接该 so 的 CMake 片段示例： + +```cmake +# 主工程 CMakeLists.txt 中引入视觉追踪库（独立子工程） +add_subdirectory(target_tracker) + +target_link_libraries(drone_control PRIVATE target_tracker) +# 部署时需将 libtarget_tracker.so.1 一并打包到可执行文件目录 +# （或设置 INSTALL_RPATH=$ORIGIN，与 docs/部署与打包.md 目录式部署约定一致） +``` diff --git a/reference-c++/target_tracker/tests/kalman_box_filter_test.cpp b/reference-c++/target_tracker/tests/kalman_box_filter_test.cpp new file mode 100644 index 0000000..95b121b --- /dev/null +++ b/reference-c++/target_tracker/tests/kalman_box_filter_test.cpp @@ -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 + +#include +#include + +#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 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 diff --git a/reference-c++/target_tracker/tests/target_tracker_test.cpp b/reference-c++/target_tracker/tests/target_tracker_test.cpp new file mode 100644 index 0000000..897f549 --- /dev/null +++ b/reference-c++/target_tracker/tests/target_tracker_test.cpp @@ -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 + +#include +#include +#include +#include + +#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::quiet_NaN(); + const double inf = std::numeric_limits::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 diff --git a/reference-c++/target_tracker/tests/tracklet_test.cpp b/reference-c++/target_tracker/tests/tracklet_test.cpp new file mode 100644 index 0000000..665bea6 --- /dev/null +++ b/reference-c++/target_tracker/tests/tracklet_test.cpp @@ -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 + +#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 diff --git a/reference-c++/visual_track_control/CMakeLists.txt b/reference-c++/visual_track_control/CMakeLists.txt new file mode 100644 index 0000000..2c2d9a2 --- /dev/null +++ b/reference-c++/visual_track_control/CMakeLists.txt @@ -0,0 +1,122 @@ +cmake_minimum_required(VERSION 3.16) + +# ============================================================================= +# visual_track_control —— 视觉跟踪控制律静态库（独立子工程） +# +# 实现 docs/物理追踪思路.md 的水平航向控制（§4）、垂直/高度双支路控制（§5）、 +# 距离 PID（§6）、全量限幅（§8）、Coast 清积分与判丢刹车（§6.4）、 +# 超龄降级（§7.1、§9.2）。 +# +# 构建约束： +# - 与主工程完全解耦：根 CMakeLists.txt 零改动，可独立构建； +# - 仅依赖 third_party：nlohmann（单头文件）、spdlog（源码）、 +# c_library_v2-master（MAVLink 纯头文件）； +# - target_tracker 为 SHARED 库且零源码修改，原样 add_subdirectory 复用， +# TrackResult 出现在本库公共头（vtc_types.h），故以 PUBLIC 链接； +# - MAVLink include 目录设为 SYSTEM PRIVATE：压告警噪音且不泄漏给下游， +# 全工程仅 src/mavlink_command_gateway.cpp 一个 TU 包含 mavlink.h +# （MAVLINK_HELPER 默认空宏，多 TU 包含会多重定义）。 +# ============================================================================= + +project(visual_track_control LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +# target_tracker 是 SHARED 库：BUILD_RPATH 指向构建目录，示例无需 +# LD_LIBRARY_PATH 即可直接运行（部署时需随带 libtarget_tracker.so.1）。 +set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR};${CMAKE_BINARY_DIR}/target_tracker;$ORIGIN") + +# ---- 依赖：target_tracker（零改动复用；其测试块由 -DBUILD_TESTING=OFF 压制） ---- +add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../target_tracker + ${CMAKE_BINARY_DIR}/target_tracker) + +# ---- 依赖：spdlog（守卫引入，避免挂载进根构建时重复定义；关闭示例/测试/动态库/安装） ---- +if(NOT TARGET spdlog::spdlog) + set(SPDLOG_BUILD_EXAMPLE OFF CACHE BOOL "" FORCE) + set(SPDLOG_BUILD_EXAMPLE_HO OFF CACHE BOOL "" FORCE) + set(SPDLOG_BUILD_TESTS OFF CACHE BOOL "" FORCE) + set(SPDLOG_BUILD_TESTS_HO OFF CACHE BOOL "" FORCE) + set(SPDLOG_BUILD_SHARED OFF CACHE BOOL "" FORCE) + set(SPDLOG_INSTALL OFF CACHE BOOL "" FORCE) + add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../third_party/spdlog + ${CMAKE_BINARY_DIR}/spdlog) +endif() + +# ---- 静态库本体 ---- +add_library(visual_track_control STATIC + src/vtc_config.cpp + src/pid_controller.cpp + src/line_of_sight.cpp + src/tracking_control_law.cpp + src/mavlink_command_gateway.cpp) + +target_include_directories(visual_track_control PUBLIC + $) + +# nlohmann JSON：单头文件，仅配置加载器（启动期）使用，不进入热路径 +target_include_directories(visual_track_control PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../third_party/nlohmann) + +# MAVLink 纯头文件库：SYSTEM 压告警、PRIVATE 不泄漏（公共头无任何 mavlink_* 类型） +target_include_directories(visual_track_control SYSTEM PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/../third_party/c_library_v2-master) + +target_link_libraries(visual_track_control + PUBLIC target_tracker + PRIVATE spdlog::spdlog) +target_compile_features(visual_track_control PUBLIC cxx_std_17) + +# 编译告警：与主工程一致 +if(MSVC) + target_compile_options(visual_track_control PRIVATE /W4 /permissive-) +else() + target_compile_options(visual_track_control PRIVATE -Wall -Wextra -Wpedantic) +endif() + +# ---- 示例可执行文件（合成检测 → 追踪 → 控制律 → MAVLink 字节流打印） ---- +add_executable(track_control_demo examples/track_control_demo.cpp) +target_link_libraries(track_control_demo PRIVATE visual_track_control spdlog::spdlog) +target_compile_definitions(track_control_demo PRIVATE + VTC_DEMO_DEFAULT_CONFIG="${CMAKE_CURRENT_SOURCE_DIR}/config/track_control.json") +if(MSVC) + target_compile_options(track_control_demo PRIVATE /W4 /permissive-) +else() + target_compile_options(track_control_demo PRIVATE -Wall -Wextra -Wpedantic) +endif() + +# ---- 单元测试（Google Test） ---- +# 独立选项：构建本子工程时需 -DBUILD_TESTING=OFF 压制 target_tracker 的测试， +# 故本库测试开关不复用 BUILD_TESTING，二者互不干扰。 +# 开发机未安装 GTest 时自动跳过（不影响库与示例构建）。 +option(VTC_BUILD_TESTS "构建 visual_track_control 单元测试" ON) +if(VTC_BUILD_TESTS) + find_package(GTest QUIET) + if(GTest_FOUND) + enable_testing() + include(GoogleTest) + add_executable(vtc_unit_tests + tests/rate_limiter_test.cpp + tests/staleness_checker_test.cpp + tests/frame_transform_test.cpp + tests/line_of_sight_test.cpp + tests/pid_controller_test.cpp + tests/tracking_control_law_test.cpp + tests/vtc_config_test.cpp + tests/mavlink_command_gateway_test.cpp) + target_link_libraries(vtc_unit_tests PRIVATE + visual_track_control GTest::gtest GTest::gtest_main) + target_compile_definitions(vtc_unit_tests PRIVATE + VTC_TEST_CONFIG="${CMAKE_CURRENT_SOURCE_DIR}/config/track_control.json" + VTC_TEST_TMP_DIR="${CMAKE_CURRENT_BINARY_DIR}") + if(MSVC) + target_compile_options(vtc_unit_tests PRIVATE /W4 /permissive-) + else() + target_compile_options(vtc_unit_tests PRIVATE -Wall -Wextra -Wpedantic) + endif() + gtest_discover_tests(vtc_unit_tests) + else() + message(STATUS "未找到 GTest：跳过 vtc_unit_tests 单元测试") + endif() +endif() diff --git a/reference-c++/visual_track_control/config/track_control.json b/reference-c++/visual_track_control/config/track_control.json new file mode 100644 index 0000000..7dcd26b --- /dev/null +++ b/reference-c++/visual_track_control/config/track_control.json @@ -0,0 +1,48 @@ +{ + "_comment": "视觉跟踪控制律配置。全部数值为占位示例（对应 docs/物理追踪思路.md 第 8 章待定项），键名后缀标注单位；标定完成前仅限回放验证，不得用于实飞。", + + "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 + } +} diff --git a/reference-c++/visual_track_control/examples/track_control_demo.cpp b/reference-c++/visual_track_control/examples/track_control_demo.cpp new file mode 100644 index 0000000..3e1a4b6 --- /dev/null +++ b/reference-c++/visual_track_control/examples/track_control_demo.cpp @@ -0,0 +1,225 @@ +// ============================================================================= +// track_control_demo.cpp —— 视觉跟踪控制律演示程序 +// +// 数据流（模拟单线程整合的完整链路）： +// 合成运动检测框 → TargetTracker::Update()（模拟推理线程每帧调用） +// → LastResult() + 打戳 → ControlSnapshot → TrackingControlLaw::Update() +// → MavlinkCommandGateway → LoggingTransport（字节数 + 十六进制摘要） +// +// 依次演示三种分支（对应物理追踪思路 §6.4）： +// 1. 正常跟踪：持续喂入合成检测框，输出速度航向意图（垂直/距离通道 +// 统一为速度分量）； +// 2. Coast：停止喂检测 → is_predicted==true → 清积分，继续预测跟踪； +// 3. 判丢刹车：连续丢帧超过追踪库阈值 → tracked==false → 零速刹车。 +// +// 运行方式： +// track_control_demo [配置文件路径] # 缺省使用编译期内置路径 +// ============================================================================= + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "visual_track_control/i_command_transport.h" +#include "visual_track_control/mavlink_command_gateway.h" +#include "visual_track_control/tracking_control_law.h" +#include "visual_track_control/vtc_config.h" +#include "visual_track_control/vtc_types.h" + +#ifndef VTC_DEMO_DEFAULT_CONFIG +#define VTC_DEMO_DEFAULT_CONFIG "config/track_control.json" +#endif + +namespace { + +// ---- 演示时长（秒）：总时长有限，自动退出 ---- +constexpr int kNormalPhaseSeconds = 6; // 阶段1：正常跟踪 +constexpr int kCoastPhaseSeconds = 4; // 阶段2：Coast（停止喂检测） +constexpr int kLostPhaseSeconds = 3; // 阶段3：判丢刹车 + +// 追踪库判丢阈值放大：默认 8 帧在 20Hz 下 Coast 仅 0.4 秒，不便于观察； +// 演示专用放宽到 60 帧（约 3 秒），正式参数以追踪库规格为准。 +constexpr int kDemoMaxLostFrames = 60; + +const char* ModeName(drone::vtc::ControlMode mode) { + switch (mode) { + case drone::vtc::ControlMode::kVelocityHeading: return "速度航向"; + case drone::vtc::ControlMode::kBrakeHover: return "零速刹车"; + case drone::vtc::ControlMode::kDegradedHold: return "降级保持"; + } + return "未知"; +} + +/// 演示用传输实现：统计字节数并打印十六进制摘要（按秒级节流，防止刷屏） +class LoggingTransport : public drone::vtc::ICommandTransport { +public: + bool Send(const std::uint8_t* data, std::size_t len) override { + ++msg_count_; + total_bytes_ += len; + + const auto now_ms = + std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); + if (now_ms - last_print_ms_ >= 1000) { + // 十六进制摘要：取包头前 12 字节 + std::string hex; + char buf[4]; + const std::size_t n = len < 12 ? len : 12; + for (std::size_t i = 0; i < n; ++i) { + std::snprintf(buf, sizeof(buf), "%02X ", data[i]); + hex += buf; + } + spdlog::info("[Transport] 累计 {} 帧 / {} 字节; 本帧 {} 字节, 包头: {}", + msg_count_, total_bytes_, len, hex); + last_print_ms_ = now_ms; + } + return true; + } + +private: + std::int64_t last_print_ms_ = 0; + std::uint64_t msg_count_ = 0; + std::uint64_t total_bytes_ = 0; +}; + +/// 合成运动检测框：从画面右下向中心缓慢漂移（模拟目标被持续锁定） +std::vector SynthesizeDetections(double elapsed_s, + int width, int height) { + const double center_x = width * 0.5 + 160.0 - 20.0 * elapsed_s; + const double center_y = height * 0.5 + 80.0 - 10.0 * elapsed_s; + const double half_w = 70.0; + const double half_h = 50.0; + + drone::tracker::Detection det; + det.x1 = center_x - half_w; + det.y1 = center_y - half_h; + det.x2 = center_x + half_w; + det.y2 = center_y + half_h; + det.conf = 0.9; + det.cls_id = 0; + return {det}; +} + +} // namespace + +int main(int argc, char** argv) { + // ---- 启动期：一次性加载配置（缺字段/非法值直接抛异常终止） ---- + const std::string config_path = argc > 1 ? argv[1] : VTC_DEMO_DEFAULT_CONFIG; + drone::vtc::TrackingConfig cfg; + try { + cfg = drone::vtc::TrackingConfig::LoadFromJson(config_path); + } catch (const std::exception& e) { + spdlog::error("配置加载失败: {}", e.what()); + return 1; + } + spdlog::info("配置加载成功: {} (控制频率 {} Hz)", config_path, + cfg.control.frequency_hz); + + // ---- 构造：追踪库 / 控制律 / 传输 / 网关 ---- + drone::tracker::TargetTracker::Config tracker_cfg; + tracker_cfg.max_lost_frames = kDemoMaxLostFrames; // 演示专用判丢阈值 + drone::tracker::TargetTracker tracker(tracker_cfg); + + drone::vtc::TrackingControlLaw control_law(cfg); + LoggingTransport transport; + drone::vtc::MavlinkCommandGateway gateway(transport, /*sys=*/1, /*comp=*/1, + /*target_sys=*/1, /*target_comp=*/1); + + const int width = cfg.camera.image_width; + const int height = cfg.camera.image_height; + const int period_ms = static_cast(1000.0 / cfg.control.frequency_hz); + const int normal_ticks = kNormalPhaseSeconds * static_cast(cfg.control.frequency_hz); + const int coast_ticks = kCoastPhaseSeconds * static_cast(cfg.control.frequency_hz); + const int lost_ticks = kLostPhaseSeconds * static_cast(cfg.control.frequency_hz); + const int total_ticks = normal_ticks + coast_ticks + lost_ticks; + + spdlog::info("演示开始: 正常跟踪 {}s → Coast {}s → 判丢刹车 {}s (共 {} 拍, {} ms/拍)", + kNormalPhaseSeconds, kCoastPhaseSeconds, kLostPhaseSeconds, + total_ticks, period_ms); + + drone::tracker::FrameShape frame_shape; + frame_shape.height = height; + frame_shape.width = width; + + const char* current_phase = nullptr; + + // ---- 主循环：按配置频率固定节拍（§7.1） ---- + for (int tick = 0; tick < total_ticks; ++tick) { + const double elapsed_s = static_cast(tick) / cfg.control.frequency_hz; + // 模拟单调时钟：1 秒偏移保证取样时间戳有效 + const std::int64_t now_ms = 1000 + static_cast(tick) * period_ms; + + // ---- 阶段划分 ---- + const char* phase = tick < normal_ticks ? "正常跟踪" + : tick < normal_ticks + coast_ticks ? "Coast" + : "判丢刹车"; + if (phase != current_phase) { + spdlog::info("===== 阶段切换: {} =====", phase); + current_phase = phase; + } + + // ---- 模拟推理线程：仅正常跟踪阶段喂入检测框 ---- + // 阶段2/3 停止喂检测：先 Coast（is_predicted），后判丢（tracked==false） + std::vector detections; + if (tick < normal_ticks) { + detections = SynthesizeDetections(elapsed_s, width, height); + } + tracker.Update(detections, frame_shape); + + // ---- 控制线程：LastResult() + 打戳（TrackResult 无时间戳，§7.2） ---- + drone::vtc::ControlSnapshot snapshot; + snapshot.track = tracker.LastResult(); + snapshot.track_time_ms = now_ms; + + // 雷达：正常阶段关联成立；Coast/刹车阶段取消关联（演示 §6.3 降级路径） + snapshot.radar_distance_m = 40.0 - 3.0 * elapsed_s; + snapshot.associated_to_target = (tick < normal_ticks) && + snapshot.radar_distance_m > 0.0; + snapshot.radar_time_ms = now_ms; + + // 自机姿态：演示中始终新鲜有效（接入主工程时来自 FlightStateSnapshot） + snapshot.attitude_present = true; + snapshot.attitude_time_ms = now_ms; + + // ---- 控制律单周期计算 ---- + const drone::vtc::ControlOutput output = control_law.Update(snapshot, now_ms); + + // ---- 按模式经网关发送 ---- + const auto time_boot_ms = static_cast(now_ms); + switch (output.mode) { + case drone::vtc::ControlMode::kVelocityHeading: + gateway.SendVelocityHeading(output, time_boot_ms); + break; + case drone::vtc::ControlMode::kBrakeHover: + gateway.SendBrakeHover(time_boot_ms); + break; + case drone::vtc::ControlMode::kDegradedHold: + break; // 降级保持：不发送指令（仅告警，§7.1） + } + + // ---- 半秒一次的摘要打印（演示观测用，非生产热路径） ---- + if (tick % 10 == 0) { + spdlog::info( + "[t={:5.1f}s] 阶段={} 模式={}{} tracked={} vx={:+.2f} vy={:+.2f} " + "vz={:+.2f} yaw_rate={:+.1f}dps d={:5.1f}m 关联={}", + elapsed_s, phase, ModeName(output.mode), + output.coast_active ? "(Coast)" : "", snapshot.track.tracked, + output.vx_mps, output.vy_mps, output.vz_mps, output.yaw_rate_dps, + snapshot.radar_distance_m, snapshot.associated_to_target); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(period_ms)); + } + + control_law.Reset(); // 停止视觉闭环：清全部状态（§6.4） + spdlog::info("演示结束"); + return 0; +} diff --git a/reference-c++/visual_track_control/include/visual_track_control/frame_transform.h b/reference-c++/visual_track_control/include/visual_track_control/frame_transform.h new file mode 100644 index 0000000..1318f64 --- /dev/null +++ b/reference-c++/visual_track_control/include/visual_track_control/frame_transform.h @@ -0,0 +1,44 @@ +#pragma once + +// ============================================================================= +// frame_transform.h —— 机体系（前/右/下）→ 局部 NED（北/东/地）坐标旋转 +// +// 对应规格：docs/物理追踪思路.md +// §2.3 控制律内部计算统一在机体坐标系；输出的速度意图若采用局部系， +// 必须随姿态完成旋转并显式标注坐标系，禁止混用未标注坐标系的数据。 +// +// 实现要点： +// - 速度设定值只随**航向角 ψ**（绕下轴旋转）旋转：水平速度分量的 +// 姿态耦合项由飞控内环处理，这是 PX4 offboard 速度设定值的标准做法； +// 滚转/俯仰不参与（对速度指令为二阶小量）； +// - 旋转为正交变换：合速度大小不变，因此"限幅在机体系完成、旋转在 +// 输出端执行"不破坏 §8 限幅约束； +// - 无状态纯函数（零分配、无虚函数、无锁），可独立单元测试。 +// +// 正方向验证（NED：x=北，y=东，z=地；航向 ψ 自北顺时针为正）： +// - 机体前向 (1,0) 在 ψ=90°（机头朝东）时 → (0, 1) = 东 ✓ +// - 机体右向 (0,1) 在 ψ=0°（机头朝北）时 → (0, 1) = 东 ✓ +// ============================================================================= + +#include + +namespace drone::vtc { + +/// 机体系速度（前/右/下）→ 局部 NED（北/东/地），绕下轴按航向角 ψ 旋转： +/// v_north = v_forward·cosψ − v_right·sinψ +/// v_east = v_forward·sinψ + v_right·cosψ +/// v_down = v_down（不变） +/// 参数以引用传入并就地写出旋转结果。 +inline void RotateBodyToNed(double& v_forward, double& v_right, double& v_down, + double yaw_rad) { + (void)v_down; // 垂直分量不受绕下轴旋转影响；保留参数以保持接口对称 + const double c = std::cos(yaw_rad); + const double s = std::sin(yaw_rad); + const double north = v_forward * c - v_right * s; + const double east = v_forward * s + v_right * c; + v_forward = north; + v_right = east; + // v_down 不变 +} + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/include/visual_track_control/i_command_transport.h b/reference-c++/visual_track_control/include/visual_track_control/i_command_transport.h new file mode 100644 index 0000000..b9cc6cc --- /dev/null +++ b/reference-c++/visual_track_control/include/visual_track_control/i_command_transport.h @@ -0,0 +1,31 @@ +#pragma once + +// ============================================================================= +// i_command_transport.h —— 命令传输层抽象接口 +// +// 设计目标（双重解耦）： +// - 传输层只吃字节流：串口 / 电台 / 回环打印均可注入实现， +// 后续重写传输只换实现，不动打包与控制律； +// - 打包封装（MavlinkCommandGateway）与本接口组合，不直接依赖具体链路。 +// +// 控制律不直接写飞控（物理追踪思路 §2.2 控制单出口约束的独立库映射）： +// 网关打包后交由实现方决定如何送达。 +// ============================================================================= + +#include +#include + +namespace drone::vtc { + +/// 命令传输接口：只吃字节流 +class ICommandTransport { +public: + virtual ~ICommandTransport() = default; + + /// 发送一帧已打包的命令字节流。 + /// 返回 true = 已交付底层链路；false = 发送失败（调用方可计数/告警）。 + /// 实现方保证：不长期持有 data 指针（同步拷贝或同步写出）。 + virtual bool Send(const std::uint8_t* data, std::size_t len) = 0; +}; + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/include/visual_track_control/line_of_sight.h b/reference-c++/visual_track_control/include/visual_track_control/line_of_sight.h new file mode 100644 index 0000000..e0040e9 --- /dev/null +++ b/reference-c++/visual_track_control/include/visual_track_control/line_of_sight.h @@ -0,0 +1,38 @@ +#pragma once + +// ============================================================================= +// line_of_sight.h —— 像素偏移 → 视线角（针孔模型） +// +// 对应规格：docs/物理追踪思路.md +// §4.1 水平视线角 β： +// 针孔模型 β = atan( x_offset · 2·tan(θ_h/2) / W ) +// （线性近似 β ≈ x_offset · ( θ_h / W ) 仅在 |x_offset| 很小时成立） +// §5.1 垂直视线角 α 同理（θ_v、H） +// +// 实现要点： +// - 全程使用精确针孔模型：规格中的线性近似与针孔式在切换边界两侧 +// 函数值不连续（θ=60°、边界比例 0.2 处约 8% 角度台阶），会向控制 +// 环路注入突变；atan 计算代价可忽略，故直接全程采用精确式， +// linear_ratio 配置项随之移除（§8 待定项收敛）； +// - 构造期预计算针孔系数，热路径仅一次乘加 + atan +// （零分配、无虚函数、无锁）。 +// ============================================================================= + +namespace drone::vtc { + +/// 像素偏移 → 视线角（弧度）转换器；水平/垂直各构造一个实例 +class PixelToAngle { +public: + /// size_px：画面尺寸（W 或 H，像素，必须 > 0）； + /// fov_rad：视场角（θ_h 或 θ_v，弧度，必须 ∈ (0, π)）。 + /// 非法参数抛 std::invalid_argument（构造期校验）。 + PixelToAngle(double size_px, double fov_rad); + + /// 偏移（像素，水平右正 / 垂直下正）→ 视线角（弧度，同向为正） + double Convert(double offset_px) const; + +private: + double pinhole_scale_; // 2·tan(θ/2) / size：针孔模型系数 +}; + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/include/visual_track_control/mavlink_command_gateway.h b/reference-c++/visual_track_control/include/visual_track_control/mavlink_command_gateway.h new file mode 100644 index 0000000..91dc44f --- /dev/null +++ b/reference-c++/visual_track_control/include/visual_track_control/mavlink_command_gateway.h @@ -0,0 +1,65 @@ +#pragma once + +// ============================================================================= +// mavlink_command_gateway.h —— MAVLink 命令打包网关（对外声明） +// +// 对应规格：docs/物理追踪思路.md §2.2 +// - kVelocityHeading → SET_POSITION_TARGET_LOCAL_NED（ID 84，速度模式） +// - kBrakeHover → ID 84 全零速度 +// （原 kAttitudeTrim → ID 82 已随姿态微调支路一并移除，见 vtc_types.h） +// +// 关键构建约束（务必遵守）： +// - MAVLink 头（common/mavlink.h）只允许被本库的 +// src/mavlink_command_gateway.cpp 一个 TU 包含： +// MAVLINK_HELPER 默认空宏，多 TU 包含会多重定义； +// - 因此本头文件不出现任何 mavlink_* 类型，打包/发送缓冲用 +// 固定尺寸字节存储承载（尺寸在 .cpp 中 static_assert 校验）。 +// +// 线程与资源：无线程、无 IO、公共方法非虚；成员固定缓冲，热路径零堆分配。 +// ============================================================================= + +#include +#include + +#include "visual_track_control/i_command_transport.h" +#include "visual_track_control/vtc_types.h" + +namespace drone::vtc { + +/// MAVLink 命令打包网关：控制意图 → MAVLink 消息字节流 → 传输接口 +class MavlinkCommandGateway { +public: + /// transport：字节流传输实现（注入，生命周期由调用方保证）； + /// system_id/component_id：本机 MAVLink 身份； + /// target_system/target_component：飞控端身份。 + MavlinkCommandGateway(ICommandTransport& transport, std::uint8_t system_id, + std::uint8_t component_id, std::uint8_t target_system, + std::uint8_t target_component); + + /// 速度与航向意图 → SET_POSITION_TARGET_LOCAL_NED（ID 84，速度模式 type_mask）。 + /// 掩码选择：`yaw_rate_dps` 非零 → 角速率掩码；`yaw_deg` 非零 → 航向角掩码； + /// 两者均为零 → 角速率掩码配零角速率（保持当前航向，避免误发绝对航向 0）。 + bool SendVelocityHeading(const ControlOutput& output, std::uint32_t time_boot_ms); + + /// 零速刹车 → ID 84 全零速度（判丢等场景，§6.4）。 + bool SendBrakeHover(std::uint32_t time_boot_ms); + + // 固定缓冲尺寸（.cpp 中与实际类型大小做 static_assert 校验）： + static constexpr std::size_t kMsgBufferSize = 512; // ≥ sizeof(mavlink_message_t) + static constexpr std::size_t kTxBufferSize = 512; // ≥ MAVLINK_MAX_PACKET_LEN + +private: + ICommandTransport& transport_; // 字节流传输实现（注入） + + // MAVLink 身份 + std::uint8_t system_id_; + std::uint8_t component_id_; + std::uint8_t target_system_; + std::uint8_t target_component_; + + // 固定缓冲：热路径零堆分配（对齐 8 满足 mavlink_message_t 对齐要求） + alignas(8) std::uint8_t msg_storage_[kMsgBufferSize]; // mavlink_message_t 承载 + std::uint8_t tx_buffer_[kTxBufferSize]; // 发送字节缓冲 +}; + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/include/visual_track_control/pid_controller.h b/reference-c++/visual_track_control/include/visual_track_control/pid_controller.h new file mode 100644 index 0000000..d3a1415 --- /dev/null +++ b/reference-c++/visual_track_control/include/visual_track_control/pid_controller.h @@ -0,0 +1,59 @@ +#pragma once + +// ============================================================================= +// pid_controller.h —— 位置式 PID 控制器（纯算法构件） +// +// 对应规格：docs/物理追踪思路.md +// §6.2 距离控制律：前向速度指令 = PID(Δd)，Δd = d − d_exp +// §8 PID 参数（Kp/Ki/Kd、积分限幅、微分滤波）全部待定/配置化 +// +// 特性（热路径零分配、无虚函数、无锁）： +// - 位置式输出：out = Kp·e + Ki·∫e + Kd·d(e)/dt； +// - 积分钳位：积分项限幅在 ±integral_limit； +// - anti-windup（条件积分）：输出饱和且本拍积分增量仍推深饱和时冻结积分， +// 误差反号后允许积分回卷，防止积分饱和与残留； +// - 微分一阶低通：derivative_filter_coef ∈ [0,1)，0 = 不滤波； +// - ResetIntegral() / Reset() 均 O(1)（Coast 清积分，§6.4）。 +// ============================================================================= + +namespace drone::vtc { + +/// PID 参数（全部来自配置，代码不写死任何数值） +struct PidParams { + double kp = 0.0; // 比例增益 + double ki = 0.0; // 积分增益 + double kd = 0.0; // 微分增益 + double integral_limit = 0.0; // 积分项限幅（<=0 表示不限幅） + double derivative_filter_coef = 0.0; // 微分一阶低通系数 ∈ [0,1)：越大滤波越强 + double output_limit = 0.0; // 输出限幅（<=0 表示不限幅） +}; + +/// 位置式 PID 控制器 +class PidController { +public: + explicit PidController(const PidParams& params); + + /// 单步更新：输入误差与采样间隔（秒），返回限幅后的输出。 + /// dt_s <= 0 时保持上次输出（异常节拍保护）。 + double Update(double error, double dt_s); + + /// 清空积分项（§6.4：Coast 时防止积分饱和）；保留微分历史与上次输出 + void ResetIntegral(); + + /// 清空全部内部状态（积分、微分历史、上次输出） + void Reset(); + + /// 上次输出（只读） + double LastOutput() const { return last_output_; } + +private: + PidParams params_; + + double integral_ = 0.0; // 积分累计（已钳位） + double prev_error_ = 0.0; // 上次误差（微分用） + double filtered_derivative_ = 0.0; // 低通后的微分值 + double last_output_ = 0.0; // 上次输出 + bool has_prev_error_ = false; // 是否存在有效微分历史 +}; + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/include/visual_track_control/rate_limiter.h b/reference-c++/visual_track_control/include/visual_track_control/rate_limiter.h new file mode 100644 index 0000000..0ae8c98 --- /dev/null +++ b/reference-c++/visual_track_control/include/visual_track_control/rate_limiter.h @@ -0,0 +1,56 @@ +#pragma once + +// ============================================================================= +// rate_limiter.h —— 限幅与变化率限制（纯算法构件，header-only） +// +// 对应规格：docs/物理追踪思路.md +// §8 所有阈值集中配置：速度/加速度/方向变化率必须限幅（FR-033） +// S6 首条视觉速度设定值必须经过限幅，避免位置→速度切换产生突变 +// （状态机设计.md S6） +// +// 特性（热路径零分配、无虚函数、无锁）： +// - Clamp()：静态限幅； +// - SlewRateLimiter：保存上次输出，按配置变化率限幅；初值置 0 时 +// 天然实现"首条指令突变抑制"（S6）。 +// ============================================================================= + +#include + +namespace drone::vtc { + +/// 限幅：将 v 钳制在 [lo, hi]（调用方保证 lo <= hi） +inline double Clamp(double v, double lo, double hi) { + return std::min(std::max(v, lo), hi); +} + +/// 变化率限幅器：限制输出每秒最大变化量（单位由调用方决定， +/// 如 度/秒^2 用于航向、米/秒^2 用于速度） +class SlewRateLimiter { +public: + /// rate_limit：每秒最大变化量（<=0 表示冻结输出）；initial：初始输出值 + explicit SlewRateLimiter(double rate_limit, double initial = 0.0) + : rate_limit_(rate_limit), output_(initial) {} + + /// 朝 target 推进一个节拍：单步变化不超过 rate_limit·dt_s。 + /// dt_s <= 0 或 rate_limit <= 0 时保持当前输出（异常节拍保护）。 + double Limit(double target, double dt_s) { + if (dt_s <= 0.0 || rate_limit_ <= 0.0) { + return output_; + } + const double max_step = rate_limit_ * dt_s; + output_ = Clamp(target, output_ - max_step, output_ + max_step); + return output_; + } + + /// 复位输出值（刹车/重置时归零，保证恢复跟踪后指令从零渐变） + void Reset(double value = 0.0) { output_ = value; } + + /// 上次输出（只读） + double LastOutput() const { return output_; } + +private: + double rate_limit_; // 每秒最大变化量 + double output_; // 当前输出 +}; + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/include/visual_track_control/staleness_checker.h b/reference-c++/visual_track_control/include/visual_track_control/staleness_checker.h new file mode 100644 index 0000000..e6a6571 --- /dev/null +++ b/reference-c++/visual_track_control/include/visual_track_control/staleness_checker.h @@ -0,0 +1,38 @@ +#pragma once + +// ============================================================================= +// staleness_checker.h —— 输入超龄判定（单调毫秒时钟） +// +// 对应规格：docs/物理追踪思路.md +// §7.1 超龄数据只能触发降级或告警，不能继续生成正常控制指令； +// 超龄判定使用取样时间 + valid_for_ms（MessageHeader 语义）。 +// +// 特性：无状态纯函数封装（零分配、无虚函数、无锁）。 +// ============================================================================= + +#include + +namespace drone::vtc { + +/// 输入超龄判定器（无状态，仅提供静态判定） +class StalenessChecker { +public: + /// sample_time_ms：取样单调时钟毫秒（<=0 视为从未取样，直接判超龄）； + /// now_ms：当前单调时钟毫秒； + /// valid_for_ms：有效期（毫秒，配置给定）。 + /// 返回 true = 已超龄。 + static bool IsStale(std::int64_t sample_time_ms, std::int64_t now_ms, + std::int64_t valid_for_ms) { + if (sample_time_ms <= 0) { + return true; // 从未取样：视为超龄（触发降级） + } + if (now_ms < sample_time_ms) { + // 时间戳不一致（时钟源错配/打戳错误）：年龄不可信。 + // 单一单调时钟下不应发生；宁可降级也不信任不可核实的取样。 + return true; + } + return (now_ms - sample_time_ms) > valid_for_ms; + } +}; + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/include/visual_track_control/tracking_control_law.h b/reference-c++/visual_track_control/include/visual_track_control/tracking_control_law.h new file mode 100644 index 0000000..1ccd940 --- /dev/null +++ b/reference-c++/visual_track_control/include/visual_track_control/tracking_control_law.h @@ -0,0 +1,63 @@ +#pragma once + +// ============================================================================= +// tracking_control_law.h —— 视觉跟踪主控律（TrackingControlLaw） +// +// 对应规格：docs/物理追踪思路.md +// §4 水平航向控制（增益 → slew → 角速度限幅，位置式/速率式按配置） +// §5 垂直/高度控制（统一速度支路：α → 垂直速度，原双支路已合并） +// §6 距离控制（PID(Δd) → 前向速度；未关联按配置降级，§6.3） +// §6.4 Coast 清积分、判丢零速刹车 +// §7.1 超龄判定（姿态/视觉超龄 → 降级保持） +// §8 统一限幅 +// §2.3 输出端随机体航向把机体系速度旋转为局部 NED +// +// 线程模型：单线程调用（状态机与控制线程），成员无锁、无堆对象； +// 日志仅状态切换点（刹车进出、降级切换），高频热路径不打日志。 +// ============================================================================= + +#include + +#include "visual_track_control/line_of_sight.h" +#include "visual_track_control/pid_controller.h" +#include "visual_track_control/rate_limiter.h" +#include "visual_track_control/vtc_config.h" +#include "visual_track_control/vtc_types.h" + +namespace drone::vtc { + +/// 视觉跟踪主控律：消费控制快照，输出限幅后的控制意图 +class TrackingControlLaw { +public: + /// cfg：启动期加载的全量配置（值拷贝持有，运行期不再触碰配置源） + explicit TrackingControlLaw(const TrackingConfig& cfg); + + /// 单周期控制计算（§7.1 固定周期持续运行）。 + /// now_ms：当前单调时钟毫秒；内部据此推算 dt 并做超龄判定。 + /// 流程：①超龄判定 → ②判丢刹车 → ③Coast 清积分 → ④像素→视线角 + /// → ⑤水平航向 → ⑥垂直速度 → ⑦距离通道 → ⑧机体系→NED 旋转输出 + ControlOutput Update(const ControlSnapshot& snapshot, std::int64_t now_ms); + + /// 停止视觉闭环时清全部状态（积分、限幅器历史、节拍时钟，§6.4） + void Reset(); + +private: + TrackingConfig cfg_; // 配置值拷贝（运行期只读） + + // ---- 预计算构件（构造期完成，热路径零分配） ---- + PixelToAngle pixel_to_beta_; // 水平：x_offset → β（§4.1） + PixelToAngle pixel_to_alpha_; // 垂直：y_offset → α（§5.1） + PidController distance_pid_; // 距离通道 PID（§6.2） + + // ---- 变化率限幅器（§8 / S6 首条指令限幅） ---- + SlewRateLimiter yaw_deg_limiter_; // 位置式航向变化率（度/秒^2） + SlewRateLimiter yaw_rate_limiter_; // 速率式角速度变化率（度/秒^2） + SlewRateLimiter vx_limiter_; // 前向加速度限幅（米/秒^2） + SlewRateLimiter vz_limiter_; // 垂直加速度限幅（米/秒^2） + + // ---- 节拍与状态（仅打日志用，不参与数学） ---- + std::int64_t last_now_ms_ = 0; // 上次计算时刻 + ControlMode last_mode_ = ControlMode::kDegradedHold; // 上次输出模式（状态切换日志） +}; + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/include/visual_track_control/vtc_config.h b/reference-c++/visual_track_control/include/visual_track_control/vtc_config.h new file mode 100644 index 0000000..820a24c --- /dev/null +++ b/reference-c++/visual_track_control/include/visual_track_control/vtc_config.h @@ -0,0 +1,113 @@ +#pragma once + +// ============================================================================= +// vtc_config.h —— 视觉跟踪控制律配置（TrackingConfig + LoadFromJson） +// +// 对应规格：docs/物理追踪思路.md +// §8 限幅、阈值与配置化（所有阈值集中配置，不得散落在代码中， +// 需求分析.md NFR-007） +// §4 水平航向控制（模式、增益、限幅） +// §5 垂直/高度控制（垂直速度增益与限幅；原双支路已统一为速度支路） +// §6 距离控制（射程、PID、限幅、无距离降级动作） +// §7 控制周期与时序（频率、各输入超龄阈值） +// +// 设计约束： +// - 全部数值待定：本结构默认值与示例配置均为占位，标定前仅限回放验证； +// - LoadFromJson 启动期一次性解析：缺字段/类型错/非法值抛 +// std::runtime_error 并携带字段名（初始化抛异常规范）； +// - 热路径绝不触碰 JSON 对象，不做热更新（调参重启即可）。 +// ============================================================================= + +#include +#include + +namespace drone::vtc { + +/// 航向通道模式（§4.2：位置式/速率式按 PX4 1.11.3 Offboard 实测选择） +/// +/// 默认速率式：yaw_rate 指令天然是相对量，无坐标系歧义。位置式输出为 +/// 绝对 NED 航向（当前航向 + 限幅修正量合成），依赖新鲜自机姿态； +/// 姿态接线未完成前不得用于实飞。 +enum class HeadingMode : std::uint8_t { + kRate, // 速率式（默认）：β → 偏航角速度，输出 yaw_rate_dps + kPosition, // 位置式：β → 修正量，与当前航向合成绝对航向输出 yaw_deg +}; + +/// 雷达距离不可用（未关联/超龄）时的距离通道降级动作（§6.3，具体动作待定） +enum class NoDistanceAction : std::uint8_t { + kHold, // 保持：前向速度指令清零（安全占位语义） + kSlowApproach, // 限速接近：按 no_distance_approach_limit_mps 缓速接近 + kExit, // 退出近距操作：输出降级保持（kDegradedHold） +}; + +/// control 组：控制周期与各输入超龄阈值（§7） +struct ControlGroupConfig { + double frequency_hz = 20.0; // 控制频率（Hz，§7.1 待定） + std::int64_t visual_stale_ms = 200; // 视觉追踪结果超龄阈值（毫秒） + std::int64_t radar_stale_ms = 500; // 雷达距离超龄阈值（毫秒） + std::int64_t attitude_stale_ms = 300; // 自机姿态超龄阈值（毫秒） +}; + +/// camera 组：画面与视场角（§3.2、§4.1、§5.1；内参/畸变未标定，占位） +struct CameraGroupConfig { + int image_width = 1920; // 画面宽度 W（像素） + int image_height = 1080; // 画面高度 H（像素） + double fov_h_deg = 60.0; // 水平视场角 θ_h（度，仅水平，§4.1） + double fov_v_deg = 35.0; // 垂直视场角 θ_v（度，§5.1） +}; + +/// heading 组：水平航向通道（§4） +struct HeadingGroupConfig { + HeadingMode mode = HeadingMode::kRate; // 默认速率式（§4.2，见 HeadingMode 注释） + double gain = 1.0; // 增益：视线角(度) → 航向指令（无量纲占位） + double yaw_rate_limit_dps = 30.0; // 偏航角速度上限（度/秒，FR-033） + double yaw_slew_limit_dps2 = 60.0; // 方向变化率上限（度/秒^2，FR-033、S6 首条限幅） +}; + +/// vertical 组：垂直/高度控制（§5） +/// +/// 规格 §5.2 原设"小误差姿态微调 / 大误差垂直速度"双支路，实测评审后 +/// 统一为速度支路（见 ControlMode 注释）：切换阈值与姿态微调参数随之移除。 +/// 小误差经 vz_gain 线性映射为小垂直速度，行为连续且无模式翻转。 +struct VerticalGroupConfig { + double vz_gain_mps_per_deg = 0.1; // 垂直速度增益：α(度) → 垂直速度(米/秒) + double vz_limit_mps = 2.0; // 垂直速度限幅（米/秒） +}; + +/// distance 组：距离通道（§6） +struct DistanceGroupConfig { + double d_exp_m = 10.0; // 射程 d_exp（米，由拦截设备接口给出，§6.1 待定） + double kp = 0.0; // PID 比例（§6.2 待定） + double ki = 0.0; // PID 积分 + double kd = 0.0; // PID 微分 + double integral_limit = 0.0; // 积分项限幅 + double derivative_filter_coef = 0.0; // 微分一阶低通系数 ∈ [0,1)：0=不滤波 + double approach_velocity_limit_mps = 3.0; // 接近速度限幅（米/秒，§8） + double retreat_velocity_limit_mps = 1.0; // 后退速度限幅（米/秒，§8） + NoDistanceAction no_distance_action = NoDistanceAction::kHold; // 无距离降级（§6.3） + double no_distance_approach_limit_mps = 0.5; // kSlowApproach 限速值（米/秒） +}; + +/// accel_limit 组：各轴加速度限幅（§8，待定） +struct AccelLimitConfig { + double ax_mps2 = 2.0; // 前向加速度限幅（米/秒^2） + double ay_mps2 = 2.0; // 侧向加速度限幅（米/秒^2，预留） + double az_mps2 = 1.5; // 垂直加速度限幅（米/秒^2） +}; + +/// 视觉跟踪控制律全量配置（§8：所有阈值集中于此） +struct TrackingConfig { + ControlGroupConfig control; + CameraGroupConfig camera; + HeadingGroupConfig heading; + VerticalGroupConfig vertical; + DistanceGroupConfig distance; + AccelLimitConfig accel_limit; + + /// 从 JSON 文件一次性加载配置（启动期调用）。 + /// 缺字段 / 类型错误 / 非法取值均抛 std::runtime_error， + /// 异常消息携带字段名（如 "heading.gain"）。 + static TrackingConfig LoadFromJson(const std::string& path); +}; + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/include/visual_track_control/vtc_types.h b/reference-c++/visual_track_control/include/visual_track_control/vtc_types.h new file mode 100644 index 0000000..a156d75 --- /dev/null +++ b/reference-c++/visual_track_control/include/visual_track_control/vtc_types.h @@ -0,0 +1,102 @@ +#pragma once + +// ============================================================================= +// vtc_types.h —— 视觉跟踪控制律的中立输入/输出结构 +// +// 对应规格：docs/物理追踪思路.md +// §2 输入输出与数据流（控制意图枚举语义） +// §6.4 与追踪库的行为衔接（tracked / is_predicted 语义） +// §7.2 与 target_tracker 的衔接（TrackResult 无时间戳，由调用方在 +// LastResult() 瞬间打戳） +// +// 设计约束： +// - ControlSnapshot / ControlOutput 均为纯值类型（POD 风格）， +// 控制律热路径零堆分配； +// - 本库不依赖主工程 include/ 的任何类型，独立可编译； +// - 雷达距离必须带 associated_to_target 关联标志：未关联不得当作 +// 目标距离（§6.3、需求分析.md FR-041）。 +// ============================================================================= + +#include + +#include + +namespace drone::vtc { + +/// 控制输出模式（对应物理追踪思路 §2.2 的控制意图语义） +/// +/// 注意：规格 §5.2 原设"垂直小误差姿态微调支路"（kAttitudeTrim → +/// SET_ATTITUDE_TARGET），实测评审后取消——姿态微调支路会把整个输出切成 +/// 姿态消息，导致同周期的距离通道前向速度被丢弃（逼近停滞），且双消息 +/// 类型逐拍翻转会扰动 PX4 offboard。现垂直通道一律走速度支路（小误差 +/// 对应小 vz），全周期单一 offboard 消息。 +enum class ControlMode : std::uint8_t { + kVelocityHeading, // 速度与航向意图：速度分量 + yaw（§4.2、§5、§6.2 主输出） + kBrakeHover, // 零速刹车/悬停：判丢等场景（§6.4、§9.1） + kDegradedHold, // 降级保持：超龄/退出近距操作，禁止正常控制（§7.1、§9.2） +}; + +/// 降级原因（仅 kDegradedHold 模式有意义） +enum class DegradedReason : std::uint8_t { + kNone, // 未降级 + kAttitudeStale, // PX4 姿态过期：禁止空间转换和控制（§9.2） + kVisionStale, // 视觉追踪结果超龄：不得继续生成正常指令（§7.1） + kNoDistanceExit, // 无距离关联且配置为退出近距操作（§6.3） +}; + +/// 控制律输入快照：调用方在同一控制周期内采集并打戳 +/// +/// 注意：TrackResult 本身不含时间戳（§7.2），调用方必须在读取 +/// LastResult() 的瞬间用单调时钟打戳填入 track_time_ms； +/// 超龄判定一律使用"取样时间 + valid_for_ms"（§7.1）。 +struct ControlSnapshot { + // ---- 视觉追踪结果（控制律主输入） ---- + drone::tracker::TrackResult track; // 追踪库结果（值拷贝） + std::int64_t track_time_ms = 0; // 取样单调时钟（毫秒），<=0 视为从未取样 + + // ---- 雷达距离（§6.3：未关联仅为前向安全距离，不得当目标距离） ---- + double radar_distance_m = 0.0; // 机头前向单点距离（米） + bool associated_to_target = false; // 相机—雷达空间关联成立标志 + std::int64_t radar_time_ms = 0; // 取样单调时钟（毫秒） + + // ---- 自机姿态/速度 ---- + // 姿态过期 → 禁止控制（§9.2）。`yaw_rad` 参与输出端机体系→NED 旋转 + // （§2.3）与位置式绝对航向合成；其余字段预留，接入主工程时由 + // FlightStateSnapshot 填充（机体补偿见 §4.1）。 + bool attitude_present = false; // 姿态源当前是否可用 + std::int64_t attitude_time_ms = 0; // 姿态取样单调时钟（毫秒） + double roll_rad = 0.0; // 横滚（弧度，FRD） + double pitch_rad = 0.0; // 俯仰（弧度，FRD） + double yaw_rad = 0.0; // 航向（弧度，NED） + double yaw_rate_radps = 0.0; // 偏航角速度（弧度/秒，机体补偿预留，§4.1） + double velocity_x_mps = 0.0; // 自机速度分量（预留） + double velocity_y_mps = 0.0; + double velocity_z_mps = 0.0; +}; + +/// 控制律输出：限幅后的控制意图（§8：所有输出必须经过限幅） +/// +/// 坐标语义：速度分量在机体系（前/右/下，§2.3）内计算与限幅，输出前 +/// 已随自机航向（ControlSnapshot.yaw_rad）旋转为局部 NED（北/东/地）， +/// 与网关使用的 MAV_FRAME_LOCAL_NED 一致；因此旋转后 vy_mps 可能非零 +/// （机体系侧向恒为 0，旋转不改变合速度大小，限幅在旋转前完成）。 +struct ControlOutput { + ControlMode mode = ControlMode::kDegradedHold; // 输出模式 + bool valid = false; // 指令是否可用于发送 + + // ---- 速度分量（局部 NED：北/东/地，米/秒；由机体系随航向旋转得到） ---- + double vx_mps = 0.0; // 北向（距离通道输出经旋转，§6.2） + double vy_mps = 0.0; // 东向（机体系恒 0，航向旋转后可非零） + double vz_mps = 0.0; // 垂直（NED 下正，垂直速度支路输出，§5.2） + + // ---- 航向通道（§4.2：位置式输出 yaw_deg，速率式输出 yaw_rate_dps） ---- + double yaw_deg = 0.0; // 目标绝对航向（度，NED，右转为正；仅位置式， + // 由当前航向 + 限幅修正量合成并回绕 [-180,180)） + double yaw_rate_dps = 0.0; // 偏航角速度指令（度/秒；速率式为默认，天然相对量） + + // ---- 状态标注 ---- + DegradedReason degraded_reason = DegradedReason::kNone; // 降级原因 + bool coast_active = false; // 本周期处于 Coast（is_predicted==true，§6.4） +}; + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/src/line_of_sight.cpp b/reference-c++/visual_track_control/src/line_of_sight.cpp new file mode 100644 index 0000000..671a0bb --- /dev/null +++ b/reference-c++/visual_track_control/src/line_of_sight.cpp @@ -0,0 +1,42 @@ +// ============================================================================= +// line_of_sight.cpp —— 像素偏移 → 视线角实现（全程针孔模型） +// +// 对应规格：docs/物理追踪思路.md §4.1（水平 β）、§5.1（垂直 α）： +// 针孔模型：angle = atan( offset · 2·tan(θ/2) / size ) +// +// 原设计的线性近似双路切换已取消：线性式与针孔式在切换边界两侧不连续 +// （θ=60°、边界比例 0.2 处约 8% 角度台阶），会向控制环路注入突变； +// atan 代价可忽略，全程采用精确针孔式。 +// +// 构造期预计算针孔系数，Convert 热路径仅一次乘加 + atan。 +// ============================================================================= + +#include "visual_track_control/line_of_sight.h" + +#include +#include + +namespace drone::vtc { + +namespace { +constexpr double kPi = 3.14159265358979323846; +} + +PixelToAngle::PixelToAngle(double size_px, double fov_rad) { + // 构造期校验：非法参数直接抛异常（初始化抛异常规范） + if (!(size_px > 0.0)) { + throw std::invalid_argument("PixelToAngle: size_px 必须为正"); + } + if (!(fov_rad > 0.0 && fov_rad < kPi)) { + throw std::invalid_argument("PixelToAngle: fov_rad 必须在 (0, π) 内"); + } + + // 预计算针孔系数（§4.1 公式） + pinhole_scale_ = 2.0 * std::tan(fov_rad / 2.0) / size_px; // 2·tan(θ/2) / size +} + +double PixelToAngle::Convert(double offset_px) const { + return std::atan(offset_px * pinhole_scale_); // 针孔模型，全偏移范围精确 +} + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/src/mavlink_command_gateway.cpp b/reference-c++/visual_track_control/src/mavlink_command_gateway.cpp new file mode 100644 index 0000000..202287a --- /dev/null +++ b/reference-c++/visual_track_control/src/mavlink_command_gateway.cpp @@ -0,0 +1,119 @@ +// ============================================================================= +// mavlink_command_gateway.cpp —— MAVLink 命令打包网关实现 +// +// 对应规格：docs/物理追踪思路.md §2.2 +// kVelocityHeading → SET_POSITION_TARGET_LOCAL_NED（ID 84，速度模式） +// kBrakeHover → ID 84 全零速度 +// （原 kAttitudeTrim → ID 82 已随姿态微调支路一并移除，见 vtc_types.h） +// +// 关键构建约束：本文件是全工程唯一包含 common/mavlink.h 的 TU。 +// MAVLINK_HELPER 默认空宏（mavlink_helpers.h），多 TU 包含会多重定义。 +// +// 打包策略： +// - 使用 mavlink_msg_*_pack（无通道状态、纯静态内联、可重入）， +// 不使用 _pack_chan 通道 API； +// - mavlink_msg_to_send_buffer 输出字节流 → ICommandTransport::Send； +// - 消息/发送缓冲为成员固定持有，热路径零堆分配。 +// +// 坐标系与语义说明（待实测确认）： +// - coordinate_frame 取 MAV_FRAME_LOCAL_NED；速度分量由主控律在输出端 +// 随机体航向旋转为局部 NED（北/东/地，物理追踪思路 §2.3），本文件 +// 直接填充不再做坐标变换； +// - 速度模式 type_mask 需以 PX4 1.11.3 Offboard 实测为准 +// （物理追踪思路 §10.1）。 +// ============================================================================= + +#include "visual_track_control/mavlink_command_gateway.h" + +#include + +#include "common/mavlink.h" // 全工程唯一包含点（见文件头构建约束） + +namespace drone::vtc { +namespace { + +constexpr double kPi = 3.14159265358979323846; +constexpr double kDegToRad = kPi / 180.0; + +// ---- SET_POSITION_TARGET_LOCAL_NED（ID 84）type_mask 常量 ---- +// 掩码位语义：置位 = 忽略该字段。 +// 基础位：忽略位置 x/y/z（bit0-2）与加速度 afx/afy/afz（bit6-8）= 0x01C7。 +constexpr std::uint16_t kMaskVelocityBase = 0x01C7; +// 速度 + 航向角模式：忽略 yaw_rate（bit11） +constexpr std::uint16_t kMaskVelocityYaw = kMaskVelocityBase | 0x0800; +// 速度 + 偏航角速率模式：忽略 yaw（bit10） +constexpr std::uint16_t kMaskVelocityYawRate = kMaskVelocityBase | 0x0400; +// 刹车模式：速度全零，yaw 与 yaw_rate 均忽略（bit10、bit11） +constexpr std::uint16_t kMaskBrake = kMaskVelocityBase | 0x0C00; + +/// 固定存储解释为消息结构（打包函数只写入字节，不读取旧值） +inline mavlink_message_t* AsMessage(std::uint8_t* storage) { + return reinterpret_cast(storage); +} + +} // namespace + +// 固定缓冲尺寸校验：编译期保证存储足够（头文件不暴露 mavlink 类型） +static_assert(sizeof(mavlink_message_t) <= MavlinkCommandGateway::kMsgBufferSize, + "msg_storage_ 不足以容纳 mavlink_message_t"); +static_assert(MAVLINK_MAX_PACKET_LEN <= MavlinkCommandGateway::kTxBufferSize, + "tx_buffer_ 不足以容纳最大 MAVLink 数据包"); + +MavlinkCommandGateway::MavlinkCommandGateway(ICommandTransport& transport, + std::uint8_t system_id, + std::uint8_t component_id, + std::uint8_t target_system, + std::uint8_t target_component) + : transport_(transport), + system_id_(system_id), + component_id_(component_id), + target_system_(target_system), + target_component_(target_component) {} + +bool MavlinkCommandGateway::SendVelocityHeading(const ControlOutput& output, + std::uint32_t time_boot_ms) { + auto* msg = AsMessage(msg_storage_); + + // 掩码三分支（A2 修复）： + // 角速率非零 → 速率掩码（速率式控制律常态，§4.2）； + // 航向角非零 → 航向掩码（位置式控制律）； + // 两者均零 → 速率掩码配零角速率：保持当前航向。 + // 旧实现把"零角速率"落到航向掩码，等于误发绝对航向 0（指向正北）。 + const bool has_yaw_rate = std::fabs(output.yaw_rate_dps) > 1.0e-9; + const bool has_yaw = std::fabs(output.yaw_deg) > 1.0e-9; + const std::uint16_t type_mask = + (!has_yaw_rate && has_yaw) ? kMaskVelocityYaw : kMaskVelocityYawRate; + + mavlink_msg_set_position_target_local_ned_pack( + system_id_, component_id_, msg, + time_boot_ms, target_system_, target_component_, + MAV_FRAME_LOCAL_NED, type_mask, + 0.0F, 0.0F, 0.0F, // 位置字段：忽略 + static_cast(output.vx_mps), static_cast(output.vy_mps), + static_cast(output.vz_mps), + 0.0F, 0.0F, 0.0F, // 加速度字段：忽略 + static_cast(output.yaw_deg * kDegToRad), + static_cast(output.yaw_rate_dps * kDegToRad)); + + const std::uint16_t len = mavlink_msg_to_send_buffer(tx_buffer_, msg); + return transport_.Send(tx_buffer_, len); +} + +bool MavlinkCommandGateway::SendBrakeHover(std::uint32_t time_boot_ms) { + auto* msg = AsMessage(msg_storage_); + + // 判丢零速刹车（§6.4）：速度全零，位置/加速度/航向字段全部忽略 + mavlink_msg_set_position_target_local_ned_pack( + system_id_, component_id_, msg, + time_boot_ms, target_system_, target_component_, + MAV_FRAME_LOCAL_NED, kMaskBrake, + 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F, 0.0F, + 0.0F, 0.0F); + + const std::uint16_t len = mavlink_msg_to_send_buffer(tx_buffer_, msg); + return transport_.Send(tx_buffer_, len); +} + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/src/pid_controller.cpp b/reference-c++/visual_track_control/src/pid_controller.cpp new file mode 100644 index 0000000..687e421 --- /dev/null +++ b/reference-c++/visual_track_control/src/pid_controller.cpp @@ -0,0 +1,81 @@ +// ============================================================================= +// pid_controller.cpp —— 位置式 PID 控制器实现 +// +// 对应规格：docs/物理追踪思路.md +// §6.2 前向速度指令 = PID(Δd) +// §6.4 Coast 时积分项清空（ResetIntegral） +// +// anti-windup 策略（条件积分）：输出达到饱和限幅且本拍积分增量仍在 +// 推深饱和时，冻结本步积分累加（不采纳候选积分值），防止积分饱和； +// 误差反号后增量反向、允许积分回卷，避免饱和残留。 +// ============================================================================= + +#include "visual_track_control/pid_controller.h" + +#include + +#include "visual_track_control/rate_limiter.h" + +namespace drone::vtc { + +PidController::PidController(const PidParams& params) : params_(params) {} + +double PidController::Update(double error, double dt_s) { + if (dt_s <= 0.0) { + return last_output_; // 异常节拍保护：保持上次输出 + } + + // ---- 积分：候选值先钳位（积分限幅） ---- + double integral_candidate = integral_ + error * dt_s; + if (params_.integral_limit > 0.0) { + integral_candidate = + Clamp(integral_candidate, -params_.integral_limit, params_.integral_limit); + } + + // ---- 微分：一阶低通滤波（系数 ∈ [0,1)，0 = 不滤波） ---- + double derivative = 0.0; + if (has_prev_error_) { + derivative = (error - prev_error_) / dt_s; + const double coef = params_.derivative_filter_coef; + derivative = coef * filtered_derivative_ + (1.0 - coef) * derivative; + } + filtered_derivative_ = derivative; + prev_error_ = error; + has_prev_error_ = true; + + // ---- 位置式合成 ---- + const double integral_term = params_.ki * integral_candidate; + const double raw = params_.kp * error + integral_term + params_.kd * derivative; + + // ---- anti-windup（条件积分，C1 修复） ---- + // 冻结判据基于"本拍积分增量方向"（error·dt）：输出饱和且本拍增量仍 + // 在推深饱和时才冻结。不采用旧实现"积分项与输出同向"判据（误差反号 + // 后无法回卷），也不用 raw·error（大 kp 项会掩盖饱和方向、拖延回卷）。 + const bool saturated = params_.output_limit > 0.0 && std::fabs(raw) > params_.output_limit; + const bool increment_pushes_deeper = (error * dt_s) * raw > 0.0; + if (!(saturated && increment_pushes_deeper)) { + integral_ = integral_candidate; + } + + // ---- 输出限幅 ---- + if (params_.output_limit > 0.0) { + last_output_ = Clamp(raw, -params_.output_limit, params_.output_limit); + } else { + last_output_ = raw; + } + return last_output_; +} + +void PidController::ResetIntegral() { + integral_ = 0.0; // §6.4：Coast 清积分，防积分饱和 +} + +void PidController::Reset() { + integral_ = 0.0; + prev_error_ = 0.0; + filtered_derivative_ = 0.0; + last_output_ = 0.0; + has_prev_error_ = false; +} + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/src/tracking_control_law.cpp b/reference-c++/visual_track_control/src/tracking_control_law.cpp new file mode 100644 index 0000000..c5890c9 --- /dev/null +++ b/reference-c++/visual_track_control/src/tracking_control_law.cpp @@ -0,0 +1,239 @@ +// ============================================================================= +// tracking_control_law.cpp —— 视觉跟踪主控律实现 +// +// 对应规格：docs/物理追踪思路.md +// §4.1/§4.2 水平航向控制：像素偏移 → β → 增益 → slew → 角速度限幅， +// 位置式输出 yaw_deg / 速率式输出 yaw_rate_dps（按配置） +// §5.1/§5.2 垂直控制：统一速度支路 α → 垂直速度设定值（原"小误差姿态 +// 微调/大误差垂直速度"双支路已合并：姿态微调会把输出切成姿态 +// 消息，丢弃同周期距离通道前向速度导致逼近停滞，详见 +// vtc_types.h ControlMode 注释） +// §6.2/§6.3 距离通道：Δd = d − d_exp → PID → 前向速度（限幅）； +// 未关联/超龄 → 按配置降级动作（hold / slow_approach / exit） +// §6.4 Coast（is_predicted）清航向/距离积分；判丢（!tracked）零速刹车 +// §7.1 超龄数据只触发降级：姿态过期 → 禁止控制；视觉过期 → 降级保持 +// §8 所有输出统一限幅；首条指令经变化率限幅（S6） +// §2.3 输出端随机体航向把机体系速度旋转为局部 NED +// +// 日志纪律：仅状态切换点（刹车进出、降级切换）打印；热路径零日志、零分配。 +// ============================================================================= + +#include "visual_track_control/tracking_control_law.h" + +#include +#include + +#include + +#include "visual_track_control/frame_transform.h" +#include "visual_track_control/staleness_checker.h" + +namespace drone::vtc { +namespace { + +constexpr double kPi = 3.14159265358979323846; +constexpr double kRadToDeg = 180.0 / kPi; +constexpr double kDegToRad = kPi / 180.0; +constexpr double kMaxDtS = 0.5; // 单步 dt 上限（秒）：异常节拍保护 + +/// 角度回绕到 [-180, 180)（位置式绝对航向合成用） +double WrapDeg180(double deg) { + double d = std::fmod(deg + 180.0, 360.0); + if (d < 0.0) { + d += 360.0; + } + return d - 180.0; +} + +/// 控制模式可读名（仅状态切换日志使用） +const char* ModeName(ControlMode mode) { + switch (mode) { + case ControlMode::kVelocityHeading: return "速度航向"; + case ControlMode::kBrakeHover: return "零速刹车"; + case ControlMode::kDegradedHold: return "降级保持"; + } + return "未知"; +} + +/// 由距离配置构造 PID 参数（§6.2）；输出限幅取接近/后退限幅的较大者 +PidParams BuildDistancePidParams(const TrackingConfig& cfg) { + PidParams p; + p.kp = cfg.distance.kp; + p.ki = cfg.distance.ki; + p.kd = cfg.distance.kd; + p.integral_limit = cfg.distance.integral_limit; + p.derivative_filter_coef = cfg.distance.derivative_filter_coef; + p.output_limit = std::max(cfg.distance.approach_velocity_limit_mps, + cfg.distance.retreat_velocity_limit_mps); + return p; +} + +} // namespace + +TrackingControlLaw::TrackingControlLaw(const TrackingConfig& cfg) + : cfg_(cfg), + pixel_to_beta_(cfg.camera.image_width, cfg.camera.fov_h_deg * kDegToRad), + pixel_to_alpha_(cfg.camera.image_height, cfg.camera.fov_v_deg * kDegToRad), + distance_pid_(BuildDistancePidParams(cfg)), + yaw_deg_limiter_(cfg.heading.yaw_slew_limit_dps2), + yaw_rate_limiter_(cfg.heading.yaw_slew_limit_dps2), + vx_limiter_(cfg.accel_limit.ax_mps2), + vz_limiter_(cfg.accel_limit.az_mps2) {} + +ControlOutput TrackingControlLaw::Update(const ControlSnapshot& snapshot, + std::int64_t now_ms) { + // ---- 节拍推算（§7.1 固定周期；异常节拍用配置频率兜底） ---- + // 时钟回拨或 now_ms 未前进（同值）时不信任差值，回退配置频率： + // 单拍误差由限幅器与超龄判定兜底，不引入额外状态。 + double dt_s = 1.0 / cfg_.control.frequency_hz; + if (last_now_ms_ > 0 && now_ms > last_now_ms_) { + dt_s = std::min(static_cast(now_ms - last_now_ms_) / 1000.0, kMaxDtS); + } + last_now_ms_ = now_ms; + + ControlOutput out; + out.mode = ControlMode::kVelocityHeading; // 正常路径默认模式，分支可覆盖 + + // 状态切换日志辅助：仅模式变化时打印（刹车进出、降级切换等） + auto FinishAndLog = [&](const char* reason) { + if (out.mode != last_mode_) { + spdlog::info("[ControlLaw] 状态切换: {} -> {} ({})", + ModeName(last_mode_), ModeName(out.mode), reason); + last_mode_ = out.mode; + } + return out; + }; + + // ---- ① 超龄判定（§7.1 / §9.2） ---- + // 姿态过期或缺失：禁止空间转换和控制，立即退出正常控制（§9.2） + if (!snapshot.attitude_present || + StalenessChecker::IsStale(snapshot.attitude_time_ms, now_ms, + cfg_.control.attitude_stale_ms)) { + out.mode = ControlMode::kDegradedHold; + out.degraded_reason = DegradedReason::kAttitudeStale; + return FinishAndLog("姿态过期/缺失，禁止控制"); + } + // 视觉追踪结果超龄：不得继续生成正常指令（§7.1） + if (StalenessChecker::IsStale(snapshot.track_time_ms, now_ms, + cfg_.control.visual_stale_ms)) { + out.mode = ControlMode::kDegradedHold; + out.degraded_reason = DegradedReason::kVisionStale; + return FinishAndLog("视觉追踪结果超龄"); + } + + // ---- ② 判丢 → 零速刹车（§6.4：tracked == false 执行 kBrakeHover） ---- + if (!snapshot.track.tracked) { + if (last_mode_ != ControlMode::kBrakeHover) { + // 进入刹车：清积分与限幅器历史，恢复跟踪后指令从零渐变（S6 首条限幅） + distance_pid_.Reset(); + yaw_deg_limiter_.Reset(); + yaw_rate_limiter_.Reset(); + vx_limiter_.Reset(); + vz_limiter_.Reset(); + } + out.mode = ControlMode::kBrakeHover; // 速度分量保持默认全零 + out.valid = true; + return FinishAndLog("判丢，零速刹车"); + } + + // ---- ③ Coast：is_predicted == true → 清积分防饱和（§6.4） ---- + if (snapshot.track.is_predicted) { + distance_pid_.ResetIntegral(); + // 航向通道为纯比例增益映射（§4.2），无积分项，无需清理 + out.coast_active = true; + } + + // ---- ④ 像素偏移 → 视线角（§4.1 水平 β / §5.1 垂直 α） ---- + // x_offset = center.first − W/2（右正），y_offset = center.second − H/2（下正） + const double x_offset = + snapshot.track.center.first - cfg_.camera.image_width * 0.5; + const double y_offset = + snapshot.track.center.second - cfg_.camera.image_height * 0.5; + const double beta_deg = pixel_to_beta_.Convert(x_offset) * kRadToDeg; + const double alpha_deg = pixel_to_alpha_.Convert(y_offset) * kRadToDeg; + + // ---- ⑤ 水平航向通道（§4.2：增益 → slew → 角速度限幅） ---- + if (cfg_.heading.mode == HeadingMode::kRate) { + // 速率式：β → 偏航角速度指令，先变化率限幅再角速度限幅（FR-033） + const double target_rate = cfg_.heading.gain * beta_deg; + const double slewed = yaw_rate_limiter_.Limit(target_rate, dt_s); + out.yaw_rate_dps = Clamp(slewed, -cfg_.heading.yaw_rate_limit_dps, + cfg_.heading.yaw_rate_limit_dps); + } else { + // 位置式：β → 航向修正量，变化率限幅（方向变化率上限，FR-033）后 + // 与当前航向合成为绝对 NED 航向——ID 84 的 yaw 字段是绝对值， + // 直接发修正量会让机体转向"修正角"方向（A2 修复）。姿态已在 ① + // 通过超龄校验，yaw_rad 可用。 + const double target_yaw = cfg_.heading.gain * beta_deg; + const double correction_deg = yaw_deg_limiter_.Limit(target_yaw, dt_s); + out.yaw_deg = WrapDeg180(snapshot.yaw_rad * kRadToDeg + correction_deg); + } + + // ---- ⑥ 垂直速度通道（§5：统一速度支路） ---- + // 符号约定：α 下正（目标偏低）→ NED 下正 → vz 为正（下降趋近目标高度）。 + // 小误差自然映射为小垂直速度：无需单独的切换阈值与姿态微调支路， + // 输出模式保持 kVelocityHeading，距离通道前向速度不会被丢弃（A1 修复）。 + { + const double vz_target = cfg_.vertical.vz_gain_mps_per_deg * alpha_deg; + const double slewed = vz_limiter_.Limit(vz_target, dt_s); // 加速度限幅（§8） + out.vz_mps = Clamp(slewed, -cfg_.vertical.vz_limit_mps, + cfg_.vertical.vz_limit_mps); + } + + // ---- ⑦ 距离通道（§6.2 / §6.3） ---- + // 雷达距离不得直接当目标距离：仅关联成立且未超龄才可用（§6.3、FR-041） + const bool distance_available = + snapshot.associated_to_target && snapshot.radar_distance_m > 0.0 && + !StalenessChecker::IsStale(snapshot.radar_time_ms, now_ms, + cfg_.control.radar_stale_ms); + if (distance_available) { + // Δd = d − d_exp：d > d_exp 前向接近，d < d_exp 减速/后退（§6.2） + const double delta_d = snapshot.radar_distance_m - cfg_.distance.d_exp_m; + const double vx_raw = distance_pid_.Update(delta_d, dt_s); + const double slewed = vx_limiter_.Limit(vx_raw, dt_s); // 加速度限幅（§8） + out.vx_mps = Clamp(slewed, -cfg_.distance.retreat_velocity_limit_mps, + cfg_.distance.approach_velocity_limit_mps); + } else { + // 无可用目标距离：清积分防饱和，按配置降级动作（§6.3） + distance_pid_.ResetIntegral(); + switch (cfg_.distance.no_distance_action) { + case NoDistanceAction::kHold: + // 保持：前向指令清零（安全占位语义；接入主工程后可改为保持当前速度） + out.vx_mps = 0.0; + vx_limiter_.Reset(0.0); + break; + case NoDistanceAction::kSlowApproach: + // 限速接近：缓速前向，同样经加速度限幅 + out.vx_mps = vx_limiter_.Limit( + cfg_.distance.no_distance_approach_limit_mps, dt_s); + break; + case NoDistanceAction::kExit: + // 退出近距操作：整体降级保持 + out.mode = ControlMode::kDegradedHold; + out.degraded_reason = DegradedReason::kNoDistanceExit; + return FinishAndLog("距离未关联，退出近距操作"); + } + } + + // ---- ⑧ 机体系 → 局部 NED 旋转（§2.3，A4 修复） ---- + // 到此为止各通道限幅均在机体系完成（旋转为正交变换，合速度不变， + // 限幅约束不被破坏）；输出端随快照航向旋转为北/东/地，与网关的 + // MAV_FRAME_LOCAL_NED 一致。姿态已在 ① 通过超龄校验，yaw_rad 可用。 + RotateBodyToNed(out.vx_mps, out.vy_mps, out.vz_mps, snapshot.yaw_rad); + + out.valid = true; + return FinishAndLog(out.coast_active ? "Coast 预测跟踪" : "正常跟踪"); +} + +void TrackingControlLaw::Reset() { + // 停止视觉闭环时清全部状态（§6.4） + distance_pid_.Reset(); + yaw_deg_limiter_.Reset(); + yaw_rate_limiter_.Reset(); + vx_limiter_.Reset(); + vz_limiter_.Reset(); + last_now_ms_ = 0; + last_mode_ = ControlMode::kDegradedHold; +} + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/src/vtc_config.cpp b/reference-c++/visual_track_control/src/vtc_config.cpp new file mode 100644 index 0000000..629bc64 --- /dev/null +++ b/reference-c++/visual_track_control/src/vtc_config.cpp @@ -0,0 +1,225 @@ +// ============================================================================= +// vtc_config.cpp —— 配置加载器实现（启动期一次性解析） +// +// 对应规格：docs/物理追踪思路.md §8（阈值集中配置化，NFR-007）。 +// +// 实现要点： +// - 仅在启动期调用一次：缺字段 / 类型错误 / 非法取值抛 +// std::runtime_error，异常消息携带完整字段名（如 "heading.gain"）； +// - 热路径绝不触碰 JSON：解析结果拷贝进 TrackingConfig 后即释放； +// - 本文件是全库唯一包含 nlohmann/json 的 TU。 +// ============================================================================= + +#include "visual_track_control/vtc_config.h" + +#include +#include +#include + +#include + +namespace drone::vtc { +namespace { + +using Json = nlohmann::json; + +/// 读取并解析 JSON 文件；失败抛异常并携带路径 +Json ParseConfigFile(const std::string& path) { + std::ifstream ifs(path); + if (!ifs) { + throw std::runtime_error("配置文件打开失败: " + path); + } + try { + return Json::parse(ifs); + } catch (const std::exception& e) { + throw std::runtime_error("配置文件解析失败: " + path + " (" + e.what() + ")"); + } +} + +/// 取必需的对象分组；缺失或类型错抛异常 +const Json& RequireSection(const Json& root, const char* section) { + const auto it = root.find(section); + if (it == root.end() || !it->is_object()) { + throw std::runtime_error(std::string("配置缺失分组或类型错误: ") + section); + } + return *it; +} + +/// 取必需的数值字段；缺失/类型错抛异常并携带字段名 +double RequireNumber(const Json& section, const char* section_name, const char* key) { + const auto it = section.find(key); + if (it == section.end() || !it->is_number()) { + throw std::runtime_error(std::string("配置缺失字段或类型错误: ") + + section_name + "." + key); + } + return it->get(); +} + +/// 取必需的整数字段 +std::int64_t RequireInteger(const Json& section, const char* section_name, + const char* key) { + const auto it = section.find(key); + if (it == section.end() || !it->is_number_integer()) { + throw std::runtime_error(std::string("配置缺失字段或类型错误(需整数): ") + + section_name + "." + key); + } + return it->get(); +} + +/// 取必需的字符串字段 +std::string RequireString(const Json& section, const char* section_name, + const char* key) { + const auto it = section.find(key); + if (it == section.end() || !it->is_string()) { + throw std::runtime_error(std::string("配置缺失字段或类型错误(需字符串): ") + + section_name + "." + key); + } + return it->get(); +} + +/// 校验字段为正数；非法抛异常并携带字段名 +double RequirePositive(double value, const char* field_name) { + if (!(value > 0.0)) { + throw std::runtime_error(std::string("配置非法(必须为正): ") + field_name); + } + return value; +} + +/// 校验字段为非负数 +double RequireNonNegative(double value, const char* field_name) { + if (value < 0.0) { + throw std::runtime_error(std::string("配置非法(不得为负): ") + field_name); + } + return value; +} + +/// 校验字段在开区间 (lo, hi) +double RequireRange(double value, double lo, double hi, const char* field_name) { + if (!(value > lo && value < hi)) { + throw std::runtime_error(std::string("配置非法(超出范围): ") + field_name); + } + return value; +} + +ControlGroupConfig LoadControl(const Json& root) { + const Json& s = RequireSection(root, "control"); + ControlGroupConfig c; + c.frequency_hz = RequirePositive(RequireNumber(s, "control", "frequency_hz"), + "control.frequency_hz"); + c.visual_stale_ms = RequireInteger(s, "control", "visual_stale_ms"); + c.radar_stale_ms = RequireInteger(s, "control", "radar_stale_ms"); + c.attitude_stale_ms = RequireInteger(s, "control", "attitude_stale_ms"); + if (c.visual_stale_ms <= 0 || c.radar_stale_ms <= 0 || c.attitude_stale_ms <= 0) { + throw std::runtime_error("配置非法: control.*_stale_ms 必须为正"); + } + return c; +} + +CameraGroupConfig LoadCamera(const Json& root) { + const Json& s = RequireSection(root, "camera"); + CameraGroupConfig c; + c.image_width = static_cast( + RequirePositive(RequireInteger(s, "camera", "image_width"), "camera.image_width")); + c.image_height = static_cast( + RequirePositive(RequireInteger(s, "camera", "image_height"), "camera.image_height")); + c.fov_h_deg = RequireRange(RequireNumber(s, "camera", "fov_h_deg"), 0.0, 180.0, + "camera.fov_h_deg"); + c.fov_v_deg = RequireRange(RequireNumber(s, "camera", "fov_v_deg"), 0.0, 180.0, + "camera.fov_v_deg"); + return c; +} + +HeadingGroupConfig LoadHeading(const Json& root) { + const Json& s = RequireSection(root, "heading"); + HeadingGroupConfig h; + const std::string mode = RequireString(s, "heading", "mode"); + if (mode == "position") { + h.mode = HeadingMode::kPosition; + } else if (mode == "rate") { + h.mode = HeadingMode::kRate; + } else { + throw std::runtime_error("配置非法: heading.mode 仅支持 position / rate"); + } + h.gain = RequireNonNegative(RequireNumber(s, "heading", "gain"), "heading.gain"); + h.yaw_rate_limit_dps = RequirePositive( + RequireNumber(s, "heading", "yaw_rate_limit_dps"), "heading.yaw_rate_limit_dps"); + h.yaw_slew_limit_dps2 = RequirePositive( + RequireNumber(s, "heading", "yaw_slew_limit_dps2"), "heading.yaw_slew_limit_dps2"); + return h; +} + +VerticalGroupConfig LoadVertical(const Json& root) { + const Json& s = RequireSection(root, "vertical"); + VerticalGroupConfig v; + v.vz_gain_mps_per_deg = RequireNonNegative( + RequireNumber(s, "vertical", "vz_gain_mps_per_deg"), "vertical.vz_gain_mps_per_deg"); + v.vz_limit_mps = RequirePositive(RequireNumber(s, "vertical", "vz_limit_mps"), + "vertical.vz_limit_mps"); + return v; +} + +DistanceGroupConfig LoadDistance(const Json& root) { + const Json& s = RequireSection(root, "distance"); + DistanceGroupConfig d; + d.d_exp_m = RequirePositive(RequireNumber(s, "distance", "d_exp_m"), "distance.d_exp_m"); + d.kp = RequireNonNegative(RequireNumber(s, "distance", "kp"), "distance.kp"); + d.ki = RequireNonNegative(RequireNumber(s, "distance", "ki"), "distance.ki"); + d.kd = RequireNonNegative(RequireNumber(s, "distance", "kd"), "distance.kd"); + d.integral_limit = RequireNonNegative(RequireNumber(s, "distance", "integral_limit"), + "distance.integral_limit"); + d.derivative_filter_coef = RequireRange( + RequireNumber(s, "distance", "derivative_filter_coef"), -0.000001, 1.0, + "distance.derivative_filter_coef"); + if (d.derivative_filter_coef < 0.0) { + d.derivative_filter_coef = 0.0; // 容许 -0.0 + } + d.approach_velocity_limit_mps = RequirePositive( + RequireNumber(s, "distance", "approach_velocity_limit_mps"), + "distance.approach_velocity_limit_mps"); + d.retreat_velocity_limit_mps = RequirePositive( + RequireNumber(s, "distance", "retreat_velocity_limit_mps"), + "distance.retreat_velocity_limit_mps"); + const std::string action = RequireString(s, "distance", "no_distance_action"); + if (action == "hold") { + d.no_distance_action = NoDistanceAction::kHold; + } else if (action == "slow_approach") { + d.no_distance_action = NoDistanceAction::kSlowApproach; + } else if (action == "exit") { + d.no_distance_action = NoDistanceAction::kExit; + } else { + throw std::runtime_error( + "配置非法: distance.no_distance_action 仅支持 hold / slow_approach / exit"); + } + d.no_distance_approach_limit_mps = RequireNonNegative( + RequireNumber(s, "distance", "no_distance_approach_limit_mps"), + "distance.no_distance_approach_limit_mps"); + return d; +} + +AccelLimitConfig LoadAccelLimit(const Json& root) { + const Json& s = RequireSection(root, "accel_limit"); + AccelLimitConfig a; + a.ax_mps2 = RequirePositive(RequireNumber(s, "accel_limit", "ax_mps2"), + "accel_limit.ax_mps2"); + a.ay_mps2 = RequirePositive(RequireNumber(s, "accel_limit", "ay_mps2"), + "accel_limit.ay_mps2"); + a.az_mps2 = RequirePositive(RequireNumber(s, "accel_limit", "az_mps2"), + "accel_limit.az_mps2"); + return a; +} + +} // namespace + +TrackingConfig TrackingConfig::LoadFromJson(const std::string& path) { + const Json root = ParseConfigFile(path); + TrackingConfig cfg; + cfg.control = LoadControl(root); + cfg.camera = LoadCamera(root); + cfg.heading = LoadHeading(root); + cfg.vertical = LoadVertical(root); + cfg.distance = LoadDistance(root); + cfg.accel_limit = LoadAccelLimit(root); + return cfg; +} + +} // namespace drone::vtc diff --git a/reference-c++/visual_track_control/tests/frame_transform_test.cpp b/reference-c++/visual_track_control/tests/frame_transform_test.cpp new file mode 100644 index 0000000..9812fec --- /dev/null +++ b/reference-c++/visual_track_control/tests/frame_transform_test.cpp @@ -0,0 +1,56 @@ +// ============================================================================= +// frame_transform_test.cpp —— RotateBodyToNed 单元测试 +// +// 覆盖（物理追踪思路 §2.3，A4）： +// 零航向恒等、朝东时机体前向 → 东向、垂直分量不变、模长守恒。 +// ============================================================================= + +#include + +#include + +#include "visual_track_control/frame_transform.h" + +namespace { + +using drone::vtc::RotateBodyToNed; + +constexpr double kPi = 3.14159265358979323846; + +TEST(FrameTransformTest, ZeroYawIsIdentity) { + double x = 1.0, y = 0.5, z = -2.0; + RotateBodyToNed(x, y, z, 0.0); + EXPECT_NEAR(x, 1.0, 1e-12); // 北 = 前 + EXPECT_NEAR(y, 0.5, 1e-12); // 东 = 右 + EXPECT_NEAR(z, -2.0, 1e-12); +} + +TEST(FrameTransformTest, YawEast90MapsForwardToEast) { + double x = 2.0, y = 0.0, z = 0.0; + RotateBodyToNed(x, y, z, kPi / 2.0); // 机头朝东 + EXPECT_NEAR(x, 0.0, 1e-9); // 北向 ≈ 0 + EXPECT_NEAR(y, 2.0, 1e-9); // 东向 = 前向速度 +} + +TEST(FrameTransformTest, RightIsEastWhenHeadingNorth) { + double x = 0.0, y = 1.0, z = 0.0; + RotateBodyToNed(x, y, z, 0.0); + EXPECT_NEAR(x, 0.0, 1e-12); + EXPECT_NEAR(y, 1.0, 1e-12); +} + +TEST(FrameTransformTest, DownComponentUnchanged) { + double x = 1.0, y = 1.0, z = 3.5; + RotateBodyToNed(x, y, z, kPi / 3.0); + EXPECT_DOUBLE_EQ(z, 3.5); +} + +TEST(FrameTransformTest, PreservesHorizontalNorm) { + // 旋转为正交变换：水平合速度不变（限幅约束不被破坏的前提） + double x = 3.0, y = -4.0, z = 1.0; + const double norm_before = std::hypot(x, y); + RotateBodyToNed(x, y, z, 0.7); + EXPECT_NEAR(std::hypot(x, y), norm_before, 1e-9); +} + +} // namespace diff --git a/reference-c++/visual_track_control/tests/line_of_sight_test.cpp b/reference-c++/visual_track_control/tests/line_of_sight_test.cpp new file mode 100644 index 0000000..5762ec8 --- /dev/null +++ b/reference-c++/visual_track_control/tests/line_of_sight_test.cpp @@ -0,0 +1,69 @@ +// ============================================================================= +// line_of_sight_test.cpp —— PixelToAngle 单元测试 +// +// 覆盖（物理追踪思路 §4.1 / §5.1，C2）： +// 零偏移、画面边缘偏移 = 半视场角、奇对称、单调性、 +// 与线性近似的偏离（全程针孔模型）、构造期非法参数抛异常。 +// ============================================================================= + +#include + +#include + +#include "visual_track_control/line_of_sight.h" + +namespace { + +using drone::vtc::PixelToAngle; + +constexpr double kPi = 3.14159265358979323846; +constexpr double kRadToDeg = 180.0 / kPi; + +TEST(PixelToAngleTest, ZeroOffsetGivesZeroAngle) { + PixelToAngle conv(1000.0, 90.0 * kPi / 180.0); + EXPECT_DOUBLE_EQ(conv.Convert(0.0), 0.0); +} + +TEST(PixelToAngleTest, HalfSizeOffsetGivesHalfFov) { + // 针孔模型：偏移 = size/2 时 angle = atan(tan(θ/2)) = θ/2 + PixelToAngle conv(1000.0, 90.0 * kPi / 180.0); + EXPECT_NEAR(conv.Convert(500.0), 45.0 * kPi / 180.0, 1e-12); + EXPECT_NEAR(conv.Convert(-500.0), -45.0 * kPi / 180.0, 1e-12); +} + +TEST(PixelToAngleTest, OddSymmetry) { + PixelToAngle conv(1920.0, 60.0 * kPi / 180.0); + for (const double offset : {1.0, 100.0, 730.5}) { + EXPECT_NEAR(conv.Convert(-offset), -conv.Convert(offset), 1e-15); + } +} + +TEST(PixelToAngleTest, MonotonicIncreasing) { + PixelToAngle conv(1080.0, 35.0 * kPi / 180.0); + double prev = conv.Convert(-600.0); + for (double x = -500.0; x <= 600.0; x += 100.0) { + const double cur = conv.Convert(x); + EXPECT_GT(cur, prev); + prev = cur; + } +} + +TEST(PixelToAngleTest, DeviatesFromLinearApproximation) { + // C2：全程针孔模型。θ=90°、偏移比例 0.25 处线性近似误差约 4°， + // 针孔结果必须与之明显偏离（证明未走线性支路）。 + PixelToAngle conv(1000.0, 90.0 * kPi / 180.0); + const double pinhole_deg = conv.Convert(250.0) * kRadToDeg; // atan(250·2·tan45°/1000) + const double linear_deg = 250.0 * (90.0 / 1000.0); // θ·offset/size = 22.5° + EXPECT_NEAR(pinhole_deg, std::atan(0.5) * kRadToDeg, 1e-9); // 26.565° + EXPECT_GT(std::fabs(pinhole_deg - linear_deg), 2.0); +} + +TEST(PixelToAngleTest, ConstructorRejectsInvalidArgs) { + EXPECT_THROW(PixelToAngle(0.0, 1.0), std::invalid_argument); + EXPECT_THROW(PixelToAngle(-10.0, 1.0), std::invalid_argument); + EXPECT_THROW(PixelToAngle(1000.0, 0.0), std::invalid_argument); + EXPECT_THROW(PixelToAngle(1000.0, kPi), std::invalid_argument); + EXPECT_THROW(PixelToAngle(1000.0, kPi + 0.1), std::invalid_argument); +} + +} // namespace diff --git a/reference-c++/visual_track_control/tests/mavlink_command_gateway_test.cpp b/reference-c++/visual_track_control/tests/mavlink_command_gateway_test.cpp new file mode 100644 index 0000000..3e4fadc --- /dev/null +++ b/reference-c++/visual_track_control/tests/mavlink_command_gateway_test.cpp @@ -0,0 +1,181 @@ +// ============================================================================= +// mavlink_command_gateway_test.cpp —— MavlinkCommandGateway 单元测试 +// +// 覆盖（物理追踪思路 §2.2、§4.2，A2 掩码三分支）： +// ID 84 速度模式的 type_mask 选择（角速率非零 → 速率掩码；航向角非零 → +// 航向掩码；两者均零 → 速率掩码配零角速率，保持航向）、 +// 速度/航向字段编码、刹车消息、坐标基准、传输失败传播。 +// +// 构建约束：本文件**不得**包含 common/mavlink.h（MAVLINK_HELPER 默认空宏， +// 多 TU 包含会多重定义），故按 MAVLink v2 线格式手工解码： +// 帧头 10 字节（STX=0xFD, len, flags×2, seq, sys, comp, msgid×3 LE）， +// 载荷按字段尺寸降序排列（ID 84：time,x,y,z,vx,vy,vz,afx,afy,afz, +// yaw,yaw_rate 均 4 字节在前，随后 u16 type_mask，再 target_system、 +// target_component、coordinate_frame 三个 u8）。 +// ============================================================================= + +#include +#include +#include + +#include + +#include "visual_track_control/i_command_transport.h" +#include "visual_track_control/mavlink_command_gateway.h" +#include "visual_track_control/vtc_types.h" + +namespace { + +using drone::vtc::ControlOutput; +using drone::vtc::ICommandTransport; +using drone::vtc::MavlinkCommandGateway; + +// ---- MAVLink v2 帧布局常量（见文件头说明） ---- +constexpr std::uint8_t kMavlinkV2Stx = 0xFD; +constexpr std::size_t kHeaderLen = 10; +constexpr std::size_t kMsgId84PayloadLen = 53; + +// ---- ID 84 载荷字段偏移（相对载荷起点） ---- +constexpr std::size_t kOffVx = 16; +constexpr std::size_t kOffVy = 20; +constexpr std::size_t kOffVz = 24; +constexpr std::size_t kOffYaw = 40; +constexpr std::size_t kOffYawRate = 44; +constexpr std::size_t kOffTypeMask = 48; +// u8 三字段线序（生成头 mavlink_msg_set_position_target_local_ned.h 为准） +constexpr std::size_t kOffTargetSystem = 50; +constexpr std::size_t kOffTargetComponent = 51; +constexpr std::size_t kOffCoordFrame = 52; + +// ---- 预期掩码值（与实现中常量一致：置位 = 忽略） ---- +// 基础 0x01C7（忽略位置与加速度）| 航向相关位 +constexpr std::uint16_t kExpectMaskYaw = 0x01C7 | 0x0800; // 忽略 yaw_rate +constexpr std::uint16_t kExpectMaskYawRate = 0x01C7 | 0x0400; // 忽略 yaw +constexpr std::uint16_t kExpectMaskBrake = 0x01C7 | 0x0C00; // 两者均忽略 + +/// 捕获式传输：记录最后一次发送的字节流，可注入失败 +class CaptureTransport : public ICommandTransport { +public: + bool Send(const std::uint8_t* data, std::size_t len) override { + ++send_count_; + last_.assign(data, data + len); + return succeed_; + } + + void SetSucceed(bool ok) { succeed_ = ok; } + int send_count() const { return send_count_; } + const std::vector& last() const { return last_; } + +private: + bool succeed_ = true; + int send_count_ = 0; + std::vector last_; +}; + +float ReadFloat(const std::vector& buf, std::size_t payload_off) { + float v; + std::memcpy(&v, buf.data() + kHeaderLen + payload_off, sizeof(v)); + return v; +} + +std::uint16_t ReadU16(const std::vector& buf, std::size_t payload_off) { + return static_cast(buf[kHeaderLen + payload_off]) | + (static_cast(buf[kHeaderLen + payload_off + 1]) << 8); +} + +void ExpectId84Header(const std::vector& buf) { + ASSERT_GE(buf.size(), kHeaderLen + kMsgId84PayloadLen + 2); // +CRC + EXPECT_EQ(buf[0], kMavlinkV2Stx); + EXPECT_EQ(buf[1], kMsgId84PayloadLen); + // msgid 24 位小端（字节 7-9）= 84 + EXPECT_EQ(buf[7], 84); + EXPECT_EQ(buf[8], 0); + EXPECT_EQ(buf[9], 0); +} + +constexpr double kDegToRad = 3.14159265358979323846 / 180.0; + +TEST(MavlinkGatewayTest, YawRateModeSelectsRateMask) { + CaptureTransport transport; + MavlinkCommandGateway gateway(transport, 1, 2, 3, 4); + + ControlOutput out; + out.mode = drone::vtc::ControlMode::kVelocityHeading; + out.vx_mps = 1.5; + out.vy_mps = -0.5; + out.vz_mps = 0.25; + out.yaw_rate_dps = 5.0; + out.yaw_deg = 0.0; + ASSERT_TRUE(gateway.SendVelocityHeading(out, 12345)); + + const auto& buf = transport.last(); + ExpectId84Header(buf); + EXPECT_EQ(ReadU16(buf, kOffTypeMask), kExpectMaskYawRate); + EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVx), 1.5F); + EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVy), -0.5F); + EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVz), 0.25F); + EXPECT_FLOAT_EQ(ReadFloat(buf, kOffYawRate), static_cast(5.0 * kDegToRad)); + EXPECT_EQ(buf[kHeaderLen + kOffCoordFrame], 1); // MAV_FRAME_LOCAL_NED + EXPECT_EQ(buf[kHeaderLen + kOffTargetSystem], 3); // target_system + EXPECT_EQ(buf[kHeaderLen + kOffTargetComponent], 4); // target_component +} + +TEST(MavlinkGatewayTest, YawOnlySelectsYawMask) { + CaptureTransport transport; + MavlinkCommandGateway gateway(transport, 1, 2, 3, 4); + + ControlOutput out; + out.mode = drone::vtc::ControlMode::kVelocityHeading; + out.yaw_deg = 10.0; + out.yaw_rate_dps = 0.0; + ASSERT_TRUE(gateway.SendVelocityHeading(out, 1)); + + const auto& buf = transport.last(); + ExpectId84Header(buf); + EXPECT_EQ(ReadU16(buf, kOffTypeMask), kExpectMaskYaw); + EXPECT_FLOAT_EQ(ReadFloat(buf, kOffYaw), static_cast(10.0 * kDegToRad)); +} + +TEST(MavlinkGatewayTest, BothZeroKeepsHeadingViaRateMask) { + // A2 回归：旧实现在零角速率时落到航向掩码，等于误发绝对航向 0（正北）。 + // 正确行为：速率掩码 + 零角速率 = 保持当前航向。 + CaptureTransport transport; + MavlinkCommandGateway gateway(transport, 1, 2, 3, 4); + + ControlOutput out; + out.mode = drone::vtc::ControlMode::kVelocityHeading; + out.vx_mps = 1.0; + ASSERT_TRUE(gateway.SendVelocityHeading(out, 1)); + + const auto& buf = transport.last(); + ExpectId84Header(buf); + EXPECT_EQ(ReadU16(buf, kOffTypeMask), kExpectMaskYawRate); + EXPECT_FLOAT_EQ(ReadFloat(buf, kOffYawRate), 0.0F); +} + +TEST(MavlinkGatewayTest, BrakeHoverSendsZeroVelocityAllIgnored) { + CaptureTransport transport; + MavlinkCommandGateway gateway(transport, 1, 2, 3, 4); + ASSERT_TRUE(gateway.SendBrakeHover(777)); + + const auto& buf = transport.last(); + ExpectId84Header(buf); + EXPECT_EQ(ReadU16(buf, kOffTypeMask), kExpectMaskBrake); + EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVx), 0.0F); + EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVy), 0.0F); + EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVz), 0.0F); +} + +TEST(MavlinkGatewayTest, TransportFailurePropagates) { + CaptureTransport transport; + transport.SetSucceed(false); + MavlinkCommandGateway gateway(transport, 1, 2, 3, 4); + + ControlOutput out; + out.mode = drone::vtc::ControlMode::kVelocityHeading; + EXPECT_FALSE(gateway.SendVelocityHeading(out, 1)); + EXPECT_FALSE(gateway.SendBrakeHover(1)); + EXPECT_EQ(transport.send_count(), 2); +} + +} // namespace diff --git a/reference-c++/visual_track_control/tests/pid_controller_test.cpp b/reference-c++/visual_track_control/tests/pid_controller_test.cpp new file mode 100644 index 0000000..5e50123 --- /dev/null +++ b/reference-c++/visual_track_control/tests/pid_controller_test.cpp @@ -0,0 +1,123 @@ +// ============================================================================= +// pid_controller_test.cpp —— PidController 单元测试 +// +// 覆盖（物理追踪思路 §6.2 / §6.4）： +// 位置式 P/I/D 三项、积分钳位、输出限幅、异常节拍保护、 +// anti-windup 条件积分（C1 修复回归：饱和冻结 + 误差反号回卷）、 +// ResetIntegral / Reset 行为。 +// ============================================================================= + +#include + +#include "visual_track_control/pid_controller.h" + +namespace { + +using drone::vtc::PidController; +using drone::vtc::PidParams; + +PidParams MakeParams(double kp, double ki, double kd, double integral_limit, + double output_limit, double filter_coef = 0.0) { + PidParams p; + p.kp = kp; + p.ki = ki; + p.kd = kd; + p.integral_limit = integral_limit; + p.derivative_filter_coef = filter_coef; + p.output_limit = output_limit; + return p; +} + +TEST(PidControllerTest, ProportionalOnly) { + PidController pid(MakeParams(2.0, 0.0, 0.0, 0.0, 0.0)); + EXPECT_DOUBLE_EQ(pid.Update(3.0, 0.1), 6.0); + EXPECT_DOUBLE_EQ(pid.Update(-1.5, 0.1), -3.0); +} + +TEST(PidControllerTest, IntegralAccumulates) { + PidController pid(MakeParams(0.0, 1.0, 0.0, 0.0, 0.0)); + // 两拍 error=2、dt=0.5：积分累计 1.0 → 2.0 + EXPECT_DOUBLE_EQ(pid.Update(2.0, 0.5), 1.0); + EXPECT_DOUBLE_EQ(pid.Update(2.0, 0.5), 2.0); +} + +TEST(PidControllerTest, IntegralClampedToLimit) { + PidController pid(MakeParams(0.0, 1.0, 0.0, /*integral_limit=*/1.0, 0.0)); + for (int i = 0; i < 5; ++i) { + pid.Update(10.0, 1.0); + } + // 积分候选值被钳位在 1.0 → 输出恒为 1.0 + EXPECT_DOUBLE_EQ(pid.LastOutput(), 1.0); +} + +TEST(PidControllerTest, OutputClampedToLimit) { + PidController pid(MakeParams(10.0, 0.0, 0.0, 0.0, /*output_limit=*/3.0)); + EXPECT_DOUBLE_EQ(pid.Update(1.0, 0.1), 3.0); + EXPECT_DOUBLE_EQ(pid.Update(-1.0, 0.1), -3.0); +} + +TEST(PidControllerTest, AntiWindupFreezeWhileSaturatedSameDirection) { + // kp=0、ki=2、output_limit=5：error=+1 持续饱和时积分冻结， + // 输出恒钉在 +5（10 拍累计 10 拍也不会让积分继续增长）。 + PidController pid(MakeParams(0.0, 2.0, 0.0, 0.0, 5.0)); + for (int i = 0; i < 10; ++i) { + EXPECT_DOUBLE_EQ(pid.Update(10.0, 1.0), 5.0); + } +} + +TEST(PidControllerTest, AntiWindupUnwindsAfterErrorFlips) { + // C1 回归：误差反号后积分必须立即回卷（旧判据"积分与输出同向"会 + // 冻结回卷、输出一直钉在 +5）。 + // kp=0、ki=2、limit=5：+10 饱和 3 拍、积分冻结在 0； + // error=-1 首拍 → 输出 2×(-1) = -2（立即回卷）； + // 持续负误差使输出到达负饱和 -5（积分冻结在 -2，输出 = clamp(-6)）。 + PidController pid(MakeParams(0.0, 2.0, 0.0, 0.0, 5.0)); + for (int i = 0; i < 3; ++i) { + EXPECT_DOUBLE_EQ(pid.Update(10.0, 1.0), 5.0); + } + EXPECT_DOUBLE_EQ(pid.Update(-1.0, 1.0), -2.0); // 首拍立即回卷 + double out = -2.0; + for (int i = 1; i < 12; ++i) { + out = pid.Update(-1.0, 1.0); + } + EXPECT_NEAR(out, -5.0, 1e-9); // 旧判据此处仍为 +5.0 +} + +TEST(PidControllerTest, DerivativeFirstStepIsZero) { + PidController pid(MakeParams(0.0, 0.0, 1.0, 0.0, 0.0)); + // 首拍无微分历史：微分项为 0 + EXPECT_DOUBLE_EQ(pid.Update(5.0, 1.0), 0.0); +} + +TEST(PidControllerTest, DerivativeWithLowPassFilter) { + PidController pid(MakeParams(0.0, 0.0, 1.0, 0.0, 0.0, /*filter=*/0.5)); + pid.Update(0.0, 1.0); // 建立微分历史（导数 0） + // 第二拍原始导数 1.0，低通后 0.5×0 + 0.5×1.0 = 0.5 + EXPECT_DOUBLE_EQ(pid.Update(1.0, 1.0), 0.5); +} + +TEST(PidControllerTest, ZeroOrNegativeDtHoldsLastOutput) { + PidController pid(MakeParams(1.0, 0.0, 0.0, 0.0, 0.0)); + EXPECT_DOUBLE_EQ(pid.Update(2.0, 0.1), 2.0); + EXPECT_DOUBLE_EQ(pid.Update(99.0, 0.0), 2.0); + EXPECT_DOUBLE_EQ(pid.Update(99.0, -1.0), 2.0); +} + +TEST(PidControllerTest, ResetIntegralClearsOnlyIntegral) { + PidController pid(MakeParams(0.0, 1.0, 1.0, 0.0, 0.0)); + pid.Update(2.0, 1.0); // 积分 = 2 + pid.ResetIntegral(); + // 积分清零、微分历史保留：输出应为 kd×(0−2)/1 = −2 + EXPECT_DOUBLE_EQ(pid.Update(0.0, 1.0), -2.0); +} + +TEST(PidControllerTest, ResetClearsEverything) { + PidController pid(MakeParams(1.0, 1.0, 1.0, 0.0, 0.0)); + pid.Update(5.0, 1.0); + pid.Reset(); + EXPECT_DOUBLE_EQ(pid.LastOutput(), 0.0); + // 全清后等同全新控制器：首拍无微分历史 + EXPECT_DOUBLE_EQ(pid.Update(3.0, 1.0), 3.0 + 3.0); // kp·e + ki·∫e +} + +} // namespace diff --git a/reference-c++/visual_track_control/tests/rate_limiter_test.cpp b/reference-c++/visual_track_control/tests/rate_limiter_test.cpp new file mode 100644 index 0000000..45c7673 --- /dev/null +++ b/reference-c++/visual_track_control/tests/rate_limiter_test.cpp @@ -0,0 +1,76 @@ +// ============================================================================= +// rate_limiter_test.cpp —— Clamp / SlewRateLimiter 单元测试 +// +// 覆盖（物理追踪思路 §8、状态机设计 S6）： +// 静态限幅、变化率限幅步进、首条指令从零渐变（S6）、 +// 异常节拍保护、Reset 归零。 +// ============================================================================= + +#include + +#include "visual_track_control/rate_limiter.h" + +namespace { + +using drone::vtc::Clamp; +using drone::vtc::SlewRateLimiter; + +TEST(ClampTest, ClampsValue) { + EXPECT_DOUBLE_EQ(Clamp(5.0, -2.0, 2.0), 2.0); + EXPECT_DOUBLE_EQ(Clamp(-5.0, -2.0, 2.0), -2.0); + EXPECT_DOUBLE_EQ(Clamp(1.5, -2.0, 2.0), 1.5); + // 边界值：落在区间内 + EXPECT_DOUBLE_EQ(Clamp(2.0, -2.0, 2.0), 2.0); + EXPECT_DOUBLE_EQ(Clamp(-2.0, -2.0, 2.0), -2.0); +} + +TEST(SlewRateLimiterTest, FirstCommandRampsFromZero) { + // S6：初始输出为 0，首条大指令只能按变化率渐变 + SlewRateLimiter lim(/*rate_limit=*/2.0); + EXPECT_DOUBLE_EQ(lim.Limit(10.0, 0.5), 1.0); // 0 + 2×0.5 + EXPECT_DOUBLE_EQ(lim.Limit(10.0, 0.5), 2.0); // 继续渐变 + EXPECT_DOUBLE_EQ(lim.Limit(10.0, 10.0), 10.0); // 步长足够 → 到达目标 +} + +TEST(SlewRateLimiterTest, TracksTargetWithinStep) { + SlewRateLimiter lim(10.0); + // 步长 10×1=10，目标 3 在步长内 → 直接到达 + EXPECT_DOUBLE_EQ(lim.Limit(3.0, 1.0), 3.0); + EXPECT_DOUBLE_EQ(lim.Limit(3.0, 1.0), 3.0); // 已在目标：保持 +} + +TEST(SlewRateLimiterTest, NegativeDirection) { + SlewRateLimiter lim(2.0); + EXPECT_DOUBLE_EQ(lim.Limit(-10.0, 0.5), -1.0); + EXPECT_DOUBLE_EQ(lim.Limit(-10.0, 0.5), -2.0); +} + +TEST(SlewRateLimiterTest, ZeroOrNegativeDtHolds) { + SlewRateLimiter lim(2.0); + lim.Limit(10.0, 1.0); // 输出 2.0 + EXPECT_DOUBLE_EQ(lim.Limit(10.0, 0.0), 2.0); + EXPECT_DOUBLE_EQ(lim.Limit(10.0, -1.0), 2.0); +} + +TEST(SlewRateLimiterTest, ZeroRateLimitFreezesOutput) { + SlewRateLimiter lim(0.0); + EXPECT_DOUBLE_EQ(lim.Limit(5.0, 1.0), 0.0); +} + +TEST(SlewRateLimiterTest, ResetToValue) { + SlewRateLimiter lim(2.0); + lim.Limit(10.0, 1.0); + lim.Reset(); + EXPECT_DOUBLE_EQ(lim.LastOutput(), 0.0); + lim.Reset(3.5); + EXPECT_DOUBLE_EQ(lim.LastOutput(), 3.5); + // 复位后从新值继续渐变 + EXPECT_DOUBLE_EQ(lim.Limit(10.0, 1.0), 5.5); // 3.5 + 2×1 +} + +TEST(SlewRateLimiterTest, CustomInitialValue) { + SlewRateLimiter lim(1.0, /*initial=*/5.0); + EXPECT_DOUBLE_EQ(lim.Limit(10.0, 1.0), 6.0); +} + +} // namespace diff --git a/reference-c++/visual_track_control/tests/staleness_checker_test.cpp b/reference-c++/visual_track_control/tests/staleness_checker_test.cpp new file mode 100644 index 0000000..a4f1309 --- /dev/null +++ b/reference-c++/visual_track_control/tests/staleness_checker_test.cpp @@ -0,0 +1,41 @@ +// ============================================================================= +// staleness_checker_test.cpp —— StalenessChecker 单元测试 +// +// 覆盖（物理追踪思路 §7.1）： +// 从未取样判超龄、有效期内新鲜、超出有效期超龄、 +// 边界恰好在有效期上（不算超龄）、时间戳不一致判超龄（C4）。 +// ============================================================================= + +#include + +#include "visual_track_control/staleness_checker.h" + +namespace { + +using drone::vtc::StalenessChecker; + +TEST(StalenessCheckerTest, NeverSampledIsStale) { + EXPECT_TRUE(StalenessChecker::IsStale(0, 1000, 200)); + EXPECT_TRUE(StalenessChecker::IsStale(-5, 1000, 200)); +} + +TEST(StalenessCheckerTest, WithinValidityIsFresh) { + EXPECT_FALSE(StalenessChecker::IsStale(900, 1000, 200)); + EXPECT_FALSE(StalenessChecker::IsStale(1000, 1000, 200)); // 刚取样 +} + +TEST(StalenessCheckerTest, BeyondValidityIsStale) { + EXPECT_TRUE(StalenessChecker::IsStale(799, 1000, 200)); // 201 > 200 +} + +TEST(StalenessCheckerTest, ExactlyAtBoundaryIsFresh) { + // 超龄判定为严格大于：恰好等于有效期仍算新鲜 + EXPECT_FALSE(StalenessChecker::IsStale(800, 1000, 200)); +} + +TEST(StalenessCheckerTest, InconsistentTimestampIsStale) { + // C4：now < sample 时间戳不一致 → 年龄不可信 → 判超龄（宁降级不轻信） + EXPECT_TRUE(StalenessChecker::IsStale(2000, 1000, 200)); +} + +} // namespace diff --git a/reference-c++/visual_track_control/tests/tracking_control_law_test.cpp b/reference-c++/visual_track_control/tests/tracking_control_law_test.cpp new file mode 100644 index 0000000..4ec1bce --- /dev/null +++ b/reference-c++/visual_track_control/tests/tracking_control_law_test.cpp @@ -0,0 +1,349 @@ +// ============================================================================= +// tracking_control_law_test.cpp —— TrackingControlLaw 主控律单元测试 +// +// 覆盖（物理追踪思路 §2.3 / §4 / §5 / §6 / §7 / §9）： +// 超龄降级（姿态缺失/过期、视觉超龄、降级优先级）、判丢零速刹车、 +// Coast 清积分与标志、速率式/位置式航向通道（含绝对航向合成与回绕， +// A2）、垂直统一速度支路（A1）、距离通道三种无距离降级动作、 +// 首拍变化率限幅（S6）、机体系→NED 输出旋转（A4）。 +// ============================================================================= + +#include + +#include + +#include "visual_track_control/tracking_control_law.h" +#include "visual_track_control/vtc_config.h" +#include "visual_track_control/vtc_types.h" + +namespace { + +using namespace drone::vtc; + +constexpr double kPi = 3.14159265358979323846; +constexpr double kDegToRad = kPi / 180.0; +constexpr double kRadToDeg = 180.0 / kPi; + +/// 测试基准配置：大限幅使限幅器近似透明，便于独立验证各通道数学。 +/// 画面 1000×1000、视场角 90° → 针孔系数 2·tan(45°)/1000 = 0.002， +/// 偏移 100 像素 → 视线角 atan(0.2) ≈ 11.3099°。 +TrackingConfig MakeConfig() { + TrackingConfig cfg; + 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::kRate; + 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::kHold; + 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; +} + +/// 新鲜快照：目标在画面中心，姿态/追踪/雷达均新鲜 +ControlSnapshot MakeSnapshot(std::int64_t now_ms) { + ControlSnapshot s; + s.track.tracked = true; + s.track.is_predicted = false; + s.track.center = {500.0, 500.0}; + s.track_time_ms = now_ms; + s.attitude_present = true; + s.attitude_time_ms = now_ms; + return s; +} + +// ---- 超龄降级（§7.1 / §9.2） ---- + +TEST(TrackingControlLawTest, AttitudeMissingDegrades) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.attitude_present = false; + const auto out = law.Update(snap, 1000); + EXPECT_EQ(out.mode, ControlMode::kDegradedHold); + EXPECT_EQ(out.degraded_reason, DegradedReason::kAttitudeStale); + EXPECT_FALSE(out.valid); +} + +TEST(TrackingControlLawTest, AttitudeStaleDegrades) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.attitude_time_ms = 600; // 400 ms 前 > 300 ms 阈值 + const auto out = law.Update(snap, 1000); + EXPECT_EQ(out.mode, ControlMode::kDegradedHold); + EXPECT_EQ(out.degraded_reason, DegradedReason::kAttitudeStale); +} + +TEST(TrackingControlLawTest, VisionStaleDegrades) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.track_time_ms = 700; // 300 ms 前 > 200 ms 阈值 + const auto out = law.Update(snap, 1000); + EXPECT_EQ(out.mode, ControlMode::kDegradedHold); + EXPECT_EQ(out.degraded_reason, DegradedReason::kVisionStale); +} + +TEST(TrackingControlLawTest, NeverSampledTrackDegrades) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.track_time_ms = 0; // 从未取样 + const auto out = law.Update(snap, 1000); + EXPECT_EQ(out.mode, ControlMode::kDegradedHold); + EXPECT_EQ(out.degraded_reason, DegradedReason::kVisionStale); +} + +TEST(TrackingControlLawTest, AttitudeStaleBeatsVisionStale) { + // 降级优先级：姿态校验在前（§9.2 姿态过期禁止一切空间转换） + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.attitude_present = false; + snap.track_time_ms = 0; + const auto out = law.Update(snap, 1000); + EXPECT_EQ(out.degraded_reason, DegradedReason::kAttitudeStale); +} + +// ---- 判丢刹车（§6.4） ---- + +TEST(TrackingControlLawTest, LostTargetBrakes) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.track.tracked = false; + const auto out = law.Update(snap, 1000); + EXPECT_EQ(out.mode, ControlMode::kBrakeHover); + EXPECT_TRUE(out.valid); + EXPECT_DOUBLE_EQ(out.vx_mps, 0.0); + EXPECT_DOUBLE_EQ(out.vy_mps, 0.0); + EXPECT_DOUBLE_EQ(out.vz_mps, 0.0); +} + +// ---- Coast（§6.4） ---- + +TEST(TrackingControlLawTest, CoastSetsFlagAndKeepsTracking) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.track.is_predicted = true; + const auto out = law.Update(snap, 1000); + EXPECT_EQ(out.mode, ControlMode::kVelocityHeading); + EXPECT_TRUE(out.coast_active); + EXPECT_TRUE(out.valid); +} + +// ---- 水平航向通道（§4.2，A2） ---- + +TEST(TrackingControlLawTest, RateModeMapsBetaToYawRate) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.track.center = {600.0, 500.0}; // 目标偏右 100 像素 + const auto out = law.Update(snap, 1000); + const double beta_deg = std::atan(0.2) * kRadToDeg; // ≈ 11.31° + EXPECT_NEAR(out.yaw_rate_dps, beta_deg, 1e-6); // gain=1，右转正 + EXPECT_DOUBLE_EQ(out.yaw_deg, 0.0); +} + +TEST(TrackingControlLawTest, RateModeClampsToYawRateLimit) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.track.center = {950.0, 500.0}; // 大偏移：β≈43.5° > 30°/s 上限 + const auto out = law.Update(snap, 1000); + EXPECT_NEAR(out.yaw_rate_dps, 30.0, 1e-9); +} + +TEST(TrackingControlLawTest, PositionModeComposesAbsoluteHeading) { + // A2：位置式输出为绝对 NED 航向 = 当前航向 + 限幅修正量 + auto cfg = MakeConfig(); + cfg.heading.mode = HeadingMode::kPosition; + TrackingControlLaw law(cfg); + auto snap = MakeSnapshot(1000); + snap.track.center = {600.0, 500.0}; + snap.yaw_rad = 45.0 * kDegToRad; + const auto out = law.Update(snap, 1000); + const double beta_deg = std::atan(0.2) * kRadToDeg; + EXPECT_NEAR(out.yaw_deg, 45.0 + beta_deg, 1e-6); + EXPECT_DOUBLE_EQ(out.yaw_rate_dps, 0.0); +} + +TEST(TrackingControlLawTest, PositionModeWrapsAround180) { + auto cfg = MakeConfig(); + cfg.heading.mode = HeadingMode::kPosition; + TrackingControlLaw law(cfg); + auto snap = MakeSnapshot(1000); + snap.track.center = {600.0, 500.0}; + snap.yaw_rad = 175.0 * kDegToRad; + const auto out = law.Update(snap, 1000); + const double beta_deg = std::atan(0.2) * kRadToDeg; + // 175 + 11.31 = 186.31 → 回绕到 ≈ -173.69 + EXPECT_NEAR(out.yaw_deg, 175.0 + beta_deg - 360.0, 1e-6); + EXPECT_GT(out.yaw_deg, -180.0); + EXPECT_LT(out.yaw_deg, 180.0); +} + +// ---- 垂直统一速度支路（§5，A1） ---- + +TEST(TrackingControlLawTest, VerticalOffsetDrivesVz) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.track.center = {500.0, 600.0}; // 目标偏低 100 像素 → α>0 → 下降 + const auto out = law.Update(snap, 1000); + const double alpha_deg = std::atan(0.2) * kRadToDeg; + EXPECT_NEAR(out.vz_mps, 0.1 * alpha_deg, 1e-6); + EXPECT_EQ(out.mode, ControlMode::kVelocityHeading); // A1：不切姿态微调模式 +} + +TEST(TrackingControlLawTest, VerticalVelocityClamped) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.track.center = {500.0, 990.0}; // α≈44.5° × 0.1 = 4.45 > 2 上限 + const auto out = law.Update(snap, 1000); + EXPECT_NEAR(out.vz_mps, 2.0, 1e-9); +} + +// ---- 距离通道（§6.2 / §6.3） ---- + +TEST(TrackingControlLawTest, DistanceApproachWhenFar) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.radar_distance_m = 30.0; // Δd = +20 → kp 输出 10 → 限幅 3 + snap.associated_to_target = true; + snap.radar_time_ms = 1000; + const auto out = law.Update(snap, 1000); + EXPECT_NEAR(out.vx_mps, 3.0, 1e-9); +} + +TEST(TrackingControlLawTest, DistanceRetreatClampedWhenTooClose) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.radar_distance_m = 5.0; // Δd = -5 → kp 输出 -2.5 → 后退限幅 -1 + snap.associated_to_target = true; + snap.radar_time_ms = 1000; + const auto out = law.Update(snap, 1000); + EXPECT_NEAR(out.vx_mps, -1.0, 1e-9); +} + +TEST(TrackingControlLawTest, NoDistanceHoldZerosVx) { + TrackingControlLaw law(MakeConfig()); // 缺省 kHold + auto snap = MakeSnapshot(1000); + snap.associated_to_target = false; + const auto out = law.Update(snap, 1000); + EXPECT_EQ(out.mode, ControlMode::kVelocityHeading); + EXPECT_DOUBLE_EQ(out.vx_mps, 0.0); +} + +TEST(TrackingControlLawTest, NoDistanceSlowApproach) { + auto cfg = MakeConfig(); + cfg.distance.no_distance_action = NoDistanceAction::kSlowApproach; + TrackingControlLaw law(cfg); + auto snap = MakeSnapshot(1000); + snap.associated_to_target = false; + const auto out = law.Update(snap, 1000); + EXPECT_NEAR(out.vx_mps, 0.5, 1e-9); +} + +TEST(TrackingControlLawTest, NoDistanceExitDegrades) { + auto cfg = MakeConfig(); + cfg.distance.no_distance_action = NoDistanceAction::kExit; + TrackingControlLaw law(cfg); + auto snap = MakeSnapshot(1000); + snap.associated_to_target = false; + const auto out = law.Update(snap, 1000); + EXPECT_EQ(out.mode, ControlMode::kDegradedHold); + EXPECT_EQ(out.degraded_reason, DegradedReason::kNoDistanceExit); + EXPECT_FALSE(out.valid); +} + +TEST(TrackingControlLawTest, StaleRadarFallsBackToNoDistance) { + TrackingControlLaw law(MakeConfig()); // kHold + auto snap = MakeSnapshot(1000); + snap.radar_distance_m = 30.0; + snap.associated_to_target = true; + snap.radar_time_ms = 400; // 600 ms 前 > 500 ms 阈值 → 距离不可用 + const auto out = law.Update(snap, 1000); + EXPECT_EQ(out.mode, ControlMode::kVelocityHeading); + EXPECT_DOUBLE_EQ(out.vx_mps, 0.0); +} + +// ---- 首拍/限幅（§8、S6） ---- + +TEST(TrackingControlLawTest, FirstTickVxSlewsFromZero) { + // S6：首条速度指令必须经变化率限幅，从 0 渐变而非阶跃 + auto cfg = MakeConfig(); + cfg.accel_limit.ax_mps2 = 2.0; + TrackingControlLaw law(cfg); + auto snap = MakeSnapshot(1000); + snap.radar_distance_m = 30.0; // 通道输出本应为 3.0 + snap.associated_to_target = true; + snap.radar_time_ms = 1000; + const auto out = law.Update(snap, 1000); // 首拍：dt 按 1/20 Hz 兜底 + EXPECT_NEAR(out.vx_mps, 2.0 * (1.0 / 20.0), 1e-9); // 0.1 +} + +// ---- 机体系 → NED 旋转（§2.3，A4） ---- + +TEST(TrackingControlLawTest, OutputRotatedToNedByYaw) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.yaw_rad = kPi / 2.0; // 机头朝东 + snap.radar_distance_m = 30.0; + snap.associated_to_target = true; + snap.radar_time_ms = 1000; + const auto out = law.Update(snap, 1000); + // 机体前向 3.0 → 东向 3.0，北向 ≈ 0 + EXPECT_NEAR(out.vx_mps, 0.0, 1e-9); + EXPECT_NEAR(out.vy_mps, 3.0, 1e-9); +} + +TEST(TrackingControlLawTest, ZeroYawKeepsForwardAsNorth) { + TrackingControlLaw law(MakeConfig()); + auto snap = MakeSnapshot(1000); + snap.yaw_rad = 0.0; + snap.radar_distance_m = 30.0; + snap.associated_to_target = true; + snap.radar_time_ms = 1000; + const auto out = law.Update(snap, 1000); + EXPECT_NEAR(out.vx_mps, 3.0, 1e-9); + EXPECT_NEAR(out.vy_mps, 0.0, 1e-9); +} + +// ---- Reset（§6.4） ---- + +TEST(TrackingControlLawTest, ResetRestoresSlewFromZero) { + auto cfg = MakeConfig(); + cfg.accel_limit.ax_mps2 = 2.0; + TrackingControlLaw law(cfg); + auto snap = MakeSnapshot(1000); + snap.radar_distance_m = 30.0; + snap.associated_to_target = true; + snap.radar_time_ms = 1000; + for (std::int64_t t = 1000; t < 3000; t += 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; + const auto out = law.Update(snap, 3000); + // Reset 后限幅器归零：又从 0.1 开始渐变 + EXPECT_NEAR(out.vx_mps, 0.1, 1e-9); +} + +} // namespace diff --git a/reference-c++/visual_track_control/tests/vtc_config_test.cpp b/reference-c++/visual_track_control/tests/vtc_config_test.cpp new file mode 100644 index 0000000..8a749c6 --- /dev/null +++ b/reference-c++/visual_track_control/tests/vtc_config_test.cpp @@ -0,0 +1,224 @@ +// ============================================================================= +// vtc_config_test.cpp —— TrackingConfig::LoadFromJson 单元测试 +// +// 覆盖（物理追踪思路 §8、需求分析 NFR-007）： +// 真实配置文件加载成功与取值正确（含 A1/A2 修改后的字段结构）、 +// 缺字段 / 缺分组 / 非法枚举 / 非法数值 / 文件缺失均抛带字段名的异常。 +// +// 依赖编译期宏： +// VTC_TEST_CONFIG —— config/track_control.json 绝对路径 +// VTC_TEST_TMP_DIR —— 测试临时文件输出目录（构建目录） +// ============================================================================= + +#include +#include +#include +#include + +#include + +#include "visual_track_control/vtc_config.h" + +namespace { + +using drone::vtc::HeadingMode; +using drone::vtc::NoDistanceAction; +using drone::vtc::TrackingConfig; + +/// 与真实配置同构的完整合法 JSON（用于派生各非法变体） +const char* kFullJson = R"({ + "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 + } +})"; + +/// 在 json 中把首个 from 替换为 to（未找到即测试失败） +void ReplaceOnce(std::string& json, const std::string& from, const std::string& to) { + const auto pos = json.find(from); + ASSERT_NE(pos, std::string::npos) << "未找到待替换片段: " << from; + json.replace(pos, from.size(), to); +} + +/// 把 JSON 文本写入临时文件并返回路径（调用方用完后删除） +std::string WriteTempJson(const std::string& content, const char* name) { + const std::string path = std::string(VTC_TEST_TMP_DIR) + "/" + name; + std::ofstream ofs(path); + EXPECT_TRUE(static_cast(ofs)) << "无法写入临时配置: " << path; + ofs << content; + return path; +} + +TEST(VtcConfigTest, LoadRealConfigSucceeds) { + const TrackingConfig cfg = TrackingConfig::LoadFromJson(VTC_TEST_CONFIG); + EXPECT_DOUBLE_EQ(cfg.control.frequency_hz, 20.0); + EXPECT_EQ(cfg.control.visual_stale_ms, 200); + EXPECT_EQ(cfg.camera.image_width, 1920); + EXPECT_EQ(cfg.camera.image_height, 1080); + EXPECT_DOUBLE_EQ(cfg.camera.fov_h_deg, 60.0); + EXPECT_EQ(cfg.heading.mode, HeadingMode::kRate); // A2：默认速率式 + EXPECT_DOUBLE_EQ(cfg.heading.yaw_rate_limit_dps, 30.0); + EXPECT_DOUBLE_EQ(cfg.vertical.vz_gain_mps_per_deg, 0.1); + EXPECT_DOUBLE_EQ(cfg.vertical.vz_limit_mps, 2.0); + EXPECT_DOUBLE_EQ(cfg.distance.d_exp_m, 10.0); + EXPECT_EQ(cfg.distance.no_distance_action, NoDistanceAction::kHold); + EXPECT_DOUBLE_EQ(cfg.accel_limit.ax_mps2, 2.0); +} + +TEST(VtcConfigTest, FullJsonRoundTripMatchesExpected) { + // 内嵌合法 JSON 与真实配置文件行为一致 + const std::string path = WriteTempJson(kFullJson, "vtc_cfg_full.json"); + const TrackingConfig cfg = TrackingConfig::LoadFromJson(path); + EXPECT_EQ(cfg.heading.mode, HeadingMode::kRate); + EXPECT_DOUBLE_EQ(cfg.distance.kp, 0.5); + std::remove(path.c_str()); +} + +TEST(VtcConfigTest, PositionModeAccepted) { + std::string json = kFullJson; + ReplaceOnce(json, "\"mode\": \"rate\"", "\"mode\": \"position\""); + const std::string path = WriteTempJson(json, "vtc_cfg_position.json"); + const TrackingConfig cfg = TrackingConfig::LoadFromJson(path); + EXPECT_EQ(cfg.heading.mode, HeadingMode::kPosition); + std::remove(path.c_str()); +} + +TEST(VtcConfigTest, MissingFileThrowsWithPath) { + try { + TrackingConfig::LoadFromJson("/nonexistent/vtc_no_such_file.json"); + FAIL() << "应当抛出异常"; + } catch (const std::runtime_error& e) { + EXPECT_NE(std::string(e.what()).find("vtc_no_such_file.json"), + std::string::npos); + } +} + +TEST(VtcConfigTest, MalformedJsonThrows) { + const std::string path = WriteTempJson("{ this is not json", "vtc_cfg_bad.json"); + EXPECT_THROW(TrackingConfig::LoadFromJson(path), std::runtime_error); + std::remove(path.c_str()); +} + +TEST(VtcConfigTest, MissingFieldThrowsWithFieldName) { + std::string json = kFullJson; + ReplaceOnce(json, "\"gain\": 1.0,\n", ""); + const std::string path = WriteTempJson(json, "vtc_cfg_missing.json"); + try { + TrackingConfig::LoadFromJson(path); + FAIL() << "应当抛出异常"; + } catch (const std::runtime_error& e) { + EXPECT_NE(std::string(e.what()).find("heading.gain"), std::string::npos); + } + std::remove(path.c_str()); +} + +TEST(VtcConfigTest, MissingSectionThrows) { + std::string json = kFullJson; + ReplaceOnce(json, + ",\n \"accel_limit\": {\n" + " \"ax_mps2\": 2.0,\n" + " \"ay_mps2\": 2.0,\n" + " \"az_mps2\": 1.5\n" + " }", + ""); + const std::string path = WriteTempJson(json, "vtc_cfg_nosection.json"); + try { + TrackingConfig::LoadFromJson(path); + FAIL() << "应当抛出异常"; + } catch (const std::runtime_error& e) { + EXPECT_NE(std::string(e.what()).find("accel_limit"), std::string::npos); + } + std::remove(path.c_str()); +} + +TEST(VtcConfigTest, InvalidHeadingModeThrows) { + std::string json = kFullJson; + ReplaceOnce(json, "\"mode\": \"rate\"", "\"mode\": \"magic\""); + const std::string path = WriteTempJson(json, "vtc_cfg_badmode.json"); + try { + TrackingConfig::LoadFromJson(path); + FAIL() << "应当抛出异常"; + } catch (const std::runtime_error& e) { + EXPECT_NE(std::string(e.what()).find("heading.mode"), std::string::npos); + } + std::remove(path.c_str()); +} + +TEST(VtcConfigTest, InvalidNoDistanceActionThrows) { + std::string json = kFullJson; + ReplaceOnce(json, "\"no_distance_action\": \"hold\"", + "\"no_distance_action\": \"zoom\""); + const std::string path = WriteTempJson(json, "vtc_cfg_badaction.json"); + try { + TrackingConfig::LoadFromJson(path); + FAIL() << "应当抛出异常"; + } catch (const std::runtime_error& e) { + EXPECT_NE(std::string(e.what()).find("no_distance_action"), + std::string::npos); + } + std::remove(path.c_str()); +} + +TEST(VtcConfigTest, NegativeFrequencyThrows) { + std::string json = kFullJson; + ReplaceOnce(json, "\"frequency_hz\": 20.0", "\"frequency_hz\": -1.0"); + const std::string path = WriteTempJson(json, "vtc_cfg_negfreq.json"); + try { + TrackingConfig::LoadFromJson(path); + FAIL() << "应当抛出异常"; + } catch (const std::runtime_error& e) { + EXPECT_NE(std::string(e.what()).find("control.frequency_hz"), + std::string::npos); + } + std::remove(path.c_str()); +} + +TEST(VtcConfigTest, FovOutOfRangeThrows) { + std::string json = kFullJson; + ReplaceOnce(json, "\"fov_h_deg\": 60.0", "\"fov_h_deg\": 200.0"); + const std::string path = WriteTempJson(json, "vtc_cfg_badfov.json"); + try { + TrackingConfig::LoadFromJson(path); + FAIL() << "应当抛出异常"; + } catch (const std::runtime_error& e) { + EXPECT_NE(std::string(e.what()).find("camera.fov_h_deg"), std::string::npos); + } + std::remove(path.c_str()); +} + +} // namespace diff --git a/reference-c++/visual_track_control/视觉跟踪控制律实现文档.md b/reference-c++/visual_track_control/视觉跟踪控制律实现文档.md new file mode 100644 index 0000000..a114a33 --- /dev/null +++ b/reference-c++/visual_track_control/视觉跟踪控制律实现文档.md @@ -0,0 +1,443 @@ +# 视觉跟踪控制律实现文档（visual_track_control） + +> 独立静态库子工程，实现 `../docs/物理追踪思路.md`（下称"规格"）定义的 +> 视觉跟踪阶段（S7 VISUAL_TRACKING）控制律：水平航向、垂直速度、 +> 距离 PID 三路控制 + 统一限幅 + Coast 清积分 / 判丢刹车 + 超龄降级， +> 输出端随机体航向完成机体系→局部 NED 旋转，经 `MavlinkCommandGateway` +> 打包为 MAVLink 字节流，交由注入的 `ICommandTransport` 实现送达。 +> +> 本文档按 `../AGENTS.md`"实现文档规范"六要素组织。三份文档分工明确、 +> 互不重复： +> +> - `../docs/物理追踪思路.md`——控制律**物理与行为规格**（唯一权威）， +> 本文档不重复抄录公式推导； +> - `../docs/视觉跟踪控制律技术路线.md`——**语言无关实现路线**：单周期八步 +> 伪代码、构件与状态清单、坐标系与符号约定、打包掩码取舍、重写指引； +> - 本文档——**C++ 落地说明**：公共头一览、构建与测试方式、日志行为、 +> 排查与修改要点。 +> +> 追踪库行为约定见 `../target_tracker/target_tracker.md`，同样只引用不复述。 + +--- + +## 一、功能职责与边界 + +命名空间 `drone::vtc`，头文件保护一律 `#pragma once`。 + +**做什么**： + +- **三路控制律**（`TrackingControlLaw::Update()` 单周期计算）： + - 水平航向（规格 §4）：像素偏移 → 水平视线角 β → 增益 → 变化率限幅 → + 角速度限幅，按 `HeadingMode` 配置输出速率式 `yaw_rate_dps`（默认） + 或位置式 `yaw_deg`（绝对 NED 航向，见 §3.8）； + - 垂直速度（规格 §5）：垂直视线角 α → 统一速度支路 → `vz_mps` + 垂直速度设定值（原"小误差姿态微调/大误差垂直速度"双支路已取消， + 原因见 `vtc_types.h` 的 `ControlMode` 注释与 §3.8）； + - 距离通道（规格 §6）：`Δd = d − d_exp` → `PidController` → 前向速度 + `vx_mps`；雷达未关联/超龄时按 `NoDistanceAction` 配置降级 + （hold / slow_approach / exit）。 +- **统一限幅**（规格 §8）：所有输出通道内联完成限幅——`Clamp` 静态限幅 + + `SlewRateLimiter` 变化率限幅（航向修正/角速度、前向/垂直加速度）， + 全部在机体系完成。 +- **坐标系旋转**（规格 §2.3）：输出端用 `RotateBodyToNed()` 把机体系 + 速度（前/右/下）随快照航向 ψ 旋转为局部 NED（北/东/地），与网关的 + `MAV_FRAME_LOCAL_NED` 一致；仅绕下轴旋转（滚转/俯仰对速度指令为 + 二阶小量，由飞控内环处理）。 +- **Coast 清积分**（规格 §6.4）：`is_predicted == true` 时清空距离 PID + 积分项（`ResetIntegral()`），防积分饱和。 +- **判丢刹车**（规格 §6.4）：`tracked == false` 时输出 `kBrakeHover` + 零速刹车，并清积分与全部限幅器历史（保证恢复跟踪后指令从零渐变）。 +- **超龄降级**（规格 §7.1、§9.2）：姿态过期/缺失 → `kDegradedHold` + （`kAttitudeStale`，禁止控制）；视觉结果超龄 → `kDegradedHold` + （`kVisionStale`）；判定器为无状态 `StalenessChecker::IsStale()`。 +- **MAVLink 打包**（规格 §2.2）：`MavlinkCommandGateway` 把 + `ControlOutput` 打包为 `SET_POSITION_TARGET_LOCAL_NED`（ID 84）字节流。 +- **配置化**（规格 §8、需求分析 NFR-007）：全部阈值集中在 + `TrackingConfig`，由 `TrackingConfig::LoadFromJson()` 启动期一次性加载， + 代码不写死任何参数。 + +**不做什么**（边界）： + +- **不做感知**：不运行检测/光流/雷达驱动，只消费调用方填入的 + `ControlSnapshot`； +- **不做状态机仲裁**：S6/S8 切换、目标丢失策略（`ITargetLostStrategy`）、 + 障碍接管均在状态机层，本库只按输入标志产生当拍输出； +- **不写串口/传输**：库内无任何链路代码，字节流交给 `ICommandTransport` + 实现方； +- **不解析 MAVLink 输入**：只有打包发送方向，无 `mavlink_parse_*`； +- **不做机体补偿**：`ControlSnapshot` 的 `roll_rad` / `pitch_rad` / + `yaw_rate_radps` / `velocity_*` 字段为预留输入（规格 §4.1）；当前仅 + `yaw_rad` 参与计算（NED 旋转与位置式航向合成）； +- **不做热更新**：配置启动期加载一次，调参重启生效。 + +--- + +## 二、接口与数据流 + +### 2.1 公共头一览 + +| 头文件 | 内容 | +|--------|------| +| `include/visual_track_control/vtc_types.h` | `ControlMode`、`DegradedReason`、`ControlSnapshot`、`ControlOutput` | +| `include/visual_track_control/vtc_config.h` | `HeadingMode`、`NoDistanceAction`、各分组配置结构、`TrackingConfig` | +| `include/visual_track_control/tracking_control_law.h` | `TrackingControlLaw`（主控律） | +| `include/visual_track_control/pid_controller.h` | `PidParams`、`PidController` | +| `include/visual_track_control/rate_limiter.h` | `Clamp()`、`SlewRateLimiter`（header-only） | +| `include/visual_track_control/staleness_checker.h` | `StalenessChecker`（header-only） | +| `include/visual_track_control/line_of_sight.h` | `PixelToAngle`（像素→视线角，全程针孔模型） | +| `include/visual_track_control/frame_transform.h` | `RotateBodyToNed()`（机体系→局部 NED，header-only） | +| `include/visual_track_control/i_command_transport.h` | `ICommandTransport`（传输抽象） | +| `include/visual_track_control/mavlink_command_gateway.h` | `MavlinkCommandGateway`（MAVLink 打包） | + +### 2.2 关键字段（以代码为准） + +`enum class ControlMode`：`kVelocityHeading`（速度航向主输出）/ +`kBrakeHover`（零速刹车）/ `kDegradedHold`（降级保持，禁止正常控制）。 +（原 `kAttitudeTrim` 姿态微调模式已取消，见 §3.8。） + +`enum class DegradedReason`：`kNone` / `kAttitudeStale`（姿态过期）/ +`kVisionStale`（视觉超龄）/ `kNoDistanceExit`（无距离且配置退出近距操作）。 + +`ControlSnapshot`（输入，纯值类型）： + +| 字段 | 含义 | +|------|------| +| `track` | `drone::tracker::TrackResult` 值拷贝（追踪库主目标输出，字段定义见 `../target_tracker/target_tracker.md` §二） | +| `track_time_ms` | 追踪结果取样单调时钟（毫秒）；**<=0 视为从未取样，直接判超龄** | +| `radar_distance_m` | 机头前向单点距离（米） | +| `associated_to_target` | 相机—雷达空间关联成立标志；**未关联不得当目标距离**（规格 §6.3、需求分析 FR-041） | +| `radar_time_ms` | 雷达取样单调时钟 | +| `attitude_present` / `attitude_time_ms` | 姿态源可用性与取样时刻（过期→禁止控制，规格 §9.2） | +| `yaw_rad` | 自机航向（弧度，NED）；**参与计算**：输出端机体系→NED 旋转（§2.3）与位置式绝对航向合成 | +| `roll_rad` / `pitch_rad` / `yaw_rate_radps` | 自机姿态其余分量（弧度，FRD）；预留，机体补偿未实现（规格 §4.1） | +| `velocity_x_mps` / `velocity_y_mps` / `velocity_z_mps` | 自机速度分量（预留，接入主工程时由 `FlightStateSnapshot` 填充） | + +`ControlOutput`（输出，已限幅）： + +| 字段 | 含义 | +|------|------| +| `mode` / `valid` | 输出模式 / 指令是否可用于发送 | +| `vx_mps` / `vy_mps` / `vz_mps` | **局部 NED 速度分量（北/东/地，米/秒）**：各通道在机体系计算并限幅后，输出端随 `snapshot.yaw_rad` 旋转得到；旋转为正交变换，合速度大小不变。机体系侧向恒为 0，旋转后 `vy_mps` 可非零 | +| `yaw_deg` | 位置式目标**绝对航向**（度，NED，右转为正）：由当前航向 + 变化率限幅后的修正量合成，回绕到 [-180, 180)（ID 84 的 yaw 字段为绝对值） | +| `yaw_rate_dps` | 速率式偏航角速度指令（度/秒，右转为正；天然是相对量，无坐标系歧义） | +| `degraded_reason` | 降级原因（仅 `kDegradedHold` 有意义） | +| `coast_active` | 本周期处于 Coast（`is_predicted == true`） | + +`TrackingConfig` 六分组：`control`（`frequency_hz`、`visual_stale_ms`、 +`radar_stale_ms`、`attitude_stale_ms`）、`camera`（`image_width`、 +`image_height`、`fov_h_deg`、`fov_v_deg`）、`heading`（`mode`、`gain`、 +`yaw_rate_limit_dps`、`yaw_slew_limit_dps2`）、`vertical` +（`vz_gain_mps_per_deg`、`vz_limit_mps`）、`distance`（`d_exp_m`、 +`kp`/`ki`/`kd`、`integral_limit`、`derivative_filter_coef`、 +`approach_velocity_limit_mps`、`retreat_velocity_limit_mps`、 +`no_distance_action`、`no_distance_approach_limit_mps`）、`accel_limit` +（`ax_mps2`、`ay_mps2`、`az_mps2`）。JSON 键名与字段一一对应，见 +`config/track_control.json`。`heading.mode` 默认速率式（`"rate"`）； +相机组原 `linear_ratio` 键已随线性/针孔双路一并移除（§3.5）。 + +### 2.3 ICommandTransport 契约 + +```cpp +class ICommandTransport { +public: + virtual bool Send(const std::uint8_t* data, std::size_t len) = 0; +}; +``` + +- **只吃字节流**：串口、电台、回环打印均可注入实现；后续重写传输层 + 只换实现，不动打包与控制律； +- 返回 `true` = 已交付底层链路，`false` = 发送失败（调用方可计数/告警）； +- 实现方保证不长期持有 `data` 指针（同步拷贝或同步写出）。 + +### 2.4 数据流向 + +```text +推理侧每帧: TargetTracker::Update(detections, frame_shape) +控制线程(固定周期, 规格 §7.1): + TargetTracker::LastResult() # 线程安全拷贝 + → 取样瞬间用单调时钟打戳填入 ControlSnapshot.track_time_ms + → TrackingControlLaw::Update(snapshot, now_ms) # 单周期计算 + → 按 ControlOutput.mode 分发: + kVelocityHeading → MavlinkCommandGateway::SendVelocityHeading() + kBrakeHover → MavlinkCommandGateway::SendBrakeHover() + kDegradedHold → 不发送（仅告警/降级） + → mavlink_msg_to_send_buffer → ICommandTransport::Send(字节流) +停止视觉闭环: TrackingControlLaw::Reset()（规格 §6.4） +``` + +--- + +## 三、关键实现点 + +### 3.1 传输接口只吃字节流的双重解耦 + +打包（`MavlinkCommandGateway`）与传输（`ICommandTransport`）分离： +网关只负责 `ControlOutput` → MAVLink 字节，不感知链路形态；传输实现 +只感知字节，不感知消息语义。对应规格 §2.2 控制单出口约束在独立库中的 +映射——控制律不直接写飞控。 + +### 3.2 TrackResult 无时间戳，由调用方取样打戳 + +`TrackResult` 是追踪库自有类型，本身不含时间戳（规格 §7.2）。约定： +调用方在读取 `LastResult()` 的**瞬间**用单调时钟打戳填入 +`track_time_ms`；超龄判定一律用"取样时间 + `valid_for_ms`" +（`StalenessChecker::IsStale(sample_time_ms, now_ms, valid_for_ms)`）： + +- `sample_time_ms <= 0` 视为从未取样，直接判超龄； +- `now_ms < sample_time_ms`（时间戳不一致：时钟源错配/打戳错误）同样 + 判超龄——单一单调时钟下不应发生，宁可降级也不信任不可核实的取样。 + +### 3.3 PID anti-windup 策略（条件积分） + +`PidController::Update()`（位置式）三层防护： + +1. **积分钳位**：候选积分值先钳制在 ±`integral_limit`； +2. **anti-windup（条件积分）**：输出饱和（`|raw| > output_limit`）且 + **本拍积分增量仍在推深饱和**（增量 `error·dt` 与原始输出 `raw` + 同号）时，冻结本步积分累加（不采纳候选积分值）；误差反号后增量反向， + 积分立即恢复回卷，无饱和残留。判据刻意基于"增量方向"而非"积分项与 + 输出同向"：后者在误差反号后仍会冻结回卷；也不用 `raw·error` 的 + 变体含混表述，实现即 `(error*dt_s) * raw > 0`； +3. **输出限幅**：最终输出钳制在 ±`output_limit`。 + +微分带一阶低通（`derivative_filter_coef ∈ [0,1)`，0 = 不滤波）； +`dt_s <= 0` 时保持上次输出（异常节拍保护）。`ResetIntegral()` 只清积分， +保留微分历史与上次输出；`Reset()` 清全部，均 O(1)（Coast 清积分， +规格 §6.4）。 + +### 3.4 SlewRateLimiter 兼作首条指令突变抑制 + +`SlewRateLimiter` 保存上次输出，单步变化不超过 `rate_limit · dt_s`； +构造时初值置 0，天然实现规格要求的"首条视觉速度设定值必须经过限幅" +（状态机设计 S6）：切换进入视觉跟踪后首条指令从零按加速度限幅渐变， +无需专门的切换分支。判丢进刹车时调用 `Reset()` 归零，恢复跟踪后同样 +从零渐变。主控律共 4 个限幅器：`yaw_deg_limiter_` / `yaw_rate_limiter_` +（度/秒²）、`vx_limiter_` / `vz_limiter_`（米/秒²）。 + +### 3.5 像素→角度：全程针孔模型 + +`PixelToAngle` 构造期预计算针孔系数 `2·tan(θ/2)/size`，热路径仅一次 +乘加 + `atan`：`angle = atan(offset·2·tan(θ/2)/size)`（规格 §4.1/§5.1）。 +原"小偏移线性近似/大偏移针孔"双路切换已取消：线性式与针孔式在切换边界 +两侧不连续（θ=60°、边界比例 0.2 处约 8% 角度台阶），会向控制环路注入 +突变；`atan` 代价可忽略，全程采用精确针孔式。配置中 `linear_ratio` 键 +随之移除。水平 β（`PixelToAngle(W, θ_h)`）与垂直 α（`PixelToAngle(H, θ_v)`） +各一个实例，构造期校验参数合法性（尺寸、视场角），非法抛 +`std::invalid_argument`。 + +### 3.6 全工程仅 mavlink_command_gateway.cpp 一个 TU 包含 MAVLink 头 + +c_library_v2 的 `mavlink_helpers.h` 中 **`MAVLINK_HELPER` 默认为空宏**， +辅助函数直接以外部链接定义在头文件中；多个翻译单元包含 `common/mavlink.h` +会产生**多重定义**链接错误。因此： + +- `common/mavlink.h` 只被 `src/mavlink_command_gateway.cpp` 包含 + （该文件也是全库唯一的 MAVLink 知识落点）；单元测试同样**不得**包含 + 该头，按 MAVLink v2 线格式手工解码字节流断言（见 §5.3）； +- 公共头 `mavlink_command_gateway.h` 不出现任何 `mavlink_*` 类型， + 打包/发送缓冲用成员固定字节存储承载（`msg_storage_` / `tx_buffer_`， + 各 512 字节，`.cpp` 中 `static_assert` 校验 ≥ `sizeof(mavlink_message_t)` + 与 `MAVLINK_MAX_PACKET_LEN`）； +- 打包用 `mavlink_msg_*_pack`（无通道状态、纯静态内联、可重入）， + 不使用 `_pack_chan` 通道 API； +- CMake 中 MAVLink include 目录设为 `SYSTEM PRIVATE`：压第三方告警噪音， + 且不泄漏给下游。 + +同理，`nlohmann/json` 只被 `src/vtc_config.cpp` 一个 TU 包含。 + +### 3.7 配置启动期一次加载，不做热更新 + +`TrackingConfig::LoadFromJson()` 仅在启动期调用一次：解析结果拷贝进 +`TrackingConfig` 值对象后即与 JSON 脱钩，热路径绝不触碰 JSON 对象； +调参重启即可。若未来需要热更新，扩展点为**原子指针交换**：持有 +`std::atomic`，后台解析新配置成功后原子换指针， +控制线程每拍读取当前指针快照使用（当前未实现，不引入额外复杂度）。 + +### 3.8 主控律单周期流程 + +`Update()` 按固定流程执行（`last_now_ms_` 推算 dt，首拍与异常节拍 +（时钟回拨/`now_ms` 未前进）用 `1.0 / control.frequency_hz` 兜底， +dt 上限 `kMaxDtS = 0.5` 秒）： + +① 超龄判定（姿态过期/缺失 → `kDegradedHold`；视觉超龄 → `kDegradedHold`） +→ ② 判丢刹车（清积分与限幅器历史）→ ③ Coast 清距离积分 +→ ④ 像素偏移 → β/α（针孔模型）→ ⑤ 水平航向（按 `HeadingMode` 分支） +→ ⑥ 垂直统一速度支路（α → `vz_gain·α` → 加速度限幅 → 速度限幅） +→ ⑦ 距离通道（关联成立且未超龄才可用；否则清积分并按 +`no_distance_action` 降级）→ ⑧ 机体系→NED 旋转（`RotateBodyToNed`， +随 `snapshot.yaw_rad`）→ 置 `valid = true` 输出。 + +两个设计决策记录： + +- **垂直双支路合并（A1）**：规格 §5.2 原设"小误差姿态微调（`kAttitudeTrim` + → ID 82）/ 大误差垂直速度"双支路。姿态微调支路会把整个输出切成姿态 + 消息，导致同周期的距离通道前向速度被丢弃（逼近停滞），且双消息类型 + 逐拍翻转会扰动 PX4 offboard。现垂直通道一律走速度支路：小误差经 + `vz_gain` 线性映射为小垂直速度，行为连续、全周期单一 offboard 消息。 +- **位置式航向输出绝对值（A2）**：ID 84 的 `yaw` 字段是**绝对** NED + 航向。若把 β 修正量直接填入，飞控会转向"修正角"方向而非目标方向。 + 位置式分支输出 `WrapDeg180(当前航向 + 限幅修正量)`；速率式 + （默认）输出角速度指令，天然相对量、无此问题。 + +### 3.9 MAVLink 消息映射（规格 §2.2） + +全库仅 ID 84 `SET_POSITION_TARGET_LOCAL_NED` 一种消息（原 `kAttitudeTrim` +→ ID 82 `SET_ATTITUDE_TARGET` 已随姿态微调支路移除）： + +| 模式 | 消息 | 要点 | +|------|------|------| +| `kVelocityHeading` | ID 84 | `coordinate_frame = MAV_FRAME_LOCAL_NED`（速度分量已是 NED 语义）。速度模式 `type_mask` 基础位 `kMaskVelocityBase = 0x01C7`（忽略位置与加速度），航向三分支：角速率非零 → `kMaskVelocityYawRate`（再忽略 yaw）；仅航向角非零 → `kMaskVelocityYaw`（再忽略 yaw_rate）；**两者均零 → 速率掩码配零角速率（保持当前航向）**——旧实现把零角速率落到航向掩码，等于误发绝对航向 0（指向正北） | +| `kBrakeHover` | ID 84 | `kMaskBrake = 0x0DC7`：速度全零，位置/加速度/航向/角速率字段全部忽略 | + +--- + +## 四、日志行为 + +遵循 `../AGENTS.md` 日志纪律：**库内仅状态切换点打日志，热路径零日志、 +零堆分配**。 + +| 位置 | 日志 | 触发条件 | +|------|------|----------| +| `tracking_control_law.cpp`（`Update()` 内 `FinishAndLog`） | `spdlog::info` "[ControlLaw] 状态切换: X -> Y (原因)" | **仅当 `ControlMode` 相对 `last_mode_` 发生变化**时打印（刹车进出、降级切换、进入/退出 Coast 跟踪）；同一模式连续多拍不重复打印 | +| `tracking_control_law.cpp` | —— | 每拍正常计算（含三路通道数学）**不打任何日志** | +| `mavlink_command_gateway.cpp` / `pid_controller.cpp` / `line_of_sight.cpp` / `vtc_config.cpp` | —— | 不打日志（配置错误走异常，见第六章） | + +示例程序（`examples/track_control_demo.cpp`，非生产代码）另有两处 +**演示观测日志**，均带节流防刷屏： + +- `LoggingTransport::Send()`：字节统计与包头十六进制摘要，**秒级节流** + （距上次打印 ≥ 1000 ms 才输出）； +- 主循环状态摘要：**每 10 拍一次**（20 Hz 下即半秒一次）。 + +--- + +## 五、构建、测试与运行方式 + +### 5.1 构建 + +`../AGENTS.md` 规定构建环境为 WSL2 Ubuntu 24.04。**当前开发机 WSL 仅 +装有 Ubuntu 22.04（发行版名 `Ubuntu`）**，本库的构建与测试验证在 +22.04（cmake 3.22 / g++ 11.4）完成，行为与 24.04 无差异；24.04 发行版 +安装后应按其复核一遍（见 §6.4）。 + +```bash +cmake -S visual_track_control -B build_vtc -DBUILD_TESTING=OFF +cmake --build build_vtc -j$(nproc) +./build_vtc/track_control_demo +``` + +- **为什么 `-DBUILD_TESTING=OFF`**：本库以 `add_subdirectory` 零改动复用 + `target_tracker`，而 `target_tracker/CMakeLists.txt` 中 `include(CTest)` + 会把 `BUILD_TESTING` 默认置为 `ON`，从而连带构建追踪库的 3 个测试 + 可执行文件。本子工程不关心其测试，显式关闭以加快构建（追踪库自身的 + 测试方式见 `../target_tracker/target_tracker.md` §五）。 +- **本库自身测试不复用 `BUILD_TESTING`**：由独立选项 `VTC_BUILD_TESTS` + （默认 `ON`）控制，与上面的关闭互不干扰。 +- **示例无需 `LD_LIBRARY_PATH`**：`target_tracker` 为 SHARED 库， + 构建期 `CMAKE_BUILD_RPATH` 已指向构建目录，`track_control_demo` + 可直接运行。 +- 示例缺省配置路径由编译期宏 `VTC_DEMO_DEFAULT_CONFIG` 内置为 + `config/track_control.json` 的绝对路径；也可命令行传入 + `track_control_demo [配置文件路径]`。 + +### 5.2 示例演示内容 + +单线程模拟完整链路（合成运动检测框 → `TargetTracker::Update()` → +`LastResult()` + 打戳 → `TrackingControlLaw::Update()` → Gateway → +`LoggingTransport` 字节打印），按配置频率固定节拍依次走三阶段 +（对应规格 §6.4）： + +1. **正常跟踪（6 秒）**：持续喂入向画面中心漂移的合成检测框，雷达 + 关联成立，输出速率式速度航向意图（vx 按加速度限幅渐变至接近速度上限， + 角速度随 β 收敛而减小）； +2. **Coast（4 秒）**：停止喂检测 → `is_predicted == true` → 清积分、 + 继续预测跟踪；同时取消雷达关联，演示 §6.3 无距离降级路径（hold： + 前向清零）； +3. **判丢刹车（3 秒）**：连续丢帧超过追踪阈值 → `tracked == false` → + 零速刹车。 + +示例将追踪库判丢阈值放大为 `kDemoMaxLostFrames = 60` 帧（默认 8 帧在 +20 Hz 下 Coast 仅 0.4 秒不便观察），**仅演示专用**，正式参数以追踪库 +规格为准。程序跑完总拍数后自行调用 `TrackingControlLaw::Reset()` 并 +正常退出（返回 0），无需手动终止。 + +### 5.3 单元测试（Google Test） + +测试源文件位于 `tests/`，覆盖 8 个测试套件： + +| 测试文件 | 覆盖要点 | +|----------|----------| +| `rate_limiter_test.cpp` | `Clamp` 边界；`SlewRateLimiter` 单步限幅、`dt<=0`/冻结保护、`Reset` | +| `staleness_checker_test.cpp` | 从未取样、时钟回拨（`now < sample`）、恰在有效期边界（临界判新鲜） | +| `frame_transform_test.cpp` | ψ=0/90° 基向量映射、合速度守恒（正交性）、`v_down` 不变 | +| `line_of_sight_test.cpp` | 零偏移、半幅=半视场、奇对称、单调性、与线性近似的偏离（证明未走线性支路）、构造期非法参数抛异常 | +| `pid_controller_test.cpp` | P/I/D 分量、积分钳位、输出限幅、抗饱和冻结、**误差反号回卷回归**（旧判据钉在 +5，条件积分到 -5）、微分低通、`dt<=0`、`ResetIntegral`/`Reset` | +| `tracking_control_law_test.cpp` | 姿态缺失/过期、视觉超龄、从未取样、姿态比视觉更严的优先级、判丢刹车、Coast 标志、速率/位置两种航向模式（含绝对航向回绕）、垂直支路与限幅、距离通道四种降级动作、雷达超龄回落、首拍变化率限幅（S6）、NED 旋转、`Reset` 后从零渐变 | +| `vtc_config_test.cpp` | 真实配置加载、position 模式可接受、缺文件/缺字段/缺分组/类型错/枚举非法/数值越界均抛异常且消息携带字段名 | +| `mavlink_command_gateway_test.cpp` | 手工解码 MAVLink v2 字节流（不含 `mavlink.h`，构建约束见 §3.6）：帧头、ID 84、三种掩码分支（含"双零 → 速率掩码"回归）、坐标基准字节、目标系统/组件字节、传输失败传播 | + +运行方式（构建后）： + +```bash +./build_vtc/vtc_unit_tests # 直接跑 +ctest --test-dir build_vtc --output-on-failure # 或经 ctest（gtest_discover_tests 注册） +``` + +依赖：系统安装 Google Test（`libgtest-dev`，FindGTest 模块模式，无需 +CMake config 包）；未找到时自动跳过测试构建并打印状态，不影响库与示例。 +`vtc_config_test` 通过编译期宏 `VTC_TEST_CONFIG` 读取真实配置文件 +`config/track_control.json`——**修改该配置文件时须同步核对测试断言**。 + +### 5.4 部署提示 + +`target_tracker` 为 SHARED 库（`libtarget_tracker.so.1`）：部署 +`track_control_demo` 或未来接入主工程时，必须将该 so 一并随带 +（目录式部署与 RPATH 约定见 `../target_tracker/target_tracker.md` §7）。 +本库 `visual_track_control` 自身为 STATIC 库，链接进可执行文件，无需随带。 + +--- + +## 六、排查 / 修改要点 + +### 6.1 配置缺字段 / 非法值报错定位 + +`LoadFromJson()` 缺字段、类型错误、非法取值均抛 `std::runtime_error`， +**异常消息携带完整字段名**（如 `配置缺失字段或类型错误: heading.gain`、 +`配置非法(必须为正): control.frequency_hz`、 +`配置非法: distance.no_distance_action 仅支持 hold / slow_approach / exit`）， +按异常文本直接定位到 `config/track_control.json` 的对应分组与键即可； +文件打开/解析失败则携带文件路径。`PixelToAngle` 构造期参数非法（尺寸 +非正、视场角不在 (0, π)）抛 `std::invalid_argument`，同样见消息文本。 + +### 6.2 MAVLink 消息待实测适配项 + +以下各项均以 **PX4 1.11.3 Offboard 实测为准**（规格 §10.1 待定项）， +全部集中在 `src/mavlink_command_gateway.cpp`，修改时只动这一个文件： + +| 适配项 | 位置 | 现状 | +|--------|------|------| +| `coordinate_frame` | `SendVelocityHeading()` / `SendBrakeHover()` 的 `mavlink_msg_set_position_target_local_ned_pack` 调用 | 固定取 `MAV_FRAME_LOCAL_NED`；速度分量已由主控律旋转为局部 NED 语义（§3.8 步骤⑧），语义是否匹配待实测确认 | +| 速度模式 `type_mask` | 匿名命名空间 `kMaskVelocityBase` / `kMaskVelocityYaw` / `kMaskVelocityYawRate` / `kMaskBrake` | 掩码位组合待实测确认 | + +### 6.3 接入主工程时需补齐的事项 + +1. **机体补偿（可选）**：机体系→NED 旋转已在库内完成（仅绕下轴按 + `yaw_rad`，规格 §2.3）；`roll_rad` / `pitch_rad` / `yaw_rate_radps` / + `velocity_*` 字段仍为预留，规格 §4.1 的机体补偿启用时由姿态源填充； +2. **`TrackedTarget` 主题接线**：`TrackResult` 尚未进入主工程 + `include/common/types.h`；需先按 `../target_tracker/target_tracker.md` + §7 与规格 §7.2 正式定义 `TrackedTarget` 消息与 `kTrackedTarget` 主题， + 由接线部件完成 `TrackResult` → `TrackedTarget` 转换与控制线程订阅； +3. **传输实现替换**：把示例的 `LoggingTransport` 换成真实链路实现 + （`ICommandTransport` 注入），打包与控制律零改动； +4. **雷达关联标志来源**：`associated_to_target` 必须由感知融合层按 + 规格 §6.3 / 需求分析 FR-041 的判据给出，控制律不自行关联。 + +### 6.4 红线声明 + +- `config/track_control.json` 与 `TrackingConfig` 默认值中**所有数值均为 + 占位值**（对应规格第 8 章待定项与需求分析 §7 未标定前提）：相机内参/ + 畸变/外参未标定、PID 与限幅参数未整定。标定与实测整定完成前，本库 + **仅限 SITL/台架录制数据回放验证数学链路，不得用于实飞**。 +- 构建环境差异：`../AGENTS.md` 规定 WSL2 Ubuntu 24.04，当前开发机仅 + 装有 Ubuntu 22.04，验证在 22.04 完成；安装 24.04 后需复核构建与测试。 diff --git a/utils/README.md b/utils/README.md new file mode 100644 index 0000000..074af5b --- /dev/null +++ b/utils/README.md @@ -0,0 +1,58 @@ +# utils —— reference-c++ 两个子项目的 Python 移植 + +将 `reference-c++/` 下的 `target_tracker`（多目标追踪过滤器）与 +`visual_track_control`（视觉跟踪控制律）移植为 Python，**两包互不依赖** +（唯一的数据转换点在胶水层 demo）。 + +## 目录结构 + +``` +utils/ +├── target_tracker/ # 包 1：卡尔曼滤波多目标追踪（无第三方硬依赖，仅 numpy） +│ └── README.md # 接口清单 + C++ 差异 +├── visual_track_control/ # 包 2：三通道控制律 + MAVSDK offboard 网关 +│ └── README.md # 接口清单 + C++ 差异 +├── demo_track_control.py # 胶水层集成演示（模拟检测 → 追踪 → 控制律 → 发送） +├── config/track_control.json # 控制律配置（拷贝自 reference-c++） +├── tests/ # pytest 测试（对应 C++ 全部 13 个测试文件） +│ ├── target_tracker/ # 27 条 +│ └── visual_track_control/ # 78 条 +├── conftest.py # 保证可从任意工作目录导入两包 +└── pytest.ini # 收集 *_test.py 命名（与 C++ 测试文件名对齐） +``` + +## 环境 + +conda env `pytorch`（`D:\program file\conda\envs\pytorch`），Python 3.12， +依赖：numpy、mavsdk、pytest。 + +## 运行测试 + +```bash +cd utils +"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q +``` + +105 条测试全部通过（含 C++ 验收用例的逐条移植：§9.4 六条硬验收、A1/A2/A4 +回归、PID anti-windup C1、超龄 C4、首拍限幅 S6）。 + +## 运行演示 + +```bash +# mock 传输（本地记录指令，无实机） +python utils/demo_track_control.py --speedup 10 + +# MAVSDK 实发（需飞控/SITL 在 udp://:14540） +python utils/demo_track_control.py --transport mavsdk +``` + +时间线 13 s（20 Hz）：正常跟踪 6 s（vx 从零渐变 → 逼近限速 3 m/s）→ +目标消失 4 s（Coast 预测，60 帧后轨道判亡）→ 判零速刹车 3 s。 + +## 移植约定 + +- MAVLink 部分改为 **MAVSDK 语义映射**（offboard API，非字节透传），三分支 + 映射保留 C++ A2 回归语义，详见包 2 README。 +- 测试逐条移植 C++ gtest 断言（`EXPECT_DOUBLE_EQ` → 精确 `==`， + `EXPECT_NEAR` → `pytest.approx`）；MAVSDK 部分因无 SITL/实机，仅 mock 验证。 +- 行为修复项（C1/C2/C4、A1/A2/A4、S6）在测试文件头注释中标注对应章节。 diff --git a/utils/__pycache__/conftest.cpython-312-pytest-9.1.1.pyc b/utils/__pycache__/conftest.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000000000000000000000000000000000000..a8a06ab6515fcae4d9eac9d214f42966876f9f79 GIT binary patch literal 739 zcmX@j%ge<81U2)fX9+MeFgylvV1NzE`1}OOn9h*G5XF$fn8K997{vspnR8i^nHU*z zS)_8dj9FAPhC{9KOCWchbRIb%Pb3q_W96}?LtD&4M31m?SJBk;mpD&dc zrazTEOAb?28Do($c9|?iOw}ojsAjHaf~a9+sAQ^S(qt*k0`dzgOHzwV6rL^E`Leb5 z`QCN!_sn{-f9{L*jnAepc(HERliht!C$D<4W7X5`OP}nW^L+NYr+cS9ov{9C*9rxQ z*7tjQp3ZH5+Q0tk`aMrumnxK&Wabp>D?HiJ{d`u-lO0PydV5+QH#EHj+3u&wc#9<< zu_QxN@D_V~d`f TrackState: + """两包之间的唯一转换点：TrackResult（box 为对象）→ TrackState（plain tuple）。""" + box = result.box + raw = result.raw + return TrackState( + tracked=result.tracked, + primary_id=result.primary_id, + center=result.center, + box=(box.x1, box.y1, box.x2, box.y2) if box is not None else None, + confidence=result.confidence, + is_predicted=result.is_predicted, + lost_frames=result.lost_frames, + n_active=result.n_active, + raw=(raw.x1, raw.y1, raw.x2, raw.y2) if raw is not None else None, + ) + + +def simulate_detections(tick: int, normal_ticks: int, period_s: float): + """场景检测源：正常阶段目标匀速横移，之后消失。""" + if tick >= normal_ticks: + return [] + cx = 960.0 + 3.0 * tick # 3 px/帧 匀速右移 + cy = 540.0 + w, h = 120.0, 160.0 + return [Detection(cx - w / 2.0, cy - h / 2.0, cx + w / 2.0, cy + h / 2.0, 0.9)] + + +def simulate_radar(tick: int, normal_ticks: int, period_s: float): + """场景雷达源：正常阶段关联成立、距离 30→10 m；之后未关联。""" + if tick >= normal_ticks: + return 0.0, False + distance = max(10.0, 30.0 - tick * period_s * 3.0) + return distance, True + + +class MockCommandSender: + """CommandSender 的 mock 实现：记录最近一条指令描述（无实机验证能力）。""" + + def __init__(self) -> None: + self.last_desc = "-" + + async def start(self) -> bool: + logger.info("[MockTransport] offboard 启动（模拟）") + return True + + async def send_velocity_heading(self, output: ControlOutput, + time_boot_ms=None) -> bool: + self.last_desc = (f"vx={output.vx_mps:+.2f} vy={output.vy_mps:+.2f} " + f"vz={output.vz_mps:+.2f} " + f"yaw_rate={output.yaw_rate_dps:+.2f}°/s " + f"yaw={output.yaw_deg:+.2f}°") + return True + + async def send_brake_hover(self, time_boot_ms=None) -> bool: + self.last_desc = "零速刹车（全零机体速度设定点）" + return True + + async def stop(self) -> None: + logger.info("[MockTransport] offboard 停止（模拟）") + + +async def make_sender(args) -> object: + if args.transport == "mock": + return MockCommandSender() + from mavsdk import System + drone = System() + logger.info("连接飞控 %s ...", args.conn) + await drone.connect(system_address=args.conn) + async for state in drone.core.connection_state(): + if state.is_connected: + logger.info("飞控已连接") + break + return MavsdkCommandGateway(drone) + + +async def dispatch(sender, out: ControlOutput, now_ms: int): + """按模式分发指令；DEGRADED_HOLD 不发送（禁止正常控制）。""" + if out.mode is ControlMode.VELOCITY_HEADING and out.valid: + return await sender.send_velocity_heading(out, now_ms) + if out.mode is ControlMode.BRAKE_HOVER and out.valid: + return await sender.send_brake_hover(now_ms) + return None + + +async def run_demo(args) -> None: + cfg = load_from_json(args.config) + period_s = 1.0 / cfg.control.frequency_hz + period_ms = int(round(period_s * 1000.0)) + + tracker = TargetTracker(TrackerConfig(max_lost_frames=_DEMO_MAX_LOST_FRAMES)) + law = TrackingControlLaw(cfg) + sender = await make_sender(args) + + normal_ticks = int(round(_NORMAL_SECS / period_s)) + coast_ticks = int(round(_COAST_SECS / period_s)) + lost_ticks = int(round(_LOST_SECS / period_s)) + total_ticks = normal_ticks + coast_ticks + lost_ticks + + if not await sender.start(): + logger.error("offboard 启动失败，退出") + return + + logger.info("开始 %d 拍（%.1f s，%.1f Hz）：正常 %.1fs → Coast %.1fs → 判丢 %.1fs", + total_ticks, total_ticks * period_s, cfg.control.frequency_hz, + _NORMAL_SECS, _COAST_SECS, _LOST_SECS) + try: + for tick in range(total_ticks): + now_ms = 1000 + tick * period_ms + detections = simulate_detections(tick, normal_ticks, period_s) + radar_m, associated = simulate_radar(tick, normal_ticks, period_s) + + result = tracker.update(detections) + snap = ControlSnapshot( + track=track_result_to_state(result), + track_time_ms=now_ms, # 每拍取样即打戳（视觉始终新鲜） + radar_distance_m=radar_m, + associated_to_target=associated, + radar_time_ms=now_ms if associated else 0, + attitude_present=True, + attitude_time_ms=now_ms, # 模拟姿态源持续可用 + yaw_rad=0.0, # 机头朝北（NED 旋转恒等） + ) + out = law.update(snap, now_ms) + await dispatch(sender, out, now_ms) + + if tick % _STATUS_EVERY_TICKS == 0: + logger.info( + "t=%5.2fs 检测=%d 追踪[trk=%d pred=%d lost=%d] | " + "[%s] vx=%+.2f vy=%+.2f vz=%+.2f yaw_rate=%+.2f coast=%d | 发送: %s", + (tick * period_s), len(detections), result.tracked, + result.is_predicted, result.lost_frames, out.mode.name, + out.vx_mps, out.vy_mps, out.vz_mps, out.yaw_rate_dps, + out.coast_active, sender.last_desc) + await asyncio.sleep(period_s / args.speedup) + finally: + await sender.stop() + logger.info("演示结束") + + +def main() -> None: + parser = argparse.ArgumentParser(description="视觉跟踪控制链路集成演示") + parser.add_argument("--transport", choices=["mock", "mavsdk"], default="mock", + help="指令发送方式：mock=本地记录，mavsdk=offboard 实发") + parser.add_argument("--conn", default="udp://:14540", + help="MAVSDK 连接串（仅 --transport mavsdk 生效）") + parser.add_argument("--speedup", type=float, default=1.0, + help="时间倍速（>1 加速演示）") + parser.add_argument("--config", default=str(_DEFAULT_CONFIG), + help="控制律配置文件路径") + args = parser.parse_args() + + logging.basicConfig(level=logging.INFO, + format="%(asctime)s.%(msecs)03d %(levelname)s %(message)s", + datefmt="%H:%M:%S") + asyncio.run(run_demo(args)) + + +if __name__ == "__main__": + main() diff --git a/utils/pytest.ini b/utils/pytest.ini new file mode 100644 index 0000000..eb6a8ff --- /dev/null +++ b/utils/pytest.ini @@ -0,0 +1,3 @@ +[pytest] +testpaths = tests +python_files = *_test.py test_*.py diff --git a/utils/target_tracker/README.md b/utils/target_tracker/README.md new file mode 100644 index 0000000..9344d6e --- /dev/null +++ b/utils/target_tracker/README.md @@ -0,0 +1,29 @@ +# target_tracker（Python 版） + +多目标追踪过滤器：8 维恒速卡尔曼滤波 + 贪婪关联 + 主目标锁定选举。 +移植自 `reference-c++/target_tracker/`（规格：docs/视觉追踪技术路线.md）。 + +## 接口清单 + +| 名称 | 说明 | +| --- | --- | +| `Detection(x1, y1, x2, y2, conf, cls_id=0)` | 单帧检测框输入 | +| `FrameShape(height, width)` | 画面尺寸（仅 Coast 兜底选举取画面中心用） | +| `TrackResult` | 每帧输出九字段：tracked / primary_id / center / box / confidence / is_predicted / lost_frames / n_active / raw | +| `TrackerConfig(max_lost_frames=8, max_association_dist=200.0, min_hits=3, dist_weight=0.5, iou_weight=0.5)` | 追踪器配置 | +| `TargetTracker(config=None)` | 追踪器主类 | +| `.update(detections, frame_shape=None) -> TrackResult` | 单帧主流程（§7 六步） | +| `.last_result() -> TrackResult` | 跨线程读取最近一帧结果（加锁拷贝） | +| `.reset()` / `.active_count()` / `.active_tracks()` / `.get_config()` | 生命周期与查询 | +| `KalmanBoxFilter` / `BoxCXCYWH` | 单目标滤波器（可独立使用：initiate / predict / update） | +| `Tracklet` / `BoxXYXY` / `to_center_size` / `to_xyxy` / `compute_iou` | 轨道与几何工具 | + +## 与 C++ 版的差异 + +- **依赖**：矩阵运算用 numpy（C++ 手写定长运算避免 Eigen）；关联/选举路径用纯标量，与 C++ 逐位同序。 +- **Optional 语义**：C++ `std::optional` → Python `None`；`State()` → `state` 属性（返回拷贝）。 +- **线程模型**：与 C++ 一致——仅 `last_result()` 加 `threading.Lock`，`update()` 单线程调用。 +- **字段命名**：成员函数蛇形命名（`IsDead` → `is_dead` 属性，`MarkMiss` → `mark_miss`）。 +- 行为语义（死亡判定 `lost_count > max_lost` 严格大于、best_cost=1.0 严格小于、 + ID 严格递增不复用、Q 用状态原值 / R 用 abs×2 的刻意不对称、S 求逆前 1e-9 正则化） + 与 C++ 完全一致，由 27 条移植测试锁定。 diff --git a/utils/target_tracker/__init__.py b/utils/target_tracker/__init__.py new file mode 100644 index 0000000..1f020b8 --- /dev/null +++ b/utils/target_tracker/__init__.py @@ -0,0 +1,15 @@ +"""target_tracker —— 轻量级多目标追踪过滤器（C++ 版 reference-c++/target_tracker 的 Python 移植）。 + +纯算法包，零第三方运行依赖之外的约束：仅依赖 numpy，不依赖 visual_track_control。 +""" + +from .kalman_box_filter import STD_POS, STD_VEL, INI_POS, INI_VEL, R_SCALE, BoxCXCYWH, KalmanBoxFilter +from .target_tracker import Detection, FrameShape, TargetTracker, TrackerConfig, TrackResult +from .tracklet import BoxXYXY, Tracklet, compute_iou, to_center_size, to_xyxy + +__all__ = [ + "STD_POS", "STD_VEL", "INI_POS", "INI_VEL", "R_SCALE", + "BoxCXCYWH", "BoxXYXY", "KalmanBoxFilter", "Tracklet", + "Detection", "FrameShape", "TargetTracker", "TrackerConfig", "TrackResult", + "compute_iou", "to_center_size", "to_xyxy", +] diff --git a/utils/target_tracker/__pycache__/__init__.cpython-312.pyc b/utils/target_tracker/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..92aaace4c0d7800f904a5384ffb4fa5aafb2c7ad GIT binary patch literal 973 zcmaiy&rcIU6vt=#gY6GmD4+-9q1UEC@5X4fg-8S>lqgJ^OxEqRtnBPI+o@?ip^*R) zq6dWtA^u1sY6u>T256N(z>^1JvKJC*yX91sgNY|+T0CH4oNPXM@6GIc-}mx9`FtM0 z&fxOm4Tl|oJ?{KDwnO7UWyT^%002z)TLv7=AZF{;}EL0x9te0jY z;c(EJoe3H^i4CmDxLXc~PydzATDlt?%F}UO3tFqiYGJ-!n%$ec;sj>f1+^ zxv6?-Zh!f`wYFB?n5`~s*0vwip3Rj%uGHQwlsE5H3JWZYwe_O9@}yo`Di^1kS3xaH z(s}l8d1L-Ca#P7pvFYZ_O3`c1KKu&){6Mk6we zVv!5|J*NAjo{Nk|#;#p5+b&Bgku(=aVd>W-$-Mg zBh7I#w`oLlEvckTZ^IqNnXF1#4v{4s9UC1pU5&Vksp*w9yS!jT7wkZ(tlqu>b%7 literal 0 HcmV?d00001 diff --git a/utils/target_tracker/__pycache__/kalman_box_filter.cpython-312.pyc b/utils/target_tracker/__pycache__/kalman_box_filter.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..f01aa7502de9204a981bd2cb2cc28a9ee449dd9d GIT binary patch literal 6297 zcmb7IeNaCge`I8sC@hyEHH$MT>KnO{Alk%p;tQxmD2^(pIGSVJ^iigqp|Q@o z*>Vl1Dc5q^avi5D*K_)E13ZnKq1?n!yr$-{RE0{NGxC;R(^W=km1oQ-XTC}+GJFQi zWj@1jX3ltqsbn?+8%E7RZc>kxXBeu4;w+~rE<@#BRTNO=nR4G8=(DPQ<#T<1zKm9)5*8NT3 zq{$S&{&D=)1^bp2E9~)>lcRxC@oT>u`E{S2@RdB_Jr#Um#flZHL>H;zMW;wy6-RjT z@OE&t>-2E@?T5GD=e!lds?n~qqg|oVyLU$W|2X_^VEFR&(VN#tZrxpZl(T={^~}TD zA&@kUy!+?jzzO^Dw>G?Of6@L{#qk1rg|EQgP+&h?V6TT(J+$hfS%3KLhqup-cAXsl zsM+p+>zPNyv6~Y?$r^jSzYF&kAoSpqo3L`!K8C07 zpd};c5?vK;mmri@{WAMn$=Ci1n-UdqQ&r-MUQSdFyj*qhi>F7;E9bs6W`MpPTGDc& zua+OvK&v7lEkD>qW8xL4L@1srr@=^>H^4N@S&k{!@LFESv1l&P(#tJPqGga<+CJr%bQyc?B8DZD)2_{+<*%oyAvM$^y86>=Y~7Z!r?sV{~&(%)X2$? z?O+4f?u=cH!nuuf-h#788jW%pBR#jqA~%#n83A>2k(i3(@b^h{gG3)Lb(kcB)9G;? z<(*E+O3Tb#~gbuM?JNAGlk={TLl3`!(zrQ7QgC9Biv@_4+WORV;K1n9It zC&@rE2S^V!0hIc2`_|_V?k3(U;ySt&F63Z^m*brr@p|||kr%{+bz-$!I5^d24<2#3 zkGee0L*C=g%4)aBla;kTVno$Qo`ApL0CI-qbW|j2P8s?$-bHd#>r9%!qK9-SE`p$v8a)9s>VFqcy@;INv)g)w5ixl`6!3B zlwc=8UJs+xk+IF8lNe8}r1kT}E08Q;NsJSW(pd{uSM8}5t6lDDKhJ>+Y6RY0Dd5iR zWF88Fv7&_2>3=-!kXEWa`5-5(1QMX`+p;6eT9-xFbQa&UEe~qW=~d3U-kf{3Rmd57 zA~&e{mtnqaAyW<*r^il>$Q|h2eE_9H^KvGw0PT7rrVfunoIty?h zI!%E3%9{11)gH6j2MqQ{*MwIquW21rW?EBC@=`%4QB?EA_0~NPOaWCICM<2oucyQLx2(9#%c zZ2r}N;YoS7678=@wO`B=u!fgODp1@F`6x!A=4QLhW@} z$dmAtv@SwiJ{g+RAUJVPT(zr8((fqU;oQBeOsVWE-bw7J3GEgxbP3j%0}=4>?W(sa zCJNX2h_|Ud&*GSaKmyc*gtrx4+1j?XJ+~{bBd?pj&i1g~uU{|gDeGI(C-iUmENft4 z@t|#+>WulV`Ipy69qq2R!rt}Wjy_jUVQj(rLE8q!H|;a{CPo3-EKUNQ$qxkDhhCd% zPB07$N6!b^M9B>PAhY=I80> zwCFTHbrwZhnQPMhroVkZKnS=UqQ6|G$Rl>86_lTpneNp?)+nzR^d@ggL?e@nXjN;| zuZYv3X!2xdlh*$%Y1tEgvToTkROzb;u!WdnC>=vL8$+KYn4ZaDU&bjSci?e4GL$fh z(L;tVf^kpMm+jrEAQM&)nVv!cIWf?=h|5#OLvpZ9#w&^5D`#;IodQ|OY*W$;BIm62 z3QDD(cT4m!0WsPx7bShr*Akwqt`Pn6(!*1t;{_NX?19Cqf%u>Q8d@7#3lcD_Hal%y z*&W$2+tO}2W^>4$*2gdBM8zu&Z4K?m+Wdp_m-iO;<=xExFuyd5oP0AQ{D$vVgrr|Yd#bhk6nFf3T1hx22fomP?Hwnm-JLV{kh3^tFt(5oDs6#kx z(i^Es=%4Nlr3Sq!tu*SzB&?;ZaJp|Ota`?{$JDH)$uQ$esWXhYK%FwNS_492lQCsB z#Ei58xflXLS^=&-jRj5ME}oW~_b7Mrd}pvGIM)S~AAq{aG`&QlvC)J(z!LySPug_K zex81GUQQz}Q@Op`WNQgWu{nL>nBfZ4B4hc#Us^8-* z+fuZ%m|()FfPW#@>_8-)yV~P&S4nKOr(V)I%ba_POC*-}@sfFGwWr8kRZNJNNP6dX z=ayYN%jB>zZN8ff9FQ9?LG{a#Oa=*tG)N3NEYU=w1xYKw%VfPoACYLE#2h&+pi?U0 zA~k5}>T2Oj67PR3J!UBU-7rWvl@>BAHh|fT%&;b+Z`DWXR#Wq?AbsCrO=DzaN9oHY zZRS`8q{_V|Jy3Zj$lRZS4WR);(4MF*x~JVXkexSZSsJ9_MJf@SHiusiZ;mq2xlty( zEwn4@idhyxKf@Zc?q@$1+0nWq@>=U_?YW(mz2tg*Pko>3sxS8RhS=f_gW1moi^oj~ zMeh||Xb8UogWH$JEcrniw_=r}k}JZsa#1zGU(&Ip{aBaKAq?cK7_=00i`N@^8u~~- z-Tlj$<#~B!S@R;0;YW+w>9*ol`F$Id&B<(eGxTP-65h%)mIs-k?3^I;?wa7C;4%0= z8#3O{%89INT^G&$&2v%G`cnJeuKgYRV>Sp@=k@>nei4;vYw?AAUt05G*1Yc^v}FC; zVjE}S`J;fZR-fl>+_skb+uA2fve>`dXrSrxsU5xgrFp2nE{ohegO>{$T+@OI$j&TYe-I@t}j9ap_ zo1*jE_r~TsCMc+nFJGiBj2g!&c(ilh<3m{#uE${`cr21R`8|{u2u4&%D}Q)ZyixW` z{Hj1!B3XsxWh7gWY(;{xgKPi-p5v~r;K?>*;H#Lj@2O`Y88#|k6JH|xKm-#&Ap!($ zil)D1w6x|wb0~W8H7%51C|=fi)`sfFB1+~%C7r?Hd#g~04#>DhC3zi+M{ zwxY1@?%ADlGZ|0ld_s!@x27{J@u<3=7j+G9I`ZHz{K@m^PDQSutp;*dE zu{0~|q@6T*%A7KI$~)y$??iOlJoT>+^;TEbXPR#;p z_AOL%oN5GW&Mj06X_E$L+1VaqfpV z&K;9lGW*of?28v}ynEsL`&OZI_jkjOI zF};a1q}3r4HPD7OH@xK)6`ppklZ7(%&R$=8Pj_3VsNTgr+=nB46U0V2Xi>AFr@whm z^WF!yI2cg{nLT^=>=o5SV4a*#)VX`Qdi#8wtG%aB%<}cP++4Sh<6YkNLCzhYmE<9h zXXZSR7@;@{Rt4phJpgOKDQ9I)hEs4#R=x_TkW-Ny6VIthP7%*(NKP5gX-Q5+a%!iJ z(?cy9%nfKAI#JWY`8YRru5S~R!Kh!Ge0BDfq1ktyo;~wJBA)u%di#xE{&?nzpUph+ z`1QBPKu{0$uc#SVQPW>vGf-dS?&qQ2c zd0dpDQhKG;VGz|WS9e<%=W>Z!m#eFX?d!y}-sO6@udOpqQMp_&J6$f`h%YNVojq+n zG27*8>+bIHwP6RmK+FLmZ^Fookp-h%h(vjNx9>bP2TAHL8(Z$)cOTz#kZJ z_pqFc<$JohWj@a9+t=r7@AU3lxO&pEwY>v;7FLcCe#(m@`ULeS=Dw>&^U#KX;_;o) zZ%LSPhgt%aVJ0_fG7oJEG#}l`FG^IGc~;Z{`EQvO0}6*y)NX=4a=Q+-^>Tf?pn~fs zKe+kQ%QF-2&s>-U*&gEB4<7Q>*pIZczC+Ryg!N=^?t#?=tM|p3_n*5tbnM#XFK0*o z36|Tn_n)0P@e`?zYm;xxTzVW9PF$E$Ric7a)#_05dDxwNjI0>hFe<~S2%|!b$}uX& zi1fV#Q>BRrb6$KO2X+5dX_#P8b4)`U0}V%8rFW1$^LV`ul>cVD7+6JWPJ>N@H6g7Y zrBln{8qs@X@p5V9Fir#TX2r|YWHo7=*<>|oNlpfP3$9Qd$?2UroCb1+c+Ny}StQ3e z&GYKY#=9#XIn*WFW=tP#t*=HbjbK@sE0(b;E3iCDEyB6Fapr&E$}6@DDIeM#y|kgV|a z^>%W60hR(npm+^N*j>IFqZ*8sVT4mYeSTD88Ae#O_YsJ2eyqG|u?=kw><%-wsJ&un zYfwFE9yWxTifCC4EU%5h%F*0lO-Nr7W@@6vr9;gDYp^P$DF`#AQEMUaE5b}+)V647 zd$1zREQ;n^hqeXHVa6J@~j-jzBJXYu;2V( z^v0WKXNE4q3cm5=tBD;S;lxcfye(#Twe`E^3H%@wLTFznT95+UVUN3==vf!r?)BZ2 z15s3WwRgJ?wfnqr?yGA>9a3FKh-MMBu<6Cqtqy(0DYFvlnWq!(z$}>?cVemuBkW!J zJgCPqj2hwRMK+uV%~uPHhIR&bg_)vgS=AI{9R(d*HBX4PA`omYk|nWEGlh~Qv8n}< zWS{`U+z%j{FG(52$vNhA`ZWXq5S$8*;gp<;Q%Nd=s0cMH!)1L7uLgKkaSpPT-@>7t z$3eDtqR36I^(g^J8UnCKHp_Y10P?szurz!40a4Y}=5rr{)oSkRmXfRl|3DXb#oSaO z%lTaG-LTj>Z>vL>E*R{)$aI1r`PGo*mt$0f5z*UgFtrw=H06B{mSMCWeqO{AAe73b zlPjky*WvPnWhfurHL4!23mMA7%(~Cwv?(Tk)H3QmWy7+nn#tYMRU4)l+vv(M`Ki06 znGKSNu;K+GQb7TRxqpQyB_h_y%1}f&%&eTHJq*hn1#1Ep5R`gktl}t>vl3jpjiprH~c!DmRM z=GB^z1m!~jMnZ=vkxD)1y+N!*qzq8!Wvyhtfq96lP*l0%eO0@I!VqxP-?095v0JeFkU)1;x@mw3*-hEJHw!;hfUdmo@bpbl>2awNr zxlRw?3{CLjxY+GHVh{wU?cMD@muoO@flO)><@e$n7!6U;(yFP_hT%sp7JpC@V2EriWeBdAjvH&2I(*Jnn;s7hjY*LV7l6S=a|@q`!lldWeRotJ&rg!ZBe= zE$PMoNh5efn*45z9>9uIO%SzU%EqNTXkFNn6y>}4lGK{$!q$jd&|8wV;uUq$OzfyA_b+mt)>MiO-}q22}-fMX~CmtNiN*PUc<`-`LOKxPKNR`emTqn zG(XZpM{)_K2#JN)LhXlT=~oDf4m4WQLV}B?j$0T?V0aUJ0j;OBATWvc_?6wud z@S(0G;WjP9-6Jg|zL}`;b$KG?R|?7w+;q|c^ub3_`(TOsRerU=2x|N+$O>002{4}j zCvti%UaMb|xqkxL**i)T?MOWZO=cNGX<3RZk$NpY>p%{VfE<(^6$wh}38U~m9D&R& z>8MI^CQ^bbIkF)CA^LbJ%mM`9X(1sIngaULG$LfZ^zk+D)tXx=g6c3Jd|pL147C49 z$lO9fwVFD9fb*O2%+^ac@YS`f5jgWBs9gqRlf!Hc72(o1Vb-;nuriD9TTM~fg zO0GN>zyeDp9Gpnq1-g`mpI?52DyLS!>^TDYz3`?)?U(PRj?fOKl}8M*V7tN9hT}em zKyn-Kqb}bMi``GBpiM^-!T?h|2uW=I6MBS(RaD3T%y>h7geF@PZuJ0;4~gn_uLrkw z4oYOT%SHN;1slI+)j-4UvO)?&U7I;x--Jt$`h?+9( z2`zhYoIbg7WcBdsv7Csx9P;0Iv15;wl+q5GM=0&E^IPE|%G)|SiFYfb7YhM~sC=ZY zvk&0CDsHKZxkPQ{Org)oS(hN~CC8aS$)V5F6w$b}T>P5d(+rH3JMT_U;@*?eL zg_XhPE7_)yY0tR(g6F(v!u_u2EzhL;lIK0oW%tLP4?Ul_e|zZHhd`oX)1K+;$&F>Mzukz9?$3 zMl7`P#;-$e`wwP;dQ&h_w1gwJP_CjZ7!=iSA4cO zq%RI^2_B4^%Es;tnW_WLSMrKS-KRaLJY(*2o->~LjSlCn4s4FB}N| zM@a7&ubOChx9P2>$z7M6?>T>`-x5<%Wp&VM+oFhd#k6%rU<;h@~QAsTkW6 zw$wrwvMm9B)M&-7>^!z}T3-w$C;U(QqvrgGd1=VJbo|h#=Ei7keXwQPQXQ{i%o?&R zjpb0b`dA)iEso^ZPv_S|t5WSRSn_)Qg!$dPxAG>!U`?h+)_;`u*|d3tP@_0kL$v)q#cjVSU6{6EfD!DP+~PffmwR%jx1% zFjiGCZUz|nr*?p(^^2a}5i?NMhM0*mDb0cF&X6EReT4HZ%Q;y?>@ z?Q1!eRe3c#H(>bY%ZF$X>et_;6D5D~qKWhTdeb(V`IkENHl_SGO2sy#YJn0%Xh^{5 z-$CMbz?~>kDw8U6%hFZqu3OaT03|8WxN=La!CT{G)45%6h_1lfr^r0QN{m|kT1Ws+Dgca^iu8o&eAu4W3Q4-CE{y?JsZW($7=qt`Q zXkIwzr0~b^k{1;#SYK-W3H>pBL|qV47fh?cOC{Nl3B$2$F)*GFM}@a%ndfg8zngx1`9K` zSR$UD-(+*J1=V6WyL3t|d4d=CV{rJ%5VJ5vpIk7=iRtZ!lWt#fxLKJmIhlQ^F-SXm8EJ-q%xUyse1{er`hnu#R>k@^UXW4D|Gnj>}QZV8O<69_Ge~yqNs1L(B^2&nr z@dhCWOh&C=1L?cKtkJNV!}~xXR|v9><%zLKJv)*{U8+n_3K~z+@PRuf2AHelv>fVu z0_w?NRAsIwp*U$+KgVBY=6Oa1MS=$IChZB6TTUEK(69LVFbO7D85%(gt{y!@x#fP7 zUn|I5$(bw2;${`09HAUpt$Yu3f^UIn0sKHUWQN~d5TfVk5$c${15)Whpl}gq(*Iy< z!C*@vqn|5jYk_Zy?lz_rStkFNj`GU{nGWnY4|GA=Q52GHUJBpfL0S~QA0j-j_c$1- zSL7TJC#B@lL{$lhe<<#r_4e>S7g#xicPv!j87-rf&6arW9MtAr1<}6)+A-yg)C|{* z_J>SMBBq9rsR3|x!J^a4PAwakPZ!h-H@v+P5%Yv2&=SqH1vid1O_?jE^c7^=f&&>j zv}4mjs0>1E?alv$a9zq#>xhq=Sh)xPpMC z;V2d{2hU-IQ(n|?-F;mg5BFpwTM;p+WH6$f$q0&yLj%1%aP@>>y2Pn3oZ?BA8y>NV zYC^-4VBD>wGgx#$NQZ5B>LV)KdV9HUmRO0z4s@93*>qlHehm4vQbWlRlOC-$Fy-10 zc0<$2oa{m#%>lSr0l>iP6V)V%#vsk|hslTVC3nB5B+mg6c5L9&bocOGj;DnWyQ_^;%YhjS_e%9vWNseI83Mx3Vd zY~Sk-yxlzR3{^G#8|J^#Qh7zAEvL7i+8!xd7Ajg6E~=Z@Go8C8pp9CKgGbJq$DFT} zgz}dM^w+FqNehaYKY@)`t%ZSmqxrVLmMaD2V_PEyb;HUa9c+l2OUCFQt&LiWq6NjL z7oS=@)^M)zOyjA#@m(R?@}M$v>6QG#k-_1?vC2qUL#V7_VtuHraWXGdwieDl(~IWh zv~@ilWUdz3&o+$t&IxCPNY$25)s{~x!sRDN6zhtLj_zJN9fz2 z?O*pD@L;FDqlWLO_ydA!-n-z%CT+}cx&m!7rneh*PSnC_0ig$csfzSyDfCDKb!xma z0YEZ96FGs+azO*WZh)NRdzpZjJQEN;3>!VvT`Va5^5b>}E(H3S?w18MKO)flB7x2X zjafOq#b@7!${^ST0H*+`A=QxqNYASy6E#tX(y#O39!5eYm<{k<4gChMGwF9G$W6va z&?4OP8{iBi6Aw9W5p;wnOE3WZhIJ_b>`ns2Y$03F6M#q`ZxXWMyEtO2VZ_uhVj0#N z`0?%WhR=H}I2>046hq^cgxm1TT*(;$92wdUb>Ie;r~_tPdH{~(*&Ec6X>Q0|$yEgB zU@}({&I5l2_zWCLsln+U_Fm>n$_X_MyjP#z4j9oiuDC~Ec_iW6UyyQWTEVfs464~A zL7pcd8OKDvb@4>^I)a3DI||8xJ%Kh7(%A;wJaUOTa74TDDk2=*^9Qhm!K+3Rh#|Lw z2xctD7s@b#1xN9ZVss26fXC1s_N?mJoD911^a>r&&>*XtT)l!bLd9OBa6olc@nwFJU0O zDdl}wl!2F9#=`9#a&^l4uopE)O$Sg0i_>t;LLdBZfyMg+Kr{$wiq5*nD=yTW zuL&={b2_I9unp{XmJ`BLLUi#`bR3mO&9=afn3{sDGMZl$&_{LI5nWkGS2m@qUFiSa z9jROus$3PetR`OIscbu3nFDZAQV}Ux9x7QL)I;rh)3G(t@~U&I&a8@*H-^d^r^}nd z`K_VyrqQ;`^v8x@8J?%7%9{dPrt({F<=aa5Cf8ru@xhKH-_}26=YOH3Dr&;{JI_Eh z;}2hWC$cL>LyvJdwqBh8Eu$&c8EST6RhasHI>LwAx-dtp6;>_NT`6mk&oaJs8^b;FR;B zPd7a@dQYUVCRA86?)|i|eyYtARDX8ohRffNY~CB%ymxBf{!cgWN0w!w!etZkPYWBS z+=qhdD<$Qp`%m=;^?$IH!k29Y=!717IAn8-Zwc8}1~+6XT4W!qKXq_iHvXOSs?egk z;O1!lqOqcI{<3IQ?WkUAtbr;hi)B&vieVh->bf8^ZLEss7Y7Z|#Wm-8&h!w80;yn8 zKzTeXT2yv==c%2Da%@p6IQ5r?tRQ8$kCIzB;u>~AQVTp1j+M9;0exCs7%eW36xW7| zYcW^wS+OIyn=qpz98NBlH%96=#T-81G(njalhKNzQNx!MJm%zyf|wFZcg$fa zEVY$QU&vP~^2V&=j!@~!7zOEQQN@@uRJ1&Xgy^cxbNGyP(lSNm=tFZ9Jm%CYMQzMP zD@sS*SWzifS~lt%YaX|b?Fg0BV_{iLS*^&MP*2)I%bQ~qq^Gtx=kOVG(rXlX<4qG= zL)B|z6r?Y2`wAb@-8$@}BTu@eP1NWPD1f`1iN8W{ULGMS&tS3AH_(eZnTQ4lU(7k& z*4fq8jmLB@PkSfWro0-d%ha?bMnMEZ zk?CS2n@0!?ZH`e8#R-_rA_N^>7Na1F6EIsu2r9Y=Z)_kWt*&JPK|Sh)HY`LIQ+_5aMBa6}l4$%aVI1gY6NC zAr9E)VLJgVLlVm^iS5wFW|9VDAkh42XWAdQq*KieouP?Omj4trZYMMS)!pxOCz*(r z_M5r=_VK-LcfZ}e`+at{g@QKseD8kBPEp?=p*B+znK=tcFU3){6i0J9A6>7j)zOIS zefoMstpRWYXY?8CO|_c$3G#S=tP> zSwNeBW`MQ=ZQ*Pn&n9E69ut=XJR4ch4(sKr>(ypA=Q;Bm{{YLzuMbThe)rzB_8o%D zUFYM2_pWtJj-8VG$EOBbu2Q4AIfiE{-*Va#UhVfk;hLvs>{k8a@Ub~>s#_C zA55JZbqKtN7kIy$f81SGRuOawd-)(6#1(m=!s~bY8ae)cHW9;y<@+qKeSEMj7P)rs z+Bq)Z7AxX|N8^JX@f+9UV;{#lTVwBy#4nFbjo&D*=Nw-St}b8YsDOS=vaf_QxZkY0+XYH6^e?p3f}t)A1>8h9hy&S6S0A#pbh!0uBp!=^84Mf$gz9ZPRN%(njGt_ zkh?ye96KH#d?!9IG<9KIdFv@ygEUFkv{KSF!_%}1`c+$<7Kve5zpI{SS;@k(^#QKY zhq#Spf7a;oB_(E-1)a03V1;EQqsJF;1tmMny8Qk?(1p4aL1+V^kc}V*z%Vrfkos3u z&6B%d76N+(SA7lWe78Hm@hm3<{QToVUJUMT40?Uy?%DpZTQht^vyg==BEY3ZCxEx9 zzZy2*%`Iqsw%z#Fw&;V9wrxU@I#<%~2{cJ2fe$tcem9y~ZDv5wjx%2a=%s?1EJW>3 z-Ys<~_P{ly(+b`K9Y?o-dO)t;?6hIPWzxtbI%`&jRWqNU1!79(^)l~`<1Z#xe}x*> zZB^Z2gVQMBS&3B@!x0k_>(N3nh(y$eLzoLt$_WNoH}8i#z>3}$URVT76o@|t&`M2O z3%lLDo*qwF>_5TC!;WuxVPLywq&MEHQ*|Rh3?)UV0 zdf142&@ z+Mc!H>i&&=8za?yFHRIZI#e)HG+Z=VdU@HUWn-n6$|juaqXk6|GYl&6KC5_^>cw-KXUf1;XXDup8D^az zllC&ct-8jn6J&V*%vw=rlM@(222C@@lx9MPkdfHO6w-zC823UZGTt0Ar;Rd&%osyo zfgIfU6Bq=kO+I#1OMi|R0GMY& zUw2$^tZ>BI&&pRmPosN3kKZ^gpZ~zI0*V0vR7N>XO5{W3le|UJn53`w`T@9_oQ$vx zMauz5CYQjwSXZ*&BbmJ<93E7WeUe4t;5fZeoZVe; zI%nC>_98IA`o=p)Yz)(q4PJu16Fg>)@z}U5s9eoZ$C~*p zj}B|yi)0ass&H=d{V?>+KbQN4i0?vWnyf`F=nt_E+NN70^2tx-_Ve=9adii9@=8bSS07G)e8I6s@%5?G zL(?Cfjs0>UK6oVd>*I3!`|{v%`NY|@bDB06anAYnJcVD9N$mU)p;aK$|EF3wTEOuJ9#<$MibWJ zP(8SlWI$gM95{aoGOcOuLuw3(QC&!~5lnC$WU<<2ynb)c>k9HtlVlKipGP^x4{@nA z2yhdj5`c(yP@7IrGS~E^A|v8 zj}||Q{OJ5egeoe5&KfN!gpP?8&Veo~x^ms+MVA)cT=3gPzgcu^#mnu6ubARHWvfP? zx?NTU1G4W{tR6WudGhZp>bS@!+v6tzv97T@oGt9I&YNY10T zI(5R6Fk5&Ez%VUTLpQ6m^NK9TySSEIO({vv795Gicq^4KQMrX(Z+5&HDI8oluyDd& zev2vp565-DtOp$zQnPY2o#IVSS~=fyxLw+(ffbJ74nSD(`uDaJrXNn_;x~d=03iK< zc@y>}x0qx;NL~*ZqUe$6@=G8dbh!Q6H79cjP(#8YIz%6?hI+-2lBrncDu!HKZFE{C zM5kR`EnI}CCVIbiN!69fHy~VYw=NY*n&C3SHSE14Svp7Xkcsaqvf0iWC^ zVFP053Gg%@NUru8aK+64;9l4Zx(;?6L@&5)hbt1b+HYAG zL>)^9%>(A4!jZYdb8jtP*jKgX{6@-miC$xTcWnyu1%er!gR-$Q=1ZI%2q<$ zhI|ORHPyfd7*mLOeyYUhjD>wB^8{1TS5vF1UK-+jrlK3TS|o;jlMPEqkyEbOM=YPND@ag5Jw|+ zP6cKWEsimnL}XzbP%U=tYAah~RuC50^v;f!ELNiK?C_lo?XXW) zXIA(2j+~QO7?+t&ru&(W>{HD5V;uS@@zY&^U>E_d19pD{KT_BjV= zyJ?s~=0A${#v|&|HddRL0KBWtnjmDpo~{g=6BKlj7iO?a*z|N&_dH}(g%2XDDq+i|OS|C@8Fb-; Vq#If@gVcA+e?%93O93FN_%|@PqKW_j literal 0 HcmV?d00001 diff --git a/utils/target_tracker/kalman_box_filter.py b/utils/target_tracker/kalman_box_filter.py new file mode 100644 index 0000000..3a9be8b --- /dev/null +++ b/utils/target_tracker/kalman_box_filter.py @@ -0,0 +1,111 @@ +"""单目标 8 维恒定速度卡尔曼滤波器。 + +对应 C++ 参考实现 reference-c++/target_tracker（规格：docs/视觉追踪技术路线.md §4）： +状态 x[8] = [cx, cy, w, h, vx, vy, vw, vh]，观测 z[4] = [cx, cy, w, h]，dt = 1 帧。 +矩阵运算用 numpy 表达（C++ 版为避免 Eigen 手写定长运算，此处等价）。 +""" + +from dataclasses import dataclass + +import numpy as np + +# §4.2 / §8.2 内部常量（经验值，针对 640×480 画面调优） +STD_POS = 1.0 / 50.0 # 位置噪声比例系数 +STD_VEL = 1.0 / 200.0 # 速度噪声比例系数 +INI_POS = 2.0 # 初始位置协方差 +INI_VEL = 100.0 # 初始速度协方差 +R_SCALE = 2.0 # R 额外缩放因子（增强平滑） +_S_REG = 1e-9 # S 求逆前的对角正则化（§9.3） + +# 状态转移矩阵 F（§4.1）：I4 + 右上 I4 +_F = np.eye(8, dtype=float) +_F[0:4, 4:8] = np.eye(4) + +# 观测矩阵 H（§4.1）：[I4 | 0]，直接观测位置与尺寸、不观测速度 +_H_COLS = slice(0, 4) + + +@dataclass +class BoxCXCYWH: + """轴对齐边界框（中心点 + 宽高表示，卡尔曼观测空间）。""" + + cx: float = 0.0 + cy: float = 0.0 + w: float = 0.0 + h: float = 0.0 + + +class KalmanBoxFilter: + def __init__(self) -> None: + self._x = np.zeros(8, dtype=float) + self._p = np.zeros((8, 8), dtype=float) + self._initialized = False + + @property + def initialized(self) -> bool: + return self._initialized + + @property + def state(self) -> np.ndarray: + """状态向量 [cx, cy, w, h, vx, vy, vw, vh]（拷贝）。""" + return self._x.copy() + + def initiate(self, z: BoxCXCYWH) -> None: + # §4.3：速度置零；P = diag(2,2,2,2,100,100,100,100) + self._x = np.array([z.cx, z.cy, z.w, z.h, 0.0, 0.0, 0.0, 0.0]) + self._p = np.diag([INI_POS] * 4 + [INI_VEL] * 4) + self._initialized = True + + def predict(self) -> BoxCXCYWH | None: + # §4.4：未初始化返回 None + if not self._initialized: + return None + + # §4.2 动态过程噪声：Q = diag(σ_pos², σ_vel²)，σ 按状态中 w/h 缩放。 + # Q 直接使用状态值、不做 abs，与 R 的 abs 处理刻意不同（规格 §4.2 注记）。 + w = self._x[2] + h = self._x[3] + std_pos = np.array([w * STD_POS, h * STD_POS, w * STD_POS, h * STD_POS]) + std_vel = np.array([w * STD_VEL, h * STD_VEL, w * STD_VEL, h * STD_VEL]) + q = np.zeros((8, 8), dtype=float) + q[np.arange(4), np.arange(4)] = std_pos * std_pos + q[np.arange(4) + 4, np.arange(4) + 4] = std_vel * std_vel + + # §4.4：x ← F·x；P ← F·P·Fᵀ + Q + self._x = _F @ self._x + self._p = _F @ self._p @ _F.T + q + return BoxCXCYWH(float(self._x[0]), float(self._x[1]), + float(self._x[2]), float(self._x[3])) + + def update(self, z: BoxCXCYWH) -> None: + # §4.5：未初始化直接返回 + if not self._initialized: + return + + # §4.2 动态观测噪声：R 用 abs(w)/abs(h) 缩放并额外乘 R_SCALE + w = abs(self._x[2]) + h = abs(self._x[3]) + rw = R_SCALE * (w * STD_POS) * (w * STD_POS) + rh = R_SCALE * (h * STD_POS) * (h * STD_POS) + r = np.diag([rw, rh, rw, rh]) + + # §4.5：S = H·P·Hᵀ + R。H = [I4|0]，故 H·P·Hᵀ 即 P 左上 4×4 子块 + s = self._p[:4, :4] + r + try: + s_inv = np.linalg.inv(s + _S_REG * np.eye(4)) + except np.linalg.LinAlgError: + return # 求逆失败：防御性跳过本次更新（C++ 版同样跳过） + + # §4.5：K = P·Hᵀ·S⁻¹，P·Hᵀ 即 P 的前 4 列（8×4） + k = self._p[:, _H_COLS] @ s_inv + + # 残差 y = z − H·x（H·x 即状态前 4 维） + y = np.array([z.cx, z.cy, z.w, z.h]) - self._x[:4] + + # x ← x + K·y + self._x = self._x + k @ y + + # P ← (I8 − K·H)·P：K·H 是 K 的前 4 列铺在前 4 列上 + kh = np.zeros((8, 8), dtype=float) + kh[:, :4] = k + self._p = (np.eye(8) - kh) @ self._p diff --git a/utils/target_tracker/target_tracker.py b/utils/target_tracker/target_tracker.py new file mode 100644 index 0000000..340e3fd --- /dev/null +++ b/utils/target_tracker/target_tracker.py @@ -0,0 +1,269 @@ +"""多目标追踪过滤器（TargetTracker）。 + +对应 C++ 参考实现 reference-c++/target_tracker/src/target_tracker.cpp +（规格：docs/视觉追踪技术路线.md §5 关联 / §6 生命周期与锁定 / §7 六步主流程）。 + +线程模型（§9.5）：update() 由单线程（推理主循环）调用；仅对 last_result +缓存加锁，last_result() 返回受保护拷贝。 +""" + +import math +import threading +from dataclasses import dataclass, field +from typing import Optional, Sequence + +from .kalman_box_filter import BoxCXCYWH +from .tracklet import BoxXYXY, Tracklet, compute_iou, to_center_size + + +@dataclass +class Detection: + """单个检测框（§2.2 输入元素：[x1,y1,x2,y2,conf,cls_id]）。""" + + x1: float = 0.0 + y1: float = 0.0 + x2: float = 0.0 + y2: float = 0.0 + conf: float = 0.0 + cls_id: int = 0 + + +@dataclass +class FrameShape: + """画面尺寸（height, width）：仅 Coast 兜底选举时用于取画面中心。""" + + height: int = 0 + width: int = 0 + + +@dataclass +class TrackResult: + """每帧追踪输出（§2.3 九字段）。""" + + tracked: bool = False + primary_id: Optional[int] = None + center: tuple[float, float] = (0.0, 0.0) + box: Optional[BoxXYXY] = None + confidence: float = 0.0 + is_predicted: bool = False + lost_frames: int = 0 + n_active: int = 0 + raw: Optional[BoxXYXY] = None + + +@dataclass +class TrackerConfig: + """追踪器配置参数（§8.1 默认值取自规格）。""" + + max_lost_frames: int = 8 + max_association_dist: float = 200.0 + min_hits: int = 3 + dist_weight: float = 0.5 + iou_weight: float = 0.5 + + +@dataclass +class _AssociationResult: + matches: list[tuple[int, int]] = field(default_factory=list) + unmatched_track_ids: list[int] = field(default_factory=list) + unmatched_det_indices: list[int] = field(default_factory=list) + + +class TargetTracker: + def __init__(self, config: Optional[TrackerConfig] = None) -> None: + self._config = config if config is not None else TrackerConfig() + self._next_id = 0 + self._tracks: dict[int, Tracklet] = {} + self._frame_count = 0 + self._locked_primary_id: Optional[int] = None + self._last_result = TrackResult() + self._last_result_mutex = threading.Lock() + + @property + def config(self) -> TrackerConfig: + return self._config + + def get_config(self) -> TrackerConfig: + return self._config + + def update(self, detections: Sequence[Detection], + frame_shape: Optional[FrameShape] = None) -> TrackResult: + # §7 六步主流程 + self._frame_count += 1 + + # 入口防御：过滤坐标或置信度含 NaN/Inf 的检测（非有限值一旦进入 + # 卡尔曼状态将永久污染轨道并传导到主目标输出） + valid_detections = [ + det for det in detections + if all(math.isfinite(v) for v in (det.x1, det.y1, det.x2, det.y2, det.conf)) + ] + + # 步骤 1：所有存活轨道先行预测（推进一帧） + for track in self._tracks.values(): + track.predict() + + # 步骤 2：贪婪关联（无检测时全部轨道进入 Coast 计数） + if valid_detections: + assoc = self._associate_detections(valid_detections) + else: + assoc = _AssociationResult(unmatched_track_ids=list(self._tracks.keys())) + + # 步骤 3：用匹配到的检测更新轨道 + for track_id, det_idx in assoc.matches: + det = valid_detections[det_idx] + raw_box = BoxXYXY(det.x1, det.y1, det.x2, det.y2) + self._tracks[track_id].update(raw_box, det.conf, det.cls_id) + + # 步骤 4：未匹配轨道计丢失，死亡者回收（锁定唯一解除途径之一） + for track_id in assoc.unmatched_track_ids: + track = self._tracks.get(track_id) + if track is None: + continue + track.mark_miss() + if track.is_dead: + if self._locked_primary_id == track_id: + self._locked_primary_id = None + del self._tracks[track_id] + + # 步骤 5：未匹配检测 → 创建新轨道（next_id 从 0 递增永不复用） + for det_idx in assoc.unmatched_det_indices: + det = valid_detections[det_idx] + measurement = to_center_size(BoxXYXY(det.x1, det.y1, det.x2, det.y2)) + self._tracks[self._next_id] = Tracklet( + self._next_id, measurement, det.conf, det.cls_id, + self._config.max_lost_frames) + self._next_id += 1 + + # 步骤 6：选取/延续主目标，组装输出并缓存 + result = self._select_primary(frame_shape) + with self._last_result_mutex: + self._last_result = result + return result + + def last_result(self) -> TrackResult: + with self._last_result_mutex: + return self._last_result + + def reset(self) -> None: + # §2.1：清空所有轨道、ID 计数器、锁定状态与缓存结果（next_id 归零） + self._tracks.clear() + self._next_id = 0 + self._frame_count = 0 + self._locked_primary_id = None + with self._last_result_mutex: + self._last_result = TrackResult() + + def active_count(self) -> int: + return len(self._tracks) + + def active_tracks(self) -> dict[int, Tracklet]: + return dict(self._tracks) + + def _associate_detections(self, detections: Sequence[Detection]) -> _AssociationResult: + result = _AssociationResult() + + # §5.3：按轨道 confidence 降序（平局按 ID 升序保证确定性）。 + # 非有限置信度按 −∞ 处理（排最后），避免 NaN 破坏全序。 + def sort_key(track_id: int) -> tuple[float, int]: + raw = self._tracks[track_id].confidence + conf = raw if math.isfinite(raw) else -math.inf + return (-conf, track_id) + + sorted_ids = sorted(self._tracks.keys(), key=sort_key) + det_assigned = [False] * len(detections) + + for track_id in sorted_ids: + track = self._tracks[track_id] + # §5.3：最优代价初始 1.0，仅 cost 严格小于才匹配（cost ≥ 1.0 判为不可匹配） + best_cost = 1.0 + best_det = -1 + for d, det in enumerate(detections): + if det_assigned[d]: + continue + det_cx = (det.x1 + det.x2) / 2.0 + det_cy = (det.y1 + det.y2) / 2.0 + dist = math.hypot(track.last_center[0] - det_cx, + track.last_center[1] - det_cy) + norm_dist = dist / max(self._config.max_association_dist, 1e-3) + iou = (compute_iou(track.last_box, + BoxXYXY(det.x1, det.y1, det.x2, det.y2)) + if track.last_box is not None else 0.0) + cost = (self._config.dist_weight * norm_dist + + self._config.iou_weight * (1.0 - iou)) + if cost < best_cost: + best_cost = cost + best_det = d + if best_det >= 0: + result.matches.append((track_id, best_det)) + det_assigned[best_det] = True + else: + result.unmatched_track_ids.append(track_id) + + result.unmatched_det_indices = [ + d for d, assigned in enumerate(det_assigned) if not assigned + ] + return result + + def _select_primary(self, frame_shape: Optional[FrameShape]) -> TrackResult: + result = TrackResult(n_active=len(self._tracks)) + + # §6.3：轨道表为空 → 清除锁定，返回 tracked=false + if not self._tracks: + self._locked_primary_id = None + return result + + # §6.3 ① 锁定延续：锁定轨道存活则直接输出（Coast 中照样输出， + # 不被高置信度目标抢占） + chosen: Optional[Tracklet] = None + is_predicted = False + if self._locked_primary_id is not None: + track = self._tracks.get(self._locked_primary_id) + if track is not None: + chosen = track + is_predicted = track.is_coasting + + # §6.3 ② 重新选举（无锁定 / 锁定目标已死亡被回收） + if chosen is None: + measured = [t for t in self._tracks.values() if not t.is_coasting] + coasting = [t for t in self._tracks.values() if t.is_coasting] + + if measured: + # confirmed ← measured 中 hits ≥ min_hits 者；为空则退回 measured + # （min_hits 只影响候选池优先级，不阻止输出） + pool = [t for t in measured if t.hits >= self._config.min_hits] + if not pool: + pool = measured + chosen = pool[0] + for t in pool: + if t.confidence > chosen.confidence: + chosen = t + is_predicted = False + elif coasting: + # 无实测轨道：选离画面中心最近者（缺省按 640×480 中心 (320,240)） + center_x = frame_shape.width / 2.0 if frame_shape and frame_shape.width > 0 else 320.0 + center_y = frame_shape.height / 2.0 if frame_shape and frame_shape.height > 0 else 240.0 + best_dist_sq = math.inf + for t in coasting: + dx = t.last_center[0] - center_x + dy = t.last_center[1] - center_y + dist_sq = dx * dx + dy * dy + if dist_sq < best_dist_sq: + best_dist_sq = dist_sq + chosen = t + is_predicted = True + else: + # 理论上不可达（轨道非空则 measured/coasting 必居其一）；防御清锁 + self._locked_primary_id = None + return result + self._locked_primary_id = chosen.track_id # 选举后锁定 + + # 组装输出（§2.3 九字段）；raw 仅实测时有值，Coast 时为空 + result.tracked = True + result.primary_id = chosen.track_id + result.center = chosen.last_center + result.box = chosen.last_box + result.confidence = chosen.confidence + result.is_predicted = is_predicted + result.lost_frames = chosen.lost_count + result.raw = None if is_predicted else chosen.last_raw_box + return result diff --git a/utils/target_tracker/tracklet.py b/utils/target_tracker/tracklet.py new file mode 100644 index 0000000..87c6b0f --- /dev/null +++ b/utils/target_tracker/tracklet.py @@ -0,0 +1,110 @@ +"""轨道（Tracklet）与基础几何工具。 + +对应 C++ 参考实现 reference-c++/target_tracker/include/target_tracker/tracklet.h +（规格：docs/视觉追踪技术路线.md §3.2 / §5.2 / §7）。 +""" + +from dataclasses import dataclass +from typing import Optional + +from .kalman_box_filter import BoxCXCYWH, KalmanBoxFilter + + +@dataclass +class BoxXYXY: + """轴对齐边界框（像素坐标，左上/右下角表示）。""" + + x1: float = 0.0 + y1: float = 0.0 + x2: float = 0.0 + y2: float = 0.0 + + +def to_center_size(box: BoxXYXY) -> BoxCXCYWH: + # §7 TRACKLET_UPDATE：cx=(x1+x2)/2；cy=(y1+y2)/2；w=x2−x1；h=y2−y1 + return BoxCXCYWH((box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0, + box.x2 - box.x1, box.y2 - box.y1) + + +def to_xyxy(box: BoxCXCYWH) -> BoxXYXY: + # §7 TRACKLET_PREDICT 的 center_size 逆变换 + return BoxXYXY(box.cx - box.w / 2.0, box.cy - box.h / 2.0, + box.cx + box.w / 2.0, box.cy + box.h / 2.0) + + +def compute_iou(a: BoxXYXY, b: BoxXYXY) -> float: + """标准轴对齐 IoU（§5.2）：任一框为空、交集 ≤ 0 或并集 ≤ 0 时返回 0。""" + area_a = (a.x2 - a.x1) * (a.y2 - a.y1) + area_b = (b.x2 - b.x1) * (b.y2 - b.y1) + if area_a <= 0.0 or area_b <= 0.0: + return 0.0 + + inter_w = min(a.x2, b.x2) - max(a.x1, b.x1) + inter_h = min(a.y2, b.y2) - max(a.y1, b.y1) + if inter_w <= 0.0 or inter_h <= 0.0: + return 0.0 + + inter_area = inter_w * inter_h + union_area = area_a + area_b - inter_area + if union_area <= 0.0: + return 0.0 + return inter_area / union_area + + +class Tracklet: + """单条轨道：生命周期计数 + 辅助状态 + 卡尔曼滤波器（§3.2）。 + + 构造即创建轨道（§7 步骤 5）：用首次观测初始化滤波器，age=1、hits=1、 + lost_count=0，last_center / last_box 立即由观测重建（创建当帧即可参与输出）。 + """ + + def __init__(self, track_id: int, measurement: BoxCXCYWH, conf: float, + cls_id: int, max_lost_frames: int) -> None: + self.track_id = track_id + self.kf = KalmanBoxFilter() + self.lost_count = 0 + self.max_lost = max_lost_frames + self.confidence = conf + self.cls_id = cls_id + self.age = 1 + self.hits = 1 + self.last_center: tuple[float, float] = (measurement.cx, measurement.cy) + self.last_box: Optional[BoxXYXY] = to_xyxy(measurement) + # 创建即源于实测检测：同步记录原始检测框（Coast 期间保留不更新） + self.last_raw_box: Optional[BoxXYXY] = self.last_box + self.kf.initiate(measurement) + + @property + def is_dead(self) -> bool: + # §6.1 注记 2：lost_count > max_lost（严格大于） + return self.lost_count > self.max_lost + + @property + def is_coasting(self) -> bool: + # §3.2：lost_count > 0 即处于纯预测状态 + return self.lost_count > 0 + + def predict(self) -> None: + # §7 TRACKLET_PREDICT：预测成功时更新 last_center / last_box 且 age+1 + pred = self.kf.predict() + if pred is not None: + self.last_center = (pred.cx, pred.cy) + self.last_box = to_xyxy(pred) + self.age += 1 + + def update(self, box: BoxXYXY, conf: float, cls_id: int) -> None: + # §7 TRACKLET_UPDATE：框 → [cx,cy,w,h] → kf.update + self.kf.update(to_center_size(box)) + self.last_center = ((box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0) + self.last_box = box + self.last_raw_box = box + self.confidence = conf + self.cls_id = cls_id + self.lost_count = 0 + self.hits += 1 + self.age += 1 + + def mark_miss(self) -> None: + # §7 步骤 4：未匹配计丢失 + self.lost_count += 1 + self.age += 1 diff --git a/utils/tests/target_tracker/__pycache__/kalman_box_filter_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/target_tracker/__pycache__/kalman_box_filter_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000000000000000000000000000000000000..0d7262b699875a66bb548fc64126f877fb862831 GIT binary patch literal 29617 zcmeHQeQ+E_cAr^|_FLN3x8x7J7;LlWv$3?Y{87l**yiw2F$Z!H@Y&g0@2rJ{w9?It zjHJv`;tmc8z?kJU5&JBPIhdwqurj@y{|u}`%Ta5_v<(M&$2SF0LSls^LnRNE(o7tgdGlp`1&V6 zye`PXenA#xN0+$YLCz)JQrsa5-KG1TBK^g+-$j0L?{~wwq|4Lo-S6%8?f1d&(u=}= zzg!Aone2qHTy{Yiklhdl2OMEf-zHHIo{e>N$CA-QJ^j(c9bH;NiE0T|Ti-hn`s2_E z_=FxM3=j46KTLiOT|D~Q#g~45>F-{>^8Ra|4gJjPy?pu~Fa7n=D=)tjQWA#~N+KCg ztc$N*d%qS_+7nt-Q)2OF6UzNqta|@E)x~>zv8*e<{l(=se)Yx2!&grK;>zifE1&$+ zm5<)J^vcksQ}18-`}Z&Z{*%zZo^Jl)*&D)pX_}feAmoJ_8@X~8bTSEGKf*aB>Y`srIdq3QfMczd zvL8F0j55!xv?GFAqj^|*Cy_Z8qi0=s*Jy)c<@9HBVa()AV*O^RcDy1hRW3Q@7=#w4 zO0)ng(~>vH(#*47!t%27&qP`17gHi!z0g@{axy9!3mg0-Lfy`(cQY|+56~IK|N`?M!WgVuZu9hm0=#s{8qkbPY^e|JM%t4 zI46u?xuw;1^OlE|DhJpTL=MUodWkU~c?+J8ysRX4X7f?XGy0As3<%-My@QdMswNaI zlc7Bh|x zH7pK3j2W9*2|ySn%u}eLTzFIWg}tdzhe~SO6jsBMg8G7jk6Nst8G}uOhX_!BQgMSc~3Bi{2P6 zSMZsb@ma1tFMi|qe`MziMDt3 z9Ex>Cld?TRI18vK%R8U2%~ zh(ax0q`-JjcW+EdWSo7yatzc})TuJ1ss?n}r=UiZ@nI8bS2bEA5gMTtHCumau|_sp z**4P)rg<7SD5!*3nVYQ4EvgTG4w=U{ZM(J}_>t1nuEe@`#kAOgc#oWj$_glv>#(~H z^l2Sk>H(q}AIR3156q(}_YP#%ke=>KT81TR^b~ZF)~6)ZXcDSW)H}~ZZ-!v_0XX?qT#)c{j>c((hf|Q+L}Sw?IRoTYntB$qadNzw=i~$w ze9bA8x?bM%!k*EN@zU|Wv^=qEy1a2lYPzGYa<(jR?5WYGhQ-G;z+$}oJRv=JK1k|ekJf_iBMkqH3)@}wLHNE0$HBagJM4K@&q|@4)bAx zixas-mTXlwo23daY+y-KDmyI?1e=8lQ}4-=ri@kJ35)04!(6Dq1IgsHTWavd4?K{Z zd3enwjmvl-IkYNPwDY659pY{kE7Y0d=^Rm1Le%lBp1ziy}}1{!I`L zz^x35v{Q%03u?_Nm+sP?L?v+RZuNJT_SmQrGAIzB5_mC>sS;T7*!2}x>t)}fjg*u1 zN8XX*oG?;8*>8-1$I|9yX3sGSl%CQzgg1Hb%jJXclN``JT!|=j1m{yCTq*bHze~6* zS;1;G&rD`hK3I~vb)W9;gJmqp`$|y7OK}ndg*W7E@(%O{yGB1%ACuFMqH0UBk?h z3h1I9AoosC4~~gyljSF`F1R_d@n1?_graJit=rz-&~)3gW)3gC_uhT7FoWR7v;Ms z-%JiIPnJJjzQ4yxHO~dex3tLeeOFPwi}KARaPj0D%%v;#8pcJ9Ss9az*u{u#+XwHr z*eU0+d|GT*-V`z0DK2Po*sW}UlhTMm69y3s(1?{ApuWg#n~mAFP3gHg17)fA0ULWi2ReB)r zeOUJ9u=@QF)YXcSMlk4t)i~p%me;V$G#M*shQzCH!C*5q9k@x@^i9BW{f$rqNp8;9 z`hgqNdZ<@jpx2)J|Eczw8&z#BHK@C(^c&V|1p!f|9s@M!D7f%cmSTX04P`k7cVU3* z+>9rY?CVY_X!;Ieyn-3d>9}^Q?F!l>GLCq^@&FP}DngJjkP)BDh|h$p=>2vN{z`1+ z)VdwXxWV|Ih-)C`q$;sPY34k%_zem^;*?bwti@mp25TU&8pStRBU`8io#tU`De5!bLd+&mko9(#PGf6~2fxPQEPDzM^$dmVhv z(KwAvV}z1CgbZNwCcN-7BOzhRy)NA{arZ=3dKqWt#I+Kkdd)Sr0Br%Q{HqTP?V7Dy zIw>t3ZXVMneM>J$_&G=8G{VCvOr^1Dlbn&3j%l2S6Tm#{3mn@tvT0IkK5_Wg2FJ8X!T=3#;yjdq5>Fha zBychkILEqa>RD_ECnuowSTLkQGbo)CLfv=V#k7pHOiE2Bo_zh_@q<95VIZ0&_Cav` zV7hr?-%L#t(8Dd0HBD1KkaPT;Hxg)!hf|OY{e~RVCJ7MBa0}<51eAEv+DdYG8(k(Xr^^C2%9$6azU7O*J`oqR(W6m1-q z9DuLjcAmMYjD`vqIb;sG1I7T41+v|WZ}VbYg21fWWHS!x7&!-Q#WJ>(iNfeAVjH&s z_yP4Ok-Y@FkZ=nmJvwddf3S&5?6lkV<%F#eF0kj8>Q3GDo}b%u-GGX?i9Oe&dx$+3 zRW)+h;Vgg>c)*@pj=4;Gu4d-JhjPxLo7<%V8a}1T(P@K0u-*dPs=-nwKFV_rvlm$Z zfwgJDR?N)-*3`hldGu0Nx*ROH5m0DW4yS3z?@JL%GYKD3}K!N7g4Tu6oBHMp`5|4j)5L)tMg2&-VQ~!!a=n*>nNEzc_h}eMZ zhgl_fcmTc^cIHY2n$#5B-Atgn#=$H^k0O-=jghNp6%5x9z==3XC_6DrsS-=JCkRBO zpv{9IJwyVPA7J!h3T~3<#d|1X67n;0n>oZ}?vr=+TBOsHiC`)~tg4bS`j2VWYJKHbS7NBOWyz zE#f`NBMER(R-;-^wC4~w0UaT3C(5%>&i(LJ|2GRi`qW)9wrad4Ev4mY_u8Qyv-Wt> z=Vg^+O}~5^w(7gR$GjunQ9lx%5zpA&NB!4Ih4MN;XJ9)&L5gO5!Lf>w&685(#F5ui z$5TM0A3weCEwIx^CR7NHr_%c-)S0>n(8HT2>mpOW$OQ>M=Zyp!o5mO=c?elf9N|AR zaz=`br8o~IfHVC#=VT;srolpwm7T@ra1xq9>6~y%irl#u)Wk-p2}Y>xtu};H;s~dt z(rpv*nK}ZezzAi-DVRVZ4TMf$gtA}`#7kg=3}l6oh?l?!<#OV%7@Uj*&U7111#Dp= z&PiTtPB8>dF+}c8xu8F=d&qJ@1&q!)91q@4-)eLa&037^zeQVX3s>RO2Oq#+AA;qa zy`vCpxCd9Dl*0l-m7rsg=^8{FUu?%fi~sgU|S!L+7vy zIQrmIo*bPv262nIrNPpUkdei5GOv~`7-1>2U@PY40Kmw%a2}llL9)N#me*Lyv}w5s zSOK)PV2e#oXsJIBFNdP?@Nr01E;E+w);_Q?^vLESKQIU*AP03{2Zgu%+A20AmRfC$ z@utHC8!cmbkC2uZVJ-N&!3(`quDDKVb`+(Vj=^n!84C{muMJZ`p?>y7OQez`$jqWg{cy1d}|n$P*{Zn2xM zVb1oXz#&#%TG*ToQf{LWEwYp^D@r+gCeyP;r?ToBp=>UUnL1}t%1y1rkn#mr2B`qJ zV|(?0?ge*jaQOvyY*Vwdn~|3jhu^y}hdCq5D;90O;*O?yN0M{Ge6_;TYPSAPGt@n)8hDykfwOjv zc{eqh8z1s{ ztrE9Y0J3SH17U#S0Cbbh5TUhX1c)!I*}si|Vk7kJ$#H3=8zQhb3I0uYMI3@CLBcoo zp%zGA>I_|-^IqrNK8F^NT$}QBfz|bO(YpW(U$6_Ra{zor#;ZpUPYTr&2;L6JM=gPP zh}vTKyYW{J>y+`0a3xID_2#ilqeS+l^!Mg`@ z;c|oei>^Tm#gAq+aD4;GH)73meMDQbYTB&g)p5_zIRwT0w-C%9P=0{@lUnph0^U{G zp$&keD8v0@NtwSmu`3ai)hN6*F)C}yb5Q(4@Kv7x_{ZWw>hZ(d6EXNNr{Jly+YK-# zzPR^On;ih2W6~?qm}A6yRGM{XMa;bc9^4z?=X(P!j_K|%b!ALLQ1Uv)#8tivfSaws;7p;~sm)`lDfK6OFYc~aVV7+P1zVt<< z-`?5RX>s;-@f{J@>+FkfK^I|P#J6+y^%m?hYjxeV**h`Ub@y9s4)^BS^v(CofE}?= zH{~1S2I`ZK+v^9r2Jk`Epv{PQ(jCd z4;H07yMwdPHPczf=KeHiO~7PKP8)>59R1+(L-z%tV;H!F zG3y~X1C^_1(B;11xhUT(%3H$f$UBS9$pNEQm1l%+oFi7X@xOKg1-o8x1dv^^VQn|u ztqK^M8Ztu_$40Q*F{$&t6!M~v4cIYjnH_V_cI8Q!n+y|NaQju9gNS`kc?zkm7~mdK zYULvaPGXLk*VOkHz&!XvYCIw4G#jfNf90I{#yK#6uJ2F}90ms@9R#NonPH&G=7G)&nupI$l5RTM5*ZZ{-Ea{G5|kj>0k@6Q)gaMp`-CN>k55 zDfOJ3%+3wzzaN{gc;UhfN%FBE?UG&OK!h20%;??Z`}`c z_sD%+iEYYDz$wC4jX-d%L=?p@9i^i5-{pcRz9{_M_nA<3S@^+MPOn)1)smnX`D!%@ QmWcbruebWedq|=G2T1A7>;M1& literal 0 HcmV?d00001 diff --git a/utils/tests/target_tracker/__pycache__/target_tracker_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/target_tracker/__pycache__/target_tracker_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000000000000000000000000000000000000..2a62dd2728ef7aad7e63ece2cb67a97539bc4798 GIT binary patch literal 73797 zcmeHwYg8OZmSzrZa>!Tc6W7|EJ(s@szp7o^To+3RPuvpIlEYV$nrFt%4nVtt&uDbyj>G^;a zdI4aiUIK*)| zXX=@sO#SeQbAS2tg?E1P+rG#1^3K2hn{%%{bK$99cntpmzv1s}^9PEKXK~ZpZ4J`rcS;-b?Vqu{|mUZr^^=z zdd?kv?EG)u@*MU(?Ct0Z1ic3g-(i2i1H#|Ed-VL9Zzf;4x{EE;2WNA`vd-K{G# z(@}m2IW#wT4!3oB548pHus?h0VqYI#>D&v$Q@{H2^RK*p;qBL^-hPRS?Y)tUk3Dzp zg}*{tj-UPY%TveRnR@TJi+uxUPyR+`xT(R@3I^EyzHCjLKN0q}w&=){TrLXMC#lo84dX`!q)%oMjf>Ye-RodzYS z&yHt2+!)X3#s9-iI5)+$R^%VgZ0!gHD1nFlKKs_*ZTEi5=sJj}+v^MZ?rrVT{a)SZ z>hxbjP3GR7U|UDvUe9z0LD6jEuEW0jk%d2K6yY`%_zzS9^f^9smk)k>sC4Mt z!==M_jBJd#*GIJVpW>3Em4NtBf5zVVpO+`T^BX2b*b#EHi@r^y!a3~B09PbcMku3Q zQ!b0tnYuHq5$En-i!@oGtiVJtN6@ts$CG|~Mkwn5dP7QcAma@)PhAVT1z~%>z^t=) zjt3oqMN0mK7KOk^3J0Y|0Zs?>VB9yxVgrGZM) z59LvDN##+wP#%|E9(75Df#4*F(75>bj*ZhebQ|n8Q;PO2Dj?cwtbB6Iv zAU!TFQ@FVNYs^KDRlfvAqr_0590kRdT4^_8=c6DGrJ0W-cD;7a=4ypvteJAICMhvj ztJPPSbAcj#rCt{*GUo!tX^(Y_#eKy&VXV6_D|09{_{*M^EsU(bN~JOT$coRDQ%NjV zv032ig^`ug$}VjdxK4b5taI2IY!IRL4Fa>y;Y>X*oCOJPCM4RXJTzQ_vM3SXuR&*E znet>af5FyJs(JYZTbuS5Y_Y-#8=11jYr#uHO<~h_PEUa^+FIol-Jq1qrp_1V4waq&CG~ zLVal(g9Qn-ItRFf-m2G8zkdHk8p@q;ewMLgN4@h%=c={cy_@R;dH>!-JUcvuHH#=9 zW>k}i0%AsOs1G!dF%cO%v>nU~udUAuHzXO;>D6b*pxbCW>@#}3ZOFsJQ?Mu8R3A8zY2`<+ zu<5fx<$520;YMu%Z@1ys+ggKu+{z$XDIRU(ERUv`E?L~hb6up7 zJ<@KNC2gnI2b-A({Rs(?X8kN_?(2GZIuB+~V)!1KmgHhC9KR5N% zsUuD4WR_SK^_Nf#Xg_ghn_5GRDm+(04MF=e=wVMd;~8xMF#qaVm@hd=G$>>|6l=sY zu!A*CoecG?5^hAh-Q7zzO7+=RwP!EIUV`fS z7S?1pB(`KcAd7DLY<+qSL6#&q(liT4l3S+9m=t`Jm00j`u|Wna@jRkP!RJGwv-h*x<7hq`(?bVlku z*wJ;Lufy931GRXrH{c^P%5EcGxSMr)uwJ{tFuII*nfE}Kao891`XBD@@OAo_U0Zff zx9$u2nXQ-TO7(c2#ejIeT4UpGrDuq{L{pDv;@#t!s7UcF!`FGxk5@5o#tY_f;9xw@ zsyN2A#FuPd6Y9&2MHFi4*W{9(U_Er`853Uwd&JsO}Kw!LTF=vzO>wcFA*9f3W(QIQ}Fr^cL#e-^HF>b zVO#1A9`biGt5U-cI_lj5aeRAWnpar;bXVU^r`@HK?xj)p(umd!7;`TN5^*n|bgz$+ zwO;dRBa2u++8A{=Pik~b5CtN5NW!Sr+24}E+QnQW;TzP;;#4L22=v6Y{W7<-B^c@2`kjv0+%5ulh zZkZirgm6-#C}Q#GbU(`8L@E0nJQlf5OT>xkX4?j~MYJVDB>?@~hDru@O=?SUOz-^NpgvRnC`V{U{ge^8PWjyH{s;2nbhc*;9(w+Fp5~*j0j>D zI!?4ac8v0pJfA8Mm=G13ObHhN~<2oPt-cs^#hMgx~~7gb^UPp zuy6SG$lBeJ<<}!dJz}owDQAjP&~e04TJ?yyuBV*C1L26Pwm&@Vj26{Sy6BkTVIGh$ zidf`K1ThO8Ct69v5#%@Ql#~KhiI4%^J2ViAxYi6e1N4V*Dq1t?qGN)Gc|gJ_V(|b( z5VO#6qUEt;l$Ruz2#D!sJp(-vmuF}%Kz|QTMV?6)9TPmv0}@6Ni<=Qa%tFVBmdB1! zUXolQAeJ|y>hG$-%`{?B4UP#ObRZ-8tJ`KxYutQCmK#EuY|FVCoS?EV?0zi&^M6(el_a z4SA_4B(dyuXRguQ+kE1^OTuUu&nG;PKm&=Lq;0@+oxOY3{w!u$nk`pABI z2L3Yfr`gz?!_K0et=baQ{0t44gRe}0g*B~9cq>>SDB5jwJUVD#wVIu@E+HpaB2$$L z%sPwrWa|>F{Ox2na%BLlAS_CfQzZRDk`g+joD{T{&S>^Lt#t%0)2bqyZ1|OR1k%D^ zxucCO|BUMh#QFjI(s1eSaBj#I%4PaTto{hRsNBb{Jn}=CD31b4qi7!s=d3*Bx`34B zAt_NFh00Ugm7QD#pck1d2=cJTuOyV$#@B0PTPrD)ob33VE!u}Tn>x2l@X@xtSQ<7b zR_@rfD2sw@-EG$;pk$zfqxRCPm!kH~liIJB32vv&UnwP@#@{S(&(4Z2ll1AL#fP&{ zqursbkek)${7`=2Z{WkDLH z9WI`;b||eFQ?^4%iSk&i^d4W{m!&Y|x_>V$w&@G~ACxDu>(7?3FEp&LL$0q9AMuTz ztFM;)@~13GNl9Y_`86uiYxJdxZ6gBRFlH1<%=-ePPsTGDfk5oP9L;RULJ%wMtlE z;iUOPD9d&RhM5FrV5Onbz$^MHjLzrkTPDZ+DO--DM9aC-vje#%R$nb<2Wig-X7X@g z%?C-d81z*$$WYKATUw_4H)!->(C5&go{||s8RW^N-g%^y3^thE z3O6g;NOmP{y4Po#b&&ze#svEsSjiZ)WX0G{fCfOY4cSN8q;KZNt)!sZ@97M3LpNd8 z2Kz2%5^+LHHiw}{Pk0bmz!}6j_PqDiJLArsc%c&ULOX?7*b&u!4A01{YfOgqng($1 z8zyq*h=~jbTh9OSi}bs=>mr$r#k0wNrAmcI>!yoABFd5Nc z8R|@90nKO*;vc_TS~${oTMO8qwn2|+JV=&KiwropaTvP@kfOl2nE*LHGQL5eg}`nC z*Av)70FzaRaRY%H3EV{B7JzuZTB7kxa4KG4MzQxnrMlnW8-R%nx$uH1jrH2~W$^5D z;2~jLb#)n+F?(wF%joASmrlI1;5; zj*|6>iNgA18YPH8jJM6oe`^0@`v;rGwTjcOBC>e-LbRDM_E{b!8=uvHF?TJH;oATr z?%LtoqVCm`8XXg61P@6V)mD>%4^hM{bexEx?xg53mQdouw{cX3$ouP6xToDkgLMPj zC$$P36Iuo8OC1kL7^PSqidm?BMB)BY4c^L>Yn2D($R(TREr#{MbR_$?q>Tu@n zb@uH(L*`#!evSCBpm?xyvSf9%WOb}$?PSR{(UNOoCF^4a8~ScMy{IArrfeR(O=Tj2 zDg!tV`Zx19VxoznJx!yO3`3*4^|4!@`~zkL_vLfpbWz1*(TZr%iioxjFjiCtBvMp2 zS%ly^hOdjb35mMbO={LLp{?tGh)2XMbes~e;L&3|BbJ8YR$@-@fws=5XS*&VDq;hA>wToNYUFjZ|tiND>SOc>QSRX3ZUkFS4Eso4={ROiNN=epV$%zmR z`>Gr2->4w5z1}j_A7a5OXFspU z!C|uMVVXTpQU{1&vcyu$s^_d8%KkQGJ(QHF zht*2$v+I@S+Tz5LDc1;@vc6PbCM`}1(=I)!uUAV?_|C<-0Oj+Ae6&x5nN=mprn&?#EIP!D=*X;UduDEP@w8b!xh=KZ7{XJq-LJMON8n*06s@ZMQ%Jupo0J{_%{v{=p;ZIY=fTBz@9PC$bXhmORi+n z*67ws7c)KNuGxOqBXatYyz!AiUZm^G=12>C8@<6k0wlvQzD0mE!}TRbAK~n+J%r7w z-Q}uUAJH+*?c;>qMSxVgai^DQd;?^rHBE!d^|x0u{VkkD+z)MS5L(=>4tNgmbwZ*0 zP+PF|kk@wrJ|D2jk1u%0cn0}UQ3U=T8t{3i#$AS_n)V(G6y ztD{k?Nab`8N+ZKgx6zl4>}=U=H16 z+$wA8t&`fV6B-KxQ;#TOQEezLW})LmtC9haXtxrpOiFLU|}9OA~^mpEUuTu{=CUNf0ElE5u4v4z$tQys71~ z*T_w~zRD#<$@~dtD*|t+XvJ1J&hG`lA#4hmLgY=ZFfOG}@6{L0LymuO{q#c5pfpfx zG2<8AQx7_iKBze$tf|<4k&L|0^QUMtt;gTL&lI~jIz`OEmV+DF@3*^{=tSKIdHN2> zBtv=Py`&wGjSaYq;UU0eA1eqWz0iyc5sY5CN&*SvhXj}?q?Cy!uqm&(H*>O;I*Av! zl{ynX%xhNX0%N z>JiiS(d>od6m%T1lteuu+CG}m{T^CIl3%Dl*0Na54Uy^_WBE5JZt$we z4PKQzCS2sXi$%mNbR1{`87LyID(ThCGxPwtYYPoM5LMl^AuJuPab#w9ZE#>lPTa^{ zTXG^q!};6CIhbZopFriJwvwp#(-sKc(y=4OzS9mbYB&F3|7nMp(zBnAU5~w*2wA&^ zB>AY4D^Ud{v>UWIh5aze!ZeHaW}acHRV*m2vyG3cGI7<;odBIAV zihK_z{oXLc%tw`#zulltPN^{YvM6n|k_kSl@>0;s$yW+GInmPH(nl36JQW{R)Csen zl9R`bQch|3sIn^!vN%mu#)T;1q8ZBA*-oF8q~A0fu41gH&oWok=Y{eDlghi;SOJso zC`=GbDUC8gESs}3mzG&6%Un`o!maEbW4DUgvJ+9>&aNKG31SL5Ih@wZg$!!2&WD~Kqv0&%52+xG+HpRgKzziRzFf9hAwob^~z6qK^Z zl9cGN;EBilsubw(g%c_;Et(6{uJ1Eie=_kCsXCK>_`W-*twp!Kt239fXWp(w3U1Nk_+{87WoIctMOqR&s+1|=oh z;7X-b?M4IXjugMRQA3@0uP=PmVEaXw=CgM8-eO@sV0H~ky}E?n0%JJa(m*dj-c_uX_{y71H}>a1rC;##`` z!5xha8$9NYP)C1#@rTd+?%ksh=broN=+tjcaY^rIi8gS;;R+Maa?$RO=>`J?4iO-O zJ8sNIu1X9VrSUf`7LWF=Kug@kU7Q$AJ=cJMp5;1v7TTJMJ;=HK!Koj=jK|?mr#=yP z>Oq3y`U=Z(EnZHpSzwsg)!N4P_VMa%0eBf9=zYlFcJNRzp4ZmZW8%qzWF{MKL7wTj zJE7^V!354s{>-Gw-=ya$wUs8@36dGB9;bZnBEX!LSbj>F@!JqxU=B=3>Kq_v=!(GE zL{*0+#6|3G1fMJLSkl$$_v&4K2%TX8hi#8q4;jai!!IZ+mz|~C>E(6gxTNNE#bOAd z|KRkZDxCJ)%c5yf{hgyXoZJ|z+7+p6!JZvoj-^@OUe)BKe7I-CKYHiMJ+Z2mNab$q z_mSWf0zN^wn~>YcO~`GQn~>Wkwc94l2%zc_)ovp}k669wfW1o|Ke&)c$P3#D9g-vu!+p#OQX#>Rd#Y=~7j}_JS-F(_re5`1o=y=s| z-H2w^m&GlSDqdeq1ql4(C}d)RbsEEvPrim>h?smy8&Zv__?@y*ar}CuNd}4 z-D@VbyKzjI5j-SeRJ*%>3lGICbexDiJbH{Jl=!eurj687#z_arzZ})gmtl1{cbXjR z8Z}+hk%umlX*&LOMokBH+?KiwP@6w%uMORaKemU4&`l`UG+fKVRm((7Y5}k_hohZb zawYx38SsRkA@mB6a^xgQIsVpKp=Y-@$BcRmDSfxwp+nq{=`l$E!+v%ncS~=B5vWB; zpRA;xMTrq;UJ6>tmlSl?EOz#LtNIS^2|t@WS}6JsGErwg<&Jh{R?-|VX>^w%WYu8} zGfNl#b5Wu+5K8(9-nLgKT|iJ=!7iVcj;Ua5Z7LxKWlwJ-b#qi+3WD3Qh;=sQt zb;_QhrO)ZV8K({B0y0L1~aG`VB)(VN=Ver$BiwN$}`!c{NIMD`kz6 zlz4(_HsM|UtgY7SAMEI`^)X`P=I& znU;20XzqAj2436o+eTY#(>D7Qp3Kbey003)=eWMk#6uhDeK*EgWZdais4+i$Urd?p^;u!faG4%7XqLBB3D|mhx8!kMwN;4+QeeEj_u}&xVt6 zn1&l|M0-C0`X0q|5Pb+L3+7V6d=9cm&*d-3)I^H$K!B3?#fI z=sRbjZ|&+l(5Cx4Tm4q5Mr6dfNSMh+Y(c=|fS6J6m*3#)?l!s}#%=|452(VxVgs&B z94E4sH>DEfn=FvUuR-tpS&`x~qLB(-1Lrrgy&D))&c64o^gv-TW$?X=eFJdPPpdW* z*ZfTV)IxD=F~jwQ?I8e3&@|WlA;iVA+JgSW0pr_reV8KhtRiEIthh$3Wbb0!4ubls zWN!_3QrgD}FohNK#s3`&zC_?<08^)j?GfTxteP2*(Irw|asU0V5OyyC9{_jaf1t~N za%!vU(SL*S@91)VOLs5n^ZZ7zaf5M>jN&O8n*(HdJzXKjV^lv7)R5rq!76*c2E+SM zTW0`1AcFqZU|UzG@eZ>4Z}<=VlxxQdtDo+g*?IrGa77N~SYau=jsx6ux_ZTA^@eEm zhFEpeWc8M4^_CB+w+$5ay9QUCb{9{&tE2Afq1G{X?dghT12>&nv~=Lck8&L~+ds;4 zELj%OmiBKRDv!9APHJ>a@GuWZ7^PSqf?!>s#PiZOC_ zVT@hi(ZEMaI0_h9M+uV-ken8`tfO0U&+&D`a8-^{Sw0M3<=`1ZHUkAm2amyOhKkGjDxIwnL655_dg zgIyV8S2!*qGI+B>k_vQO+dhK2OZg$yXh5R46UUQaRNGF)v6>u-+zbyP?mF-S=!g>_ z;;tKUM&0$3+RZp7%m^NeF>-ccj9uW-z(<@M1&mxx36lfSb~(J>)ncrd0>9_-2(yTWk^k-?kUl2oAM+P0AbnQF9EqPUYx zsv^jHx~`7&S4>w|gBg|ms-z$dVA!u+6PR@lJC*C%R=mKVKXJ56?TLuRIwZHir|bF@ zxiu`#+w%pbb%yp%!-8xqDz?@$+3D5ZFq0&$B(kw%w$^_@j%(R4+Uc&OwhZI?tQ53T z(xGRYI+iSDLAG@t8tEkctOc%O9>G>CVCC2B{0vRkohrNzWw;$lnpNeh?nG)V*A^NC zrId2?9Nh&ysMF?JuAZys;ZLSJDCN>vMd7g1yM;B9tBLXnXJZevoKSWshe_64Ay>jG zBPsQ_SOfnE#L+kJDZn5&`1 zltTID&YM0fMSCd_bzNU#>VgaPYP}{@*v5AfQC;a+e6kz4N!t;TV?ygGIOqs#DOynW z3?-Hm%4xq!e9NtKxCnh1jj1w>@Zz zaUsp%Hj6&XI)@9`Tw9KmA?L{n{12>zoo8bLYp(rO9}|QmuT#`iIYQJ|AJ1an<++)! z6fjvHSu!O3*b0r$KYLuWFnd%=@)Bl`spbD6Nb%`UNa`2#DO7c5)ThiSmxYFD?N`2k zkW#}r)G%eXZ%blvwimdmpiC_OSy|( zXfHb-he+# zvu&-Z%VfKC&Zy^$i|K&-V|G!n*r(Hj`p!r9Iw*1Linty%67p2XI zWp|@#*-e|&aT9LxNosfq)B>1B-rq6Z=e+MTkroj%>}z56x^`CIJ(R=6KY8ig@qvpk z4xandzH@*1w2kAM7wBfxWpg*@B!)cl4aG7TX4>597>H?gJmdCq2iiJe)N7g|W4Ak) zBF8nD(jGD%rW>pvK(2L?{H-5(kXeV@**qf?es>_&-5c5h3AcjPGp7P#Qq5p2eIHp$ zH$2>!7sj*IXB(CDG-R~PY}}J9q>ns8&(&nTE-*9uMp$0+TM{NBVMLw$XiYGF=Y)g_ zLRgWTsey?|Ft~v~U$1<=Kf|LjyX;L2Zf0-`!KMuiHXTvTxSQ}WEPB(G=V=q;>6gbd z^ZxBh-LOJGJDeP)C2BNAqQ|2@LU$liF1z?%RG z=Jw4Ap=%RDTh&lg<21wkrj49{zgLr)l^NR1_fDWC5i?otBc2=YuVs5Fbar)mdH-XX z>Ib|&1J1450M~JBF%bWL-}-_LG`* zOlYf)VhcZth*{{kwyJ*zj~&xi4OL4Nmg*dr5TqY|3HKz{ZyMMX(Q1Y?fc{N5xoajh zIwp9S2PBLl7Pk6Cfc-H93J-Aon|SOP_5 zN0jcv<6;&%uI;44NT5XWA*^+vI8X`{ltYVn>=@80iQ>UH4v3rJ31tw-gtqgwAdf(1 z0!3!SVU-Lxy$G6xd0T;ka`a9fi`$OESr-uio&JXW(sYRXE-=|F;V+WOvZ<~{J+67N zCZerI^{t7yR|AW>S5IozF`=!d+DH*G3mw;1Q*E@!fFmtcL6cITpd3PLqu4RfBxa6C zT)(2NSp6QB4Y!VL9L*fXhi7Y~a$C%`oqKe=?C&AH(VRFcykgD*Wf@cr?u}?w{hJ2& zQURm$sG8tG9*{7KSahUB5VO#6qUEt;8nTk)5&^NSepm48#P=?0diFc&hP$_qtQ-lB z-T{xEHM=6!EwTLFe8b(!Gw`<8JJ3sv=?$xl9DE4mU9C}N+gN-=@wvEcfq8XM#G2opyJXPM# zuAJneBdEgo-mTc&3QLx;5=#Rc@+3G* zTPTerzFMNr8B4=PjdO{mk+)D9m)$n(9Lr=tdCdA9raY$JJXK$X9j4g=zlGLeeVs0? zud-31pmkhYc@!?RkIQZ!o}>;AL6k?)CAE*@h4Q$}@^G}ToL(Mc97&4_(-@VO%-N_^ zF4{{uDwUKNm6j+xup1eDL8DTr1xxpHl>pUgzMTbAA6!SlRRq=$pv7kf>EW4Pfw@HD zYCxuNpoKeI1%uUh^iVsz#A>1U95-GdY*LujWWA_Nl(NDVChi}lmnf8Hc7%YXs zt#skr1Re#b&$adE!}J1{O5poM{+|E>q@<7%dM=NzW%79DH^}e@dOCuT%lkWYE{yjd z?hf`E{~4+3Fe?ds6H_HAh(GNrI#xJPIB3LNOCVI3)A^*>zhJ02qA^$WOn8m08J17* zFfjFqA{JA1io+fR0v#t>9y_KjAA*c0ArU9Ol45}?eXlDjTiG@MpCU_!;M8VG|F)r$ zfnAf@5*!me$O95a5sLyNf|!Ml6D^M&qpT#kL_kbTWP4dJ8`?;IhOQgh2yZIfPZ0ST zLRdNt;t2T}!p(?={0x~W{0vd;px80E97;~aiOJQ{qR9eJw7?T9SOK3)MT?F-GVlmF zPI?4xlW>j-=SkcJ61h-RZ^)a>37J7E&G|u7j3xXFu+0L(kN;>BhKNQ0Hu)JvD9aM{YNAFDZ4N?k4yA zdSUvh52}t?ir_55T}pxth2RYb4mvs^6Q2#VX0y~ zWFrBw+`Q;zQa(eg(%DZ*Ycr#?77eKLP`r7Dt%DOTu`2(Xa>QoNp)X>~|FeP~lidzm zhZxy>@61SB6SSc?Xw-IndPa9D$N!*B9Nn=+(EE!UD+BI2q z24Pc^t@jo6CZ+ob$sv&fHM=^+lgp)eDd?nZD+Qe#U#6gwi{0UZo;zQ5Gnd|OQ|fm4 zPA9S;b<4v)xt5^?p)R(GwOtKco@Nd6EYvX31{Z1=YLtBqvoYgl8zjRyLnxccu?j*3 zkYibkAnbdg0`x+)ltz)c&ztY0@^*IprR;U|71C+OO1(}x%_vHHIk0W}DT-XJ!^C06LNiK7ZKc%O7jQ|;5a?{#BB#cd`6X)Lb<<>@z z$|RY5r5PH5TL`3KthbNo-Xm}=f!hhN*)vU->x+#(idaOzL%=evBO5txJoh+Zmhs$U z6ugJP+XU_;!0hX2G992jVB|d7MEG=iGn-DgcKHIqw$6jz1HO)q`+TkUds`26bp>eK zMn8<);8r=<6bHlut4=TMG)4k&ZoRZp+)+jDwJHErKmnXqAFR{m4_ zAKU-@%|mw#H;CV?O;zXL6JL>uJf`~7XTPLTV6)%SoSA07_WF8j7CfulPSWMuIs=8i z8uO)4CLB(Y3(vQ#nX>J+d!RA()tSn-Gdu0=PSlONY_^lKH2aui$~k+xot8yf3t%Lk zoq|@*M^n&AN1}R;`R&Z2xqc|i964vFy={s73MYL#X*)fQSr6EiMvnDu)}>3wy^g_* z?}ea|aHXyRA=eR9B(lmdG1Yabl$Qu8gkB`&5RyYof@1e6sr=~#a z6XLE}sny1ndJT*5nZ2jDf<^mNir$cp`x60jSU%hsISAE4KDw%f?INUjGfD=OVyy#hVlTya45n<+UUDL+pS3s;F5 z%-MxI?Y?p*H>4S1Z7}2LqnZOVjVfgu6XQqVIA0G|N0^5|;_%uyMsW`ipoxePAP^+L zCMRWt4FJS*JGxr$_v_f=A$gnkdZMWXs4q+&pfeR!3teX;ZEKXim*Q?9a3g`62z-kG zjneD$lgHsqPn9^nHey8HNZZtM_$ zsI9~AJ?!@ddJHTn8AtMcG|Kj{QTAEBq{P%@&9JM0)Hkdg3c_7G^b5g=;;o&+(gQn3 zqM(A3f!wo`19uZ0r?fm4zTBrJ;>4$mOJ2zT!~DKmxHV_vvAb1k&O1kLd*wT$S))BC zANXbO`>XzAS*+&P4{Pd%?;N>dbkoUI@0Z7FZu}_IS#vX%lQ;`*arQgO%ri6)in!Jc zHv{yCa4K3e>7rwThj~E4C}JPrp_qk^6D^M&qr4=!L_kbBKz6gW{h^`WXwlk97abEW z7Uls7qlm?Qh#+R6<3uZII1+KK9qN^o0#%8S!RfN{7k2*P&WNjaaA%~b7UyVD?WD^( zCUB*ZN5m|2oaj|D;E4O!?I^66K0c+f7#p8DVB`6FtDS+Wa_^aB?=V3lVTQ4b00}@YVLkjzBKG=%%a*POXjZ3=>dmF% zb!=F#`w#e-S(+j)_qGMR8=E#f+|<0m_yCE&OJn-Od`zELnObgs`|!%);K&`L&9U+= zk+QAWv}n3)JUzMjnM5l%2z7_|TWKzd=Xkw(SF6_>ciq?1 z)&ccTfT^zHi@n{wRFfOHTMcYV*kJgvH)zlw&y`W}95c>nq4xPp0@SACOM^b+pdSh& z18O9{(U2H4Xgb1mWvpGZX3q3ye!B1kfoBLjN8mVtcL@AF0kZ2fq6BDO%o@@O2YV?# zt#(~^xJ&Qp@b5JKJFcUNIRf_qV20^*ew^WUYX4`6!|8m|@r3)gj)MQAIi1@u?F4W+ zof|$WT<)y?h?+<0%Jq QLxFR}r=Ak$R`v}4ABNkiq5uE@ literal 0 HcmV?d00001 diff --git a/utils/tests/target_tracker/__pycache__/tracklet_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/target_tracker/__pycache__/tracklet_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000000000000000000000000000000000000..78c56a80979b92ef0becd7c752c711b47eee907b GIT binary patch literal 47576 zcmeHQdvF`adB@}MAi#$xk$T&LBw7(HTM|X;<%qIu$%^H~YEoN`%{YTXJV++Zm+wHC zRDhO#Bo^ZroUv;-Q5t&2iKy6#HJxdrI8NhcI{l~Z3_*~h2jf37iC*q>5=WV7X8c!w z-|pSs0X*U$l9ogVH^APvd*ANu?e2ZM-($b~&#J0`1IIu8<7*v{)jJ%&qlkXEG~iF) z0q~kbcC7q-PwhHL?fc0@;hOR`wxWDEkpEk}D9_$pM7*auDHSxf07nyyU%c?_@cCc9df{h3 zyztGRU3lTu3*UVD;y*uk`JGpPH~3T_aOu=9F24D~RXpj4PE}#FE5|{@x^ZsUOf5E z<)6KC>D^z2etoKGL zF8}M#FTMIaB7pHr{Bem&Y%6^#o_lZym{*UyDy>` zV1HSO$?a|NO^15rXgs#*{;2ZU{q3q6y8P2OF1~u^@;g5{|8t-{75Ioc^c+8>J zo64^?Fmx#4)a$I6F&F69jkAH-?Zo|!T3g||^ipl`+&Z3IangO{js3jAG7O^_)U5r7<4!3OZYl&*3*r1(v@&nru)$g zelgSh-c+OMXPR2dW149e&e=@k(J zzyADk-ARefZWVI9yf{&z&2@p2=emGil77yc>--ocA!Bst5E~^{=`$H6OUk3fSnbW* zQL;uK8^)Q9607u?jFP40QSuoWB@M>t&>=QTtkP#PN|u#J$$TFr1CH?WgO~W|XxO6x z&3lgWlmj~Opnhrhp3}}xTrDkO=kZ%_S>HRbC9DR%p#hc+PIKr+oVSqlb63_2&Y6dIw0f6^Saj+DKcpvooT^ zdKD7?M3q>s5|<-~``SApe^PVa&K_W>xup^H(Vo6enW09GcJ>^Oc1F6RU9nt6M2(UV ztyjra-^GSmdr$X0iqfOx7DbNqC|%KbB-Y>C8SRe7aRpTv$T^?LIUiMMeVudlH|AUe z`1fzYc}qB`ETP&$qU6X0sVaQvHku>N=13Ed@H=fYM|LPA%`oF`HDBItj_g!PRunRh zPwv}y$0J`>dX6g5u6@yX^pUn6ITn$Xp6=M~)OwHf#oIg8N0^N8k$l17Bl#l3-ho_* zwYSpK*DXW3*xnoIRbqG{kHyr;v1n&sOjWkx?zD1O>md;vbbJ`7O*N#Yv^=~o8wd~X z{m{QSrDpvr2JiYHSou=(Q1hs?Bt^lA=2Y|0u1RSLeiJ;(BO=TowUNiN1~^W*JavqU z66gW|^0mHK(@@i>v^dp(;6ziZVd(Rd(qjB3c$7y(m_aI@ix9E~I8L}cb&QG<=mG#T zJ=e9m*ZJVWRO54Z4DS0NSo6~Mq3xs6%2eBHM_xVx;KcUSkxX#qq(r|79_JAeW{`@` zA%v^}juWoH;Q%XGV5ZtQ2+Y)x0!&0vR=GaZsPm+KP|ZJqIymy=?i~{*P zN#KPfJYW8fb(E&(Njl@m$&U`ml?x9p`IV)lUjx%~S=Cl(L*zioZDsBYtKJjiP?y%&_TH2n zXuE(-%?*u9@%|7R7n|}Vy|y-Z9vx2(o~&R}EuUN|S0#MyT!Z6}LlesnQ%M%On1qmz z=_P7XEhjWMUQLtZX4)JN_>Jm=3cZR7Q3`DZ|M8udYX9fXY( zdEj1d3!wrntaM(gLS!iCjOM(2ks7XGOLtnZH?sA+vWy;x#PG@rLX~h~GYhw{u=#k6 ziP>qyU0L$B)0KP~Oyw)PD5sa#Mj;`xLK0(T6$K#*R#QL{9%U^B4G2(^&l9{6fq6}- zEhG6&ck<=4wq|Y2i!~{Yl$EWo`D(RsYhG--N~2ONL6v4gU&WS=?J|_H?O2INz|LUR zsKnxZN_PZ{QRO~hQFYa|(5qtQ=&wmhDLK7wxFPHR+~8dw1{bFuPp{4fSB^?8Z+cHv zy-@{Vqz%EDJ_K)6jkKNV8xOSr9tBdUB@=9!l<0R(PvKD!W~7$qu{0x;tO1J4V^T}% zadBA(&6m@w#bq5FXZI3F96*)c^a@lR1nO`VPsOc9u+X8J06fztKs*MA3qoKi+t65f z^C_Kv#o)^q}kf0w#JN8{Her@BL_};G#!6Xt~BZRNrJ%+(;cm5XB!HE_Z36@~N(zc+*^$Cz9I9 zg)ci#2TU}&RI=>jWceT!Ur3v`F_Non#A6bXIOxJ9^r?IDM(^+@wfpo*Ll=a^NI$MP9t{rxdN^3z~huxXr+DVCi=d=VK6JbVLOSF|R zvIZzFk4bBZplX+O(0rK)s&-ii$JwoTG71=W3w!|pr3hVo6Jxhzx=5B{ZWHe@mvj-f zK;R)f1$5C{G8HmYISVQ@mCDVCE_&tCDYco}S=7j&cD7VW@I7-(nFY18t%1NK%50)i z7V}Bs>gi&Ipo{soPjf^UP1bARoN0E~?Z}e8yqVoMtC_t#fY80%~VSC;%% z+r2}=Qo?ow3ry6Ya=wjVHnS^VLhG7lb>i)m`zc_I5946iRQXqw_GJpbLIKI!mA^(1 z_7}{j$^&$XSQj>rDi0Dw=qjz7wK+6oo}m z;G86qIx;DvC*di9bCMukq!j{knK-BN3IXq20ezi2-c8K=w)9nCmasw)9FYwj7BV2)6?*MB8#Gx~1iQ3{>S zU&JWA{@m8wtyKkIWJ^m0W(jN68+nvs7tmkCD82r)^t#tzzR}VGj_MK?Q8?=MFfpmY zRpq?hWaiYFs~{c+W>vAMlFwe{{O#SxV1O5sbsnn~1u0*n;1LQ)aB0P8S@2jZm_jJl z)ft+MRx2c6&@H~_bF7^s8LD- z#eGqo;dMNQD(GP86+V{k&j#0zN}ERZA{Y&>AK9A;Zkm+ncTP*-F%f2@P1M4Kku^YZ zc}&_w&8l72LG$HwzqqV}gWkZ#x4yHmt1Z zcTP*-F%f2@CaN%DWDQVU9+R4=S+&bLXudog7MFE!oZX5iqkuDzV{4TV08pmNu}KEP z^n1gQSX>ncTatdy#)ig?-K>)AO+u%a@EnJ#B?;0a#c22a?_$TcSeOs^V{ z7s$1VfF>OXmTY+w)TILY*|eXm0ulmdbwiQ@{K*o>m-J5~F>sYB1}HK+rfS?6#klV8 z>-DEuHsiWXF|-ks_)Jj@a2yK{nshi1<0&=6y^Z+RR=_4bd* zqtgB}hY*a&XAYh21n|BG0j&2r-}j7%_5)_td;2rN{gV>?&gm&UD#DDkpH}9Ck~Kha zc}&_*W+mEX9W-Aivl8vH4vw>X2_z1nN@TL9q3R$|$$F1cajOwofM*T~@O_T}M?-`; z^*15yH(NHATWbGwEroF~l_jFZ8e@q@vD+%2B{CK*w1vQ|#Dlq`NA|v@8L}mzP_lI2 z1Jxe3zB&^wrnYk@+_vs}d@yr_$q3~!DeWq*v#Wv%8&?Hlfp3FnOw|F+eUC&;Q9{y| zpq`Rl8i=W`U5m2F{+M-tti(K2aDZNUX4D_G<^Gs;e^@zAnrVM5EceH(`y*8Fav(b6 z_hV7HKW5z@Yb^VtZbtN~zT6+P?vDn`{(uWnYTG{AcxBIhEN0xtl5%f+I=um>qU$Vr z$t^EUdPxPwxI{SZQ~%9$e=;pDIN;vZ2QG0L<@pS9iH0JDwkENo!(yYf;lfp3t~to_ z$SdTPVxyS9WNuQZMy$9=roixm!?%3H(5LJ5S{mo%O#Aeva-Z^6yv}`ktI?M_gg(7_ zM(^pWa-ZH1eM-A63XU1~X{g+%yeY5IdrDSe1qV4zuXM0=&49cb+mC7QE<0xF;@mLu zuyb)PFrlg~SwvS>1|g- z-92#>&}>%#3`UWg<1?KtI`b%Q4r9^3At#OZQUSuDi{Z~@n!&*;#mZzZ7j-w3P$7p+ zE*Fhm|Hg*HS>4O*g%S7C9^h-Ud;pU>MS6hQG=|wVBg#6yNpneqUeZ12Sh@0q)>w{3 z@v5D684fEyX(~B$oA%WZIDK5EkeW`B5aj&&J6x)(n_R$nRdas*1|tLRMx z5w$njN7Eiuv-iekmHb#3%6YDu=Ux|4X*I6&wUP75NPBlX)6*Ymk9Eqb@(gkvz+ZjA z%Y8a5NY%b_ARQY%G_vnZL$+@F*+plg@7HGidk63O4V(tvF?7eMv^-sl;KUv2+Mzoq zrRDfd@FfBtL<>IOZ6YRgei$<_PdQJlM@?xO%)957v|k-aIMMZ-U2pM1&cnqTYm% zHNbJg<*8#5$`a@T05Y}Wj-ef+($du92u|!sJw61tKuhtP;87kCVFszV7a?Q~aGY>? z>KGLz&;MjH)wx;eHLO0jrH^HMkBEk$(aW6v18sIqL^3*XZN}vk> z$aJr5L)*wz=2ys7=C;&V%&syK70xa>FukkHZ5)AIWfo?_70p#<%MhGH-I_j#;6zLM zU?zC$q(r|79_JAeW{`>pBZRC0juS3V9iy@Yx&VMo55``x)#(RErPX-F9vltA+Jt@+ zJkBE`%pesvBZRC0juS3V9g|R&KoN~?^$Jl{| zRLz&!KF8PpozzNTs)%bSINs9YPBQng5X(Z;?$Q|2?-yrA;Q`dr@5lEXvhVvs{3-j7 zW%BmNn;B~>T(eORXKbZ7c_w6s z={^<)2MJ*dulIm%NuG2+{~r>ybG2TQafUBe5B89_G@q+nv0*%qg(4ka2qUv9`U;oX zpqi`@v{7=pq;Vcm2ex9bh zj2RMqkeTsmvT_-^hPw>q%w(ibg%2KZK=@Q-=Ro8rHmBmNj>>Z=mo|&44{45gif}$3 zR>6h&iw~V}e&DZoDL52-zUqTTb+7GwdFQCVHMJA7oxgP`Iq7db=WiVjj@jYOs9yZB*HKw@4S6BS^XwO&{NgkB)6!(; z(9Th*ZfNJER40BDQXMV*C?RWr;{fx}a5*E@6`90OKwdV}NgM=;`1*oFlYU}42H7DO zbYe2MnzrB4au-aXWaD0N7}H|j1Zpuw8JLg^`%L893RhUq%6^UPxQ!{_l-FYNPl&}r z0l_A_V@C&xBMT1wY1txXnzlo$Rm{UylS*zi%VkH>31S_Xo>(hSpeYSv`iU;dDo!&^ zf~cTDHOiME)qhs$}^>hw;(ZRCkZ zW8F+>9O>;*A@RYkj2DsTQOf)a1mwQ62*I1fr98wgADC$hV>B(;4A2w{u>&|LgU0|2 z0Ywp-@(O0UIj1T1Oo7$=D>z{1K`y*#Gql-- z*L8QxZ>qHwO)@bAC2!$1Zy~0&Nx@-!)bgq}8{0X-7t|BZ==moWz04WCv2Yd?)bz|& zOYmX~XW9nqM$bDstZd+2vLng1yTe8cj@__Bq(Xmq%a2D?voTSFF=77f=DZmbrl+$S zJ7O6vFVI+-3b|HZC_Z}WEqRl2G3U=7w78?gf+3Zi#3>~Malu4de<)RUC4=&!WF=e| z_!GfI<=b__bwO1kkf>soT-Awc^^d0KwSL2^@rKt^9>bE$l*g)##OFNa#U;OAc3b4d z^XlseTL%YIa$u#i*rw*9cWv;*26G#{O1-u=cvU){yyT>p>E3JPrLdo?Y3CoESWxn) zTA-JtpEZF+0PlD;{(k&}_*W*YrWs-0@(APftvtf4J~5xTx)B!A=L+Kt@tBv-D2xh~ zGNRH5OCE%jv#iS0%5Pxf)jnusYzKxHDcCdABnqSER4>2DF9%Ptx2R67)b&yb8JT#pH5mNoJ!%CyB*ORG0jPk7nab_C=1Jg=OAuyS7d~sZ% z0x>NxtjaWr7N>C_QmVjC=*$ZkdBcH-m9P$54xk*Vf+l;Ow5MFHsg!0ZIAb<376LPT z5r}jb?2v27pP;-@e9eM{Y^38TI1+w&5uE3c(;Xa45oJC0r#jCuMv=8J&Dwd+Jn5?& zXzYhr*W?8=rbTsAwiB28Bd=u*zkSovBC9tKV&h~*!k4IEilqQrIlu%AWDjMpSfV#& zG37DD&hv<>Etxa=D-<`jiY0-G`D}^JyDBzp$xFr7&kDwsc&btPOLRIR#X>R7E)Mg45J@PJx0JiS`- zG^>r>L%T<%TZZcpoY;+1@Rmu5eiJ;-BO=To6~l@UvIaO#xIA@?$`a@T05T0LcC&TB z4btZPQEA<<`;_+$F97V=ANFQ~>n0`oozoI{OoSO}9o?NUvIaN~5O=54F_w?RB4oGW zG2|M?R|^RhxrV(0M*tJjx;fl^RNRrk-M(3={w2&y1;@;t2(bM^u7sN@bEJfX9U(!z za4XMt*d-)EltLD!?mSTn&AaTd@}8Qf@;A!25@rZRKG+Wfv#5gsrt&Y2D^&S5Wg4Os z{hJ{k_RSEFCZO=fX)VGIQ%M%ghT$)^U1D`Oo-p|3T?cMb5UFG|_u>8K7dUs~;>q_HY(;;TvJ(K=eRPK-2^#{Zl zb<>M6l*iCRHe!rvr4VGI%Z;5)z9%@td-cpRvutKY&6<}z#?Vg7^ET4G1(R#(U2h%Z zV}%P0GwUkYSr{QJKSEKj;jd!fpyO(+jfEeEYs%-{aJ<)bv4Fj1MycTyIbS3q_q0VK zIsf6l_Re^Fx5@-Rxn+^w0UBc)xw$;tH*Qp7PblqicwP}nIiHrs?f%J>Ker?vRgT8u z5$5(Xrfg*A&RCquiI_`FE_WjPI|XZCheCd>l&uupN5LTqzDmK@DL6vGGZZ{W!HX1- z4Ly_2uS}R$z28}XWwGR3c4cAE Tx$4Twdgo1l+U$3(V)y=kmhJAa literal 0 HcmV?d00001 diff --git a/utils/tests/target_tracker/kalman_box_filter_test.py b/utils/tests/target_tracker/kalman_box_filter_test.py new file mode 100644 index 0000000..1e9160b --- /dev/null +++ b/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]) diff --git a/utils/tests/target_tracker/target_tracker_test.py b/utils/tests/target_tracker/target_tracker_test.py new file mode 100644 index 0000000..e10b6bf --- /dev/null +++ b/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) diff --git a/utils/tests/target_tracker/tracklet_test.py b/utils/tests/target_tracker/tracklet_test.py new file mode 100644 index 0000000..a94517f --- /dev/null +++ b/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 diff --git a/utils/tests/visual_track_control/__pycache__/frame_transform_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/frame_transform_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000000000000000000000000000000000000..1b9b9a83fb3926109dea3812f52ba544e31f5cec GIT binary patch literal 11989 zcmeHNYitzP6`t9heRv;!V4Fac&~1$4EDzheUSmvH$0!g9Bv7TKYUNs*ES?3M*t?rM zV~pKh$&i#7wFr}hkTj%qq^Mbkw80e(R!bDrzx{);3(+c4s(>Hl4+J7b%CDYt=efJ% z-6bMzr83$*bMCq4-Z^vU%)Q?|Xa3>!x)?at{r>cUu$^J9V?jME3Ujv`nA42FbTI-e zSo+y63q4y0xE>434A{EttX$*ha?l!QmlMv`{;~mAmka8+mzgfNU<2wA>_EMO18BM6 z1X?j{@s|y*W*KH5i3~(}i9}-JzBn1+rKl)1CWd`qj=l~*ABjs5Da!AS3&Xq==VMX9 zH-G%p{I6bG_{*=CK00-M^oYx~`2OD(-aEeZ$_G9Y-4`X%SWk3g&-(RE2m8dqNI&-7 z^8()!k4Yrn--P{(O{Q1vNhGkpr9T{9eEW@C7snQlO)icfUHbHsrRfhAM&DVw@R9HD z?*|)OZe9G%w$QDM$8KHx?ajA8UK;yge*EOZ;a3;`cr9+lJ2XBGA!Bi0)FFV{6xF3Fr#?w1%fE7f}atZT+tp-gaHqVo81ru*hY&ibZk^n@QPV_H@zN)0T7@W>0t0l62qm z>{*Ge3?5o_-#pZ!j&xBE)il9FtN!mj)TNGF5f62%dcq22_VftVLQT@6%%0woXHT!% zlX^CrJz)lQrQE}ezjn`}JoDauYQN0^-m;*oBg6M9QGj_0N(XWmGjBMkSYU45e5BXV36d$_Mn|kK9@{}koMftbgw_#>j~+_& zM`95PYW(HIizcl?@-W)v#tX`0-C``X8Vha4Lc6gLkS&(46v!@R^|x78l&=(M&T1CS zDuqNGFkja3-Osjf>HZ0c?}vTE_J|be?uiRgULf&UbR+sx_n_3*FLqOy-)*)7=r+av ziQ&v@Y{QR4Nt_>!4Do#;-zP+4QlB(TLSRM({KXvrgGZUqSFKC8oU@*LZn|Z5)l)NE z?dXoN(0I+~H4mTOeRB6@E-+CCB;5++Obn9GA0Qj-i%Qb-+ciU8~Fbu;i|q080HGw$YV9R98<0=cBo85GNN6^DfH4LuH+K)9@pp58xBtvQFjeFLbXYnp8|v@Ib( z6^Gy~*@D%EeoZWpw6Ro|U}9`xVo81ru*hk+ib)&8Mz8Q)>KnbndiqAMu$~U4%e0N2 zOK=-DdJn0O#Bc|=eB>M@Z}hbBT*fys2(g@R=5F-#dou|qXug@f(esq-8AgvY@r
    EfxYk1x3!t2zjVxClm13Hq4~ zp%A1HoaV?%B>0+8SmpOp)R*A^di7>3i9U+|$d9nzhvdgd9z#-xWG#|tBOD_X>=30iJd)*ThAZxJ279a)IXVSO7SHidhU8IM4qcOfW8WH+p`*|8NBOTEG z5$xlY?DhGaI9vN<-n#rC3L!7~8bvg5f1?lpdGgm~bPkO6-F*X|QcgY=GP3$AV3yw! ztR@?I$l1b1-I8RJ7MeS-CavOIhFRBaj^mP6$h+FGjbS@(FL{RfVdgly!D^FmKvK&x z2#zxrnqTAur%;yUlz6$VMFDT4#pZ3Lq;4aznVu2Pd zv{y9HqG^JqZsETk>M=ZVv$4|9yWlN(tcpEloEPY!KC_}vIj;qbowtbP+B^&XP->{R znB{riR@Cym7A5$nw!RDYgm9KNW_=OJq?eNF1cAW4pgTD*@?vw}C)v7(D6MRltS1T{c ztL#2{ET|di%1+V-N;-gs_4X?z?zo2Tm12W;x540N{WPKAJ@iv{8HX z{C5!U0SqJ*8tBPa+`mCKR!=N%!@N%lH$OJ zvXan27)vVNq=X~sk z3AY>Tf=Vh`?j_hlBcHN@42OcPS*11+M^cY~8llk|3XKvZ3Lg|4jEa13ob-*rhoP85 zhYUISIkaqmzxZdEP)3;>GywYCxt3|`Z0%NUgXOL`vE|s7+YD>#IJRXx`sQ=9?#F29 zy?yTwygNYI^V=`%n%YI#X_1~TJ*%89ZKurCu4(bo_Sw)*TKa7B*4tL-@ZEvSp`B3p z#>zHy(3;D%^M;PE8P?PBEnbw0zt|9-*$}>qZBz|fc?UJVXbVpr{1)rvVtsg~KKzZ9 z@l;YB=n8Z~;i;XvOjbnMZO~h3&@<}`bRK%dprf=?WOtXf0otv3EWe#1%eJA;pl4jgYc;iLp89L;{hJ^5gd2 ztLm!mu?LSY;chp&w)^#~s#mXGRrkDlAKkz6`#lmI-#&I|`|>!82X$yFE{G}{O@mPB_oY)%HVy)Yf;bc@xR&{ii{(IN!@L#$iwksOn zm{`-^8jqHa{N&)s4_~?YkFSlLIrvG}9*<|V@4cHRelqr>UzC#Q)+mX#w?r#j7A{=8 zGp6l`#KTDvX?Zc+l4wtoM0_!F))r?Ot)-&_xsCmN@93L4$waRl&s{_SIGeZ(rR1Wa#$A?%NmNxb^0*MqWMuLVVKo^62aD-FoZq z#{TKu(TlyTvUDW~{Nu5{y(33{J=XU#pz)0S{f|b@ygS-=778$#v9qt-?CJyj)Be91 z{q_5RkG=oXPcL2^edASR@$>gUn2{fz96kBkt)J~1`G?nUo_u@c=xZ=Gbh&x-)mvT1 zZyq^z>#d_Ve|d1^z@D22&))p;n8d~V_0YzoL}pLG#ijC)KkZh&?;@2-Md ze@IHg-C&0*C$XL$+ZB$PSJigfpgnCoG$~1!wmVGh-GNq899v7J7kyu_}mqC2+AsssCMwVgI3Qk_XZr`w*-G4p!bKzB;obPNAN zlY(Qq#OFh-l*PQ=yqc8lGK_CZosCxJTyxNgmB~S8pCUhn)2Xf_j@@uqQcl&a=IPFu zOiQlhbUq`?h%SF$e4 z0$N%@f`B7!@912DAcUX>K^=l+y9*+k1`OyJ!65eN=!C0};NzWE+9JtqX%~s8u}Hg? zb{WH^Jq;b5s4l`0k}e9jMB?!aOSG>gBtg<8;jIY*QiP+sI^vP`ND^8C zZh}97v=@hA@&~PjYHOj^T3Bi=)H695|KO60Qb@y}eyMSM`Mt)+w`?GZRuXAzj3gsl zS`unBtdc}~v=WzL%Z_9$u5FL6#~M@ zcR^$+L!aVIs6H(Nz3_jjPm_2%>5)x)rj#V#+NEsTJmpZ8loNEcLwEAudCH|aQf{bI zbeHaa$0_uD9_Zjas<%D5r$^R)H@Oa0UAhM}w;Ov|Gm-fI;t>A7ArxQ*WobFWKWLBGQ^GYc@wcW(? zoAR4aQC3}(pF`IafUYS7o&|a#mo`XK0-1Q`R`sIVXP2qH+A7VWz>E4`%_Kg>V+ZSqx`hsswz7k|yE+HAc{HY^D0F z;jbGQ()CnBYtesHzI^!5_g9{h)Aq;<8bKome2Dae^C3du5-?W;F~ji)g6F|15i=^6 zf;STB=pcz*hBFeZ7gT`puGOLvFkT6)s|+85cdn(b%%W15)=HdC5WE@iZJ6QjWZnls z6A4@uFIh11nK^f3EzJdnxteQ9IR#;P4Kl#^x`DrER075;LA3FU3H~Ikow=BJ zFQH<>(&4Jr6ExDzMGMh|s-dus!lh&hQiK3xxQ`*YXLR9ePvukhk=`GI)+;GN5CD*N zvWJAA!A8)_Ca3_&LkJ#5fO|T@O`LoSKwt*FNH#% z?uBMv$vmVkryk_CNF2k1yD>~?aU*H<*2GS-4oGpmHFQ$Cq)#VxBAa?014ZRzApIKwob&Z>99r@OV3&~|D3&~N*)d$!1cd?USk^S?ZMvr5 z|5aYYiUJNQP3UwY$&iT-0}4(j)?QIS7=Z#;h6@Noomjw1z<{H`;CQ8=VF*y3R|F+T zf%2@6)xuEy8wCn;5%^_`1ZBpISfdrfT!b6Gp?CU#0-)Ck7g$K=kasR%C+hlEzP%EV z^W_&7om&J*e-gmCMg7U4P$OUi0E38Jo(}*>gN>Kx56|0xO~7H^hC%O!YYP5fU9@>lm0_t)aKxl@2@#_Wxww%TaiY%~tpWV0cQs12D=)v(st))q~YPVx+pqV=u) zD>WFiW*u%6&)?^~UQlvC@76z)^#4)OM%51bjZ_Eo5?d!mqDAIC(Lr`iVQkKPF|bIb7`Zo z;>JSVm+4x0T_&Z$)~p}L_%AYVC6f2gXZOmuyNm-q2~1uqG{mWF7_4kyovf|K{OnT6 zf{7+222H3*`8+)9U)Q7r!_Ni$#vQ6@I?84~Syfbr>O75O!_g=`_n%;h+Ol&y{GJuJCa{)gx-p-X#Zj{xU!jvU?3X#CfQ;pJ~-n`%r&R+B`DGO`X5ALa~Z0_ zsLLsVpu{{6Y`iv_WpJpt@j8r{pL5C^uP|QtoxzP4-{VNh`)_A#tH1BzQM@$<^1+@D zV!r?S!yjUs;VHbO!mWm&p1cY|P(>I?!Z_P1{vtry-m-&0q=iIx25e^ZYj-}2D^N1n zvuHF7b`gBQ#)V?i4g%@rXxgzgo`@u;9E)+W9qCzMn3i{uO~}~XnOLC2NWQZ|t%kn_ zW7A;~iaNBc{^FjR-jd#y ztNyZm^7Z`U1M%+ok=o-$Z!R6mFW+asUQpb#v3vJ5_e}Wz$USqP*0Yfn1RTU#RvI!< z(R{@{lXrn;j*4bLfB_8Ct$gm1^5;!dFCyh#2#V(Gsu#;kM;H#}&h za)#$CP3{>&@*F+iJa~o5>iK%imw2{k@jOo(Go~*;nVu)cAjci1#|{d`q|BCR9H&sG zEWu0*-z*ZC_lmC_q_QWKWMU7R5T3M;8R1ZU4@`1UXY81=KNF_^Iys%`zxsv$nh!~4o;vdru|qe4E*AmV{Jb*-R;HJE0(liBpLNSWuL6A5Z{ zy76LYp*ll-NG~*u!J;WY#UeG$@2wGq!6z6KzDlY9;w%qOOz z%v)8oDkHjbm0O0!3!fp8mqpBoa)=zEnIgmtVP>i}TX-Z())NwZRcf@AM57wd(+c(& zCEUiYeOn?9Z&<=um^93u=*0+2Evd$KGs04X zMKick4@H`jT#}ipgwbhrs1*qCXdoK_g1NmM7ZoPT603k1lY%)_(3fm6O zm|+gWlM3LG_TbB&5rWbO9Mn8VNrNmy{Z~Li#m$fh*NH}m0!8D@^|Z@aa5IHU9>-A> zcn`IGs+KzyEKtnM_b`gkf`H~zpF|7;Ng0{SAX$s`W?1RZut<%4YmPLSkuj|%8<46L z01H+zFTluivO+MZp{mUcL}7`!2ai$rK#lH5z;_^z zX;5+$C{IEjb3EmFNN5_A90kg=n^`Rk+OG-p?DGPJ$qD>2r1+2p5W_SXJ`r9onAO`n zSn$|@QnkN&py06~Zv~(ys{ssnD^6AqdaJG}_W>9xK{&T{m?(NY<^Dim3Z6#7kMkHY|8n7<<5~k{bP-*I(Ea&y#MH((mW|BY4S_( zh1X`d%d)ip-*(wIOE1Y2>fgUl{ag2`Z_8faDY5#1vx(p%MZN>T*s#h!pl^n?zHu0j zcUK3T#_KgC64Rnu+OsJV-w~zXP?$SP+SU;}CDCV|Y1{c!+rn2JT9WK&NhSzbFO3w_lx6-Ou6b|;NDiVE( z#FFrRi>QKcX;>ZGxzVL!_pNMamQV7kGCmt&_puzitV8e&0*sl_y^!7{@_j7gTNCX5 zx>vL%)E)8YD$)sc_(htAsU^s+$nxhlhpgPomt^@>=@suMl7B>6e#dE(%kOv;`5F06 Yz9O#y=#)$D6uacbcc*#eMU?e_18GavKmY&$ literal 0 HcmV?d00001 diff --git a/utils/tests/visual_track_control/__pycache__/mavsdk_gateway_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/mavsdk_gateway_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000000000000000000000000000000000000..0351912350aba14304b90d225f1a1cb7b53bc0f1 GIT binary patch literal 37585 zcmeHwdvqMdneRM$-Xo13mLKwK{I=HeOMU|eJD31*Fu9mzvtGzVR*!ATl18_CWFzg& zMi>)hFpng#m|zxh4hb?W!F%rR1`@NJv;XctGa4EB&SB5V*;ul;_YV^Lo|6rK+;e|l zRabS-s5LV-3Xi+)Bh`Fe{Z;i>^?g;{_5G@<|E;Pj=)m!fw_fV_`D};dw{#*7#UQTz zBZ!wAs^fq|b*ipT=K&Y{b$2OkE~lf*bHMB5S9}M2?27+@AHVL-Kv(cUkX=(cLtWtm zVW-38xZ9z6PC8WY+b*8}fhrc~L!4j6RjYxMjsp=jh_FTtA*@xy2{CI0$EE(66>w5a4|1t0a z{-XD>t9K^4x>~!{yE#vE{FxWWpLl%YpPrt4_r*^y40a{jjz$xQ4joLiYU-yKPW^V^ zL@;>e&0kNv@yz6tZ$-8Ep|}?BZi}yNTeD{Uv39+;wKJB~THB7s+7jJKEz!B2ZmO?` zB%STu6wB^bC{kNb59NF3nl;fYrv@j6UjO8+)6x5OKlE*+h~B#=x_xu>%CA107`iZi z;kog5UYdOB#Kb>6_UVOJzSEaHlIW&1pFH{Gl^2F7&Hg=mqm%ExbNT&K6K96T&!7DC z!s+0|3-4ck?^l<9{j=ydTaU)?H%b$oc;>~)XMZ{Qi+>sa#ZRxCcz^ue_b311b?|cm zF28qbeDH@8FT5-A8N71p+?D5lIQiiRlkdGXG4Sf-FW-&+>zgg>T7En5Sa5esbmG}_ zSB4&w$*%nLB$wmLJEt!H`j@D?PcQs57>q_I-~90>1Fv0q_Q_9P{kuv6p&9)|*6p$rD!8h`#rlnu)OT9R3!n8*_MqD=SQ zwI@0?%PYTr6(x-#OP<^1_uiNscw+q3C(FtLSxrF(FMs&z#OcSlET}Y8!1ybFH~zto zd7)WxN!anh_a{I2IVJ>`;2{oyGP0su*JF`Z7)t%fR@9$0a^d{SQ z3*@2E939tg$N5W+xZ{9Rbx>13;8tB|P>SkC=us7fUTVT<>|TWactAztecQ z6v&6IM%H^DE^vJ@@_i2>y!6t9M`d~usi|O%XPolj$spqS@I}hn3Vf3?y_T2LDceKP|t+i`UYqIs>wuBmwsam2t zzLt8BhkKLlo%+M9GkMsqpLy86ugm*mvG(rvWGvR-VAaby5#5Np>+2B=I4&)U4l1Mm z`Iiic_UqGjGFE!4`Y2?xz{5%uBh9r(QS)Vwe+I$@U*M5p%ufqv_ zq#TbrJ8)KfsLuW>N^R;Y<>;{HWhBr;NvQ|b^|qnQ6VP3Uw>VFSqa!diF69)doQ^)n zJMQDobMAegP)7Z;S>KM%x?Ar_XvyfI*7nX`E#BnO=)tH#Ku13O0Myss6~9Z<5}M|x zC~s>|PrO?_=h7l{>X)`a@<*du4aL?XHg+uDnP_WI_Ql9%`WF>c|CH-Xkxai8!KC9q zp$dQaxvHnC&eUf8jn7nFiqt=sdMfpC-J;CmZMnMbnecWnjQQth{qx8COSAr^L)$O< zoBw!?D&@rP!0wRq>!=xR7R0w!zlQe`hele}=1?#ffo`qlnu2OXQ|wcx;j6eJ4ByV|Va}E=M;IoVGn$F)evpZAs4kcBu%&czih=1;x-C6&_kNk80sFTZmWSWK%$B8`OA7E-X7f+Yy@{vIvS6W5Y`+H#zpbFl20 z)-~!HSU@dFZ$WU<@jsQnxT<(P3$E5Ep0!uQil_N%*zH+#HR$v#{GHd~t^U0W)HS!D zcy^Ri8~){CbjyxwR2b{)16~o_co%o)15_t_=wVi1CS=yE;;Hj{$&nNp$u|l-hh&`p6s)3v z)iAXLOAR*DDFxJl=spA|9oJAnt2hr`DJUJ4jFpbASW9R3q)RzOcd%cSGw*BF`?}lO z6M47R+l@Ckr7fW)EG=2vh>TgV3xrNGqiCVJ=Wac9>+{K6XhBBd2G)iv@{nTfL6lW2 z@fw$Qn7x!K7FvZO^N_?Lmk`&@sz#&SHR?wouZO+9SOm_8yI==m?4xY~8wsJ46+`lH z!_#{Q?)o^ic*v6rtsM5eTYauN6WW?lwz3*7C0Z}<9JVjT!!WM2BTcUgviAhP zaim=?N6IBj>^5IBQf{G$w31Tv|3#zPBq^I69dzHKL*#PIq0dW_eX*khaupv*S%cyp znl%OWDn-ZC6y1jm+{90Q)tW1H;*jkxdI}e&mhg0E>1%?tWKj;(cZm3VFFX|I! zRcve4KH0|BQejcTvVDT&gl)|#qlvFxpAha?WKo1c-mpFzZtBBg0oU`)6n%8KBRYfn zn0-?ppIaX^f+{+0xP8pIsgKX654eoy?(6?vtFB`#9$m4zr*B)69{g*Ai0))j8=Cah z6thKK0%2U*B%(kVQ5&1|bxOa$!~+(@HQIFo}U+WuL@BFskIDzBg> zHZZ^a6>zcx!j&rip@eoEC||5ArWX`H@6vmabYqE{)7lRoNlro#ZQWM9!?b2*5 zkwySeF*Sa;NX(`>HlpV4AaV`l!@GLmeC}=UOtyFHc`v-tK#%f4#1f7WYt`~|WBQRq zZ>Jj5;yqgIaA)Em@Jn(xptGk{i!-0OMx*?E$Wp)gfUL2+U(`dsy0COjRoXTrWY(7tQCYZ- zTdXIWttVUg2^Vj>^~y%;$tEkCk%niz&3a`AuRxyL#*Nxu2%{4$5VaoMsmef_7{`3b z8K^VVr5&{?@PSzJNW7bK=vo}P+)LU04V)N;KPsy<4BS2V-Aj?#%sHqmK9zp@2N`AY z=}m(_$b}Yz%7zw?Ddyiry2c}NCMh{al*NO`{XE~D`Wz0N!gtC+Tk^uK_B6G$fE3=W6 z8KwF3rc7jIF0=~dFmB$Q39TB&&6~#*`uoU;;Ab+-QaXN?Gf8=QL}?y;R9+V3DDsM| z$ZQ#_5*mssd&48y0D^1$Cn-aQFq zb_CH$6di9H=gi+o-4Q!FY{uS6a@Dg#juYo4%@l-G79CSlkhf1&Dwe3hlD~~arWX+- z){>H#oRB~%!)y8?O6O_%B1-d_{zi}Ci|{7vMO9g3vGE+H)XzQLc42F#9^gk-PQt;Y`0j27(((mh0J{~kfn198rR zfqO=kS(oY?U)uSjo#+#OwDTu-;ODgd{2jRn`lMwUgdrkiN5NY-x{K2~C($nmQnr3{ z3eRtomj#JF;rX3BmKg4kB<>=UN@8mFL2v_uChh@&$#XPu&e7#eV@8AtGl)ja01DYm zk-#|6u^297ha=_OFXy0O)=0>h$a5cNh%|Zfjdrs^)+PIT;<|PexoMP7zUHC3zJ33l z_dfWSv3u^?z31M2cjpzLjB%i8#FKX?dXojn`ob84YUC7#Bk@+Xz58$se3(6?^&k;h zfKGmy0mr{9TR#rYd3yc8otGkwW05u4$eMvWPu_DWR6X#msTD+h8mpj3;d>}oL9DNZ zx8C%b$*Oa!?v&fCCRM?iIbFnqGg-0l_u2GDX+`y>JwSqx5;%c@6Q#YTr_pT9gaN?2 ze240TXIAzFcG!WCkM+-lwz9*{tDTfRu;?q`D>hR|zp%)|;h35Ny`x_ZR4h@0C4U<~ zyE$D3{c}l4OioA;D8p;^r@M4~) zPqQl4leK@bY5&QULQYY>vOUt26wn*yXICtdwV$z($VB^Q)N-U2Dz%(4e6l}K8;m!` zh&o%HlZqG?=C9boO11hRtgIR~V{2uqlc85NshSR8*2PCUkfix6?C59~l=++XsP42E z&q7bZGcS zwdQGKwPsyQiLEJYq?nolk~doF+a`@Ux1kK5w9Ybo=F@8aS+9ZeFHjdsl)tuOE3Osg z7Qfbh((?SO=~!plprG*lT69Ctuf;{rudj{emC?%=_x!5q*dVlE{#MX;C^OV|L~r&T zQ+tB4&tTcJi`MpEr0-}kZB$Sd*s5oSY<0;^TfJ$kqQ!~dX`XYgj?KbC&EIt3u;YfF zb4!b!b6@qhZTW`vRe8?A1G|hb>e4!(kHcIi|9OW&WCSs5SNje;5I-9*T(yME61vR6 ze4&wa272z$;~goG0azL|op z+Bb1sBi|zf{xw?D2Ozqo1gLDTUMRr58Stv@qMW`#0pa`FofPb$;4TX8rhsmSTO>-{ z!h-#p_pq%=6q(l`2%`p39qX{_N=rOy)DJ3`Wu0}V2p`k1pgN`^wt<;fv8D6H3+0gqwvt67 zXuzCGp#24%K8k>?#)DmG-=&zpq~HJr1fXF)1QT`}MbIciqn5+a3oB-^MLURSYs?N! zB}Q7D$YGA}6V*z=_b4FLg8?1f0NE@HW0Jroou<=Srw4GF!yqjT29cF>5K9l+j>LNP zxX$bF5Eg&NW5?T*M`A+#+7FQ*167*g+UA~iTD)RR7Wg>0Di>^m+kN&tINa~QRNH`` zZ(W+xIB+K+CXv}=k>+fqIiu`2zXAcs;r-`V7CEg`rkv}kCFgnAezj0g$IaDp&lWGpaYgJhU6P;iPSOt?4|qJb1$`npd-Sbz4R z*B*KKk&Lo_7!w56zx>GXrrg5ypayTxEL@)rtshh9?;|6EpUE(bXu#>jku%9rVwOCd zK#pf0lAMAB=b1-%>m@ zdBgi=)$3ImrDeDc!RuASZMkR*D9o0~L|d{U2u6P&i5Pw=!>rOmveRGABu5#yWa0!e zeZwnR1qs%nDjqvRLP!#K=X6oX+A!KIn(wNi2M}aJONJiE(mWgbyJ$r4GZ|);Rn&%w zBWIGMjG0G|PY^aozCaGAt4{>FkJdtF?w{KkPb? z@*C^3f~jCx95M|25v!jljRirdo-u97wQ*(bN^-)ks>_U9C1z8GPmY7th=D_f)Ec!m z6>8@=WVm9T$n`u$I`Km%LAJih4u^DGa7ft};BCh&2zV4c4C$##RaJmPhErA0QyrzU z=&8P9Jz3EZn^rYLI3#w9s#v0O?OfW!j9QM=LZy~dhEMhf>MR3?tX3P;*{N#7!e*w` z6WUz`n{nEq-Rc~MLq<{&TO87F`V4JxNJYi$1~w;0mIeKb-$2RHhg5(=@?K;vrLtJ( z%%MRM(dx?Dg5*RmVzqfYG*9*-UppM~PxrJEIHXS{9MWf4aZSbjq{M-R#ti=~>nAPG zuZS%US>9F`(X8E&gCPov1v#&U?bWjW^m;6IF{Qd`vHTrEXzWjde$BMSO1xW*8pwH_ zxjl!`h*><*n8|Lp|)Wn?0=gG!y#zTG*?c5u$!^ zJuJ=pEjrTWZB>~eTU~zBR&Uy>XrtoSu5V?dS-Y79exS8CGMZfh%ey(6HCn#iP7dZi z7Iu0`2Y}YD+-E?uj$H=vyAKWGHv+6(zKf%I4Y-x2Q9!F;P0&=+>6GwL?HC0N6WT?T zgg2X<{rd?Xvssi~X8Cdvat3(PvU=GLFl9@Valfuoa2)ofJW{wscqTAY2AyerRPufb z{u;p*{NwoD9c4s{w_@dR!3yb(SetI(;h+r2wj7GPmP*ZG+@|S~TZYkY>@Qf`jpgGk zdXtudypZ;2i3%jF4J)NhWqi)+!|h6~&MscVE=+*%(?$zw>IG`b0n=|`Z!kVM_kh^%hE21kZO-pMud!NCn>ld zL{xhMDfrrHK5b&-hJuyUEd^(r*=~(fAycuH>J|x_Cb~PZoZSMNQk%12rmY9{L}xEh zXH=y26Uaa_0Q4srZhF}cG_9S*&{6|5HO}g0BXX||gu3NCz>6&$LS2%JY{m&esGD=4 zEg(liTgDXg?;>5~5jm5T93#pW!cB=EmjyYhY+(?pnGLQZZ^BItRzZUGJg`fOMP87? zqzeT>{71@`F9||jk&OU$UPINi0$cil97Y|k$zXRsU_oof6#Dzfh~Q^3%u+gjmNQ9t zc|=)5i?=BQTo&Xg@`?ajrf6{BMv@*zbs2Y*#6@KdTgSaP8(2K#%m$Y7b=+)e_1d#r zU)%XImK&}et|xSG=Wu;)K0^m@%girC2PvWmvYQ?q#Cq!jW398bO)A0y1XH zKTCp)$*@QzUVGs>FjLGHSIyt4Dd1N2sNRYtYOv&Qw|HHGsc>wUl%!yJzGZQIhmF+c zMhB{I#w3-?1j|Y)IZ;=>GNm%zf~89@p@}m2nQj)pvHa7QBy@xwmb7+oKxMG&0+E~f zn|3E@OK5hObq_EWZa)k~Im{PG1sKd{mNRN3vHYmu0?ZerR2H{%sA4Tz?b*iOl5*K= z#Vue;3TQQ?hAWoHYS%Urnai5X+LZ(ZAwk%p%aSdi44;I6%J511Q>)}=nbm4UZk9PS zRz@I6fWdq=R;Jc4m=BAKJC<5Z#vs$-qy(TlXuV7{;c^KlS*k);-*W?DhP zIthfXO(B)VI_qZ47UV{vWorgznY)iX|=Aj$~a|+>WNCfaF&A-cIsA zUGF%{08M-htQIg|b;W&UwJ5jvH5_|k0;(-AU&t1c4y&%TA6DcS6Cf~6rCh&AmN$pe zd>t*%QaU?5%U(#GYfMv#sPoiDxtO`8V!ccUzcRg`&Yc}{>6&QOv;i+1tv8~N`9=Ep zsy`KEPw-XI2OO;n_GyDqMa7;}b3F)irxq>>VLndnIOVsO0-Ca44T*wJ{rM~{iEc8;~T@?`<+np_a@BJw+hfBjy} zKOJ!VAFpHHifPvah=l-kv@T$;Wj(;x+L?88c0L>&U+C=oFLF;lT7N;?PJYR=#mSRS zGrX^T53Yk!v&EdG27v;(*&$ws;8=iz zg&%e^&~3jnLi_&tWGd=Liw=vcqufsQR>wj~NeUbbN@=Ohxz}a*%vqMD?`^)|mgX~u zUmoMd)v{-pMHX5NR&n~tdc*C^775Bl&=_e~PDN)CW++&9ox^$lq&Qlzb#8Pk>=q?Xl||C_>=fqda!#Trc+E0}$|34Ifjb|wH}NsMS*6(EpSJsF zgLJvc0ZjzbUO_G|Q%;|{rtE1R0FvNnPdDH z=^`gFRX~tD0wE>}&`c7VGe9en4H7d6G7+qx6;lZSXd5J(Ya(c|+L>QPh-J@T!mN>- zyXs>%{Qoa^)h7U+ji;+IrYtp{F5Fe$95KI*4CqL^Fyi)d+8W|(fG%4&pGNZRD1`H| zGq~3+oBb_w0m#}HsS2B z^H)@>v~Ck2mZKp8_ z6_??Yn#K1>`4?|%*!M_l4DV8H#d?uz1B=>xHS`f-s~c+feUrUk_n$2KB#7;Fb+HoR z3)*XrEt}|17D@KS3{PG{yZr=j&*heFL^xK+zH8D$?AL`5(o4Fz&lvU%+eQ%#&rA@- z1`&@Fb&!IS2nwdRZ`6K7@g^)t!wl_t3RDW%PGa9DN)9nf?i2QBY>bh_On1Epllbw8 zl=jYEEgr)Tcs;H3!9N{eLNh*`@*1*z1OIx%)Io;v;S{+^SbgJI{f2D)2JFgpz7+w; z^Y!OjbMtpztRC}BiG%GCR&XaCk4*4Qn6dgb+4?oK)75SSAcuG7>eqohjRLL9 zMQGQn48jl*a_1|KpIsy_38e{AZcc^GtEdzP(k4jkZk4IWOmd2*?X7l868AIdh|N}4 zkl+g*Q`vstia;IqD*hX1=~r=cBP6_#U>!yAAzsNv$L7ShYHVN4v3(Mp;ef)}!v!De z#=}LvmfOwERk0Bg9xg16<>6xe5R{EGTF8Uf^ae}*b}z;9k%5PcDT&z7Jy1e#3r zO!xgyZ5Om}&e4FEWeY{HB}z+(qG1#NfE1f&aX+1Yhk^$v*iXTO6#O}Yg6~PO?vZt# z->2(lA4zsq%$-campBLj>n6BfGSbR-$ofbc?wI}fvP8pT=8F9Rd zHy|Ir1LiW@?>}|59@EvMG2`p78nt%5f2Tqejh?k;3YhwsBI9&=gn}LlXx~Y;h)R2r zDDq`#^sHf(tkJA3Hk+U8#eW+G17tpCVbjs;>Zl}}vdso=0{9%E^={)Ip zy!y9}s&U7je|K#DgLjE@;qM&?{&0`endL?1z+r{tIOdFK2vvpQgweMB#dh-KT>DJ$1f8|)m^#j zs_uEa=WAD^)lwlQ*t9h7boad7)7#V2@Av#yU0qN>`pQ4N(s#065I&-Ukz9J__6cNO z5fovcpooeqE$(x%v%6nPxJ04fv(GE?E53a`TydxU{egXf{@}hK>K7=|32~pScuolW zLW&o#PVoVT6+d9T5&&GF1OXQ+GT7*LVCAHj|?6JsyAO8{lBfC;cw|Ox#bNc0(?|l2}zkK)l+b@4~{Dolf+MB<+ z`o`(&FaA8DCVP`=av+gxORQeqek7$0#?!H!8c!THdT*z$wRWfbC9+xS?)uM8Ui<0a zeSH4Zwc+t=BPXxF|K9a;KfikXwd?=-cH}?a+^}X{JHf7x&kuim{wG)7JALiFU(B38 z6WP_iqrE3`<=pU%*Wdl<`19A^e0%29$s0fXXJDXs{ons~=EZ-z@#B%1cfW&TaOV6A zGw04-f9Zv*-#vZh{nxI(^ag6<1G!Y&(bRx4n2lV0?e#0a`X%oLW9jaQTzUT&GjE)p z`RU)!y!S)g9Qa>PYRTM=fn4&S8c#>sBdkn6;RZeBol#e*u59K?0Tg$|)p);_@ahYJ zPJSG29JiYQUJ-J59z5lRyqI_8-FZ)5dP_X3x7jU3T|1-RlIw65M+%2h3a9j8#L}t$ zRIao{Nvf$M@m%UiGS-_)=kQDunPhJ%lo`xr2Xh=NDykB06!ogqv+5&2V^ISHWPp-v zYpfAggr!fO?%A?`mzu#3@9&A{;`>YXU}@S(=yoav;eZl zN^*bva59!LTQ^=v|K2Tz_J}tH(X&Uq4GbYc zz)18P*rdLJ-)NuI9M!T!Zyf(S>)|zL^{Foid7+QyrXm&aCKgS}T$QoL*7!QJY@ZS^aK`KSvYKQeVi(S?9BQ_K2W8 zWb2=%sv_kDGvWzLdyH{#=7G1GZnV*r>D4dB4$!@my@r(P_m7Ys8Zc zJSD8J9KTYpEXe!yl~Y&q%BeGYGR_VwCxDd`#Nojq7edE`=)#@X^wqcB$OwAIzx~=z zeoX1zZ3j6{X$kaH#wz2_kGzZA)JMg2%z3LY*8|t4C_#yy}Ya@Zp zY!vIt2e)1y-1>1@B^to!OUcXUqZHhpJx25@7FSCPV~Kb=9aEE8HIeBDHAO4yiisxg}=;ZD3uxbs;Xwx zQbVjaqxQ#hvE*~vbbJ6rIx{dHRvYOD$aEV%qaF4_r@gS=Uf9TorngyZzqFZO;&BW|!4HkEbQQC8|K>tlmPWQTJE22_b)mzJ=Z%&M7eMx~#JS;-8fkEvhA#YH%@ zZ-LN)q6e40Di^!Q8paaube?lhH9kHS+eal$ zgy`HHY0HQIea-`TZDYsmf=>L*l!AmBblv-%q zgfV4hr;)|DsrDk>%*>8dBHrv3%6>CRezHL~{G`;V;sX%k?O=_-UQ2+6#P+)uK< z_?CM%Da+})06|bpnX^auCM3jy2szJ(=UkB#kK!_9I#KxE5{a}24iY~!2d;vf6go`O zlk$@GPu0m6bOaL95ui<(q(}6dr9KCVr%Xp+X%pxTsz{dP4;_Jp=_C!C+61N}sE%uG zPu1~E*&njHn@Q&$CaZgNiOZX#mzpKp=;%$WNO`XzX&Gl-;vzX6+8S&jA-;vE-Dnzh zSt_m@^tknnrD;3cN+7dzRuQ1maYzYVk7h|s5H8pbGw!xz5|TvLe-gF{NDyMwCK^DU zO#<<#@qvR$^)b|!WLxLzYSgH$1lkC!A<#}>ErAXK>j0uYE<1=Zm9{rKFW<%XI$p!Y z0G-w6g-sf*W__RIyWH~fT`rN1_h*wzESKRbLv;@tJc2{}M}RU8{=&+MJ>Qsa+&dL~ z=6LsS>K2W3e1GpL@k76SCO8}%=^Aag??=@mltXSNM*n~+wFg27*LYTIaH zQeJUcqW??0%nK%*L@gL%LYT6$(}c@wFH&D7x|sn+l`Z0D)p=b~4!YlxW^WO5s(UAsMdZhUeo_|yY+j@0t$t8>T#O*+ooC+FjWVbPjR;GB8j z5%HVQw=yo6Oo0^%q|XC8hqoZX&Ot^?$>50#9@sg&Fk*Tc-|Nf!VBsJMi={qhEjLR1 z46B$Ao0y;4*!bQ+jOS;E zoV+##a9sGkSCAW>^xwohuRHS$nCJRMXNHD{Cj6^Mn*fS#fZ?IhrlNbQo+`$uVxqoj z%spAZ`m&$?FXQfpoOwA$t5B82hEO zIc=IfwhjFff<{$g{J}Oew~=1jUEwdLI~#7iJx&>Vq)N%^2j zz+@ z4fO$Ls1N9Bqj1e+ENt{-oE^v*8R=ylUL2u9owBe{Z)1-Z87=2g5jH4Zb2ZhIiZExY zvHHk}U6H^@xeHJbV#Rrs#RVBFlDxp=w;nQk@#gDwh6+;Th^Q^M&6@LCbYU$np;orF zxb&{ERQ1M8K$TeiIh&Z+%a$sQHIHDKtzQSG&YDqkB=!~#D`|EMjHuCe7l$_ayUo1S zy6P?FZJFtw^2cS~{4<=t}S`F+XtK=={2-`Nz{j__$30Q?~I235Y|(D7G0FQd`Qs+cg}~Z zkJCW55_p2ZHUirT!~isU8!RrB=eR!iv01$8-~chLS}fj6UZF{HNs0BQQ&~+tfL_Qp zrVT)71b-T;!k>1WA?5CU!=Ez6vuS|4pS(5TA@X4`%r)xkzE46yc;T78;l9zuquNw> z<*=K6+R`hF8eeJqLED7XUTk2XDB(2L1n`5lqBPbty{H}eQ=Jow+9&1q%M$&6pjYsc z2`8zVm!_<&dHJH${v){DPzShdWT&O}kv3ingkpo49cwbPxHr#NKs1;wIuHqAqQik| z8VqK|yJLqGSmBs2CbSeLIh7Rn;*q-D#@s<@;i=ETnx{yNxnpf?1l4wCS>$!M`y#d=GyG?T{SzjTcv(+g0Di4ek!F z`kh+i9{8SN`fZ%AwJ|#I;ZXeQ%E5WIB2#Ut+!JpiZaV3~FM}E$PZQv z%$o|W7KC?keX!0wSjNl%G;_gxARlCcLM`E1z=7s1L~U$>!UNBB!O3~i(&^8$R!yz4 zQl|?FvJzJ6^Rli54%Hm4W$Kd+zQrM2ivs;9WXn?sLReU^lQR#bA7sABHF#hgWG~=_ zz1yMJ`i8EB;(DvvL!zh|3)wPsTK1SNL_}57Vy(jc%LAg&6N{7TV!{l35=kXpt7X(u zMwP_`l}8Az*T`UBrkS#C0F20UKTh7!{6-?88 z95mES(L&7>E!1p_7HTe11%(N9Ps+`gCHlXlSMZVvC#7c6)DXs$m7PWwVM0`UQNlgU zEQJZ_wJc0X&lmM-e$j;J+!o%1k+zId7}MzP(H@-GfP1Fo709#jrU`jPad&xK6Dle~ zk7{y+!GQG;r%}BFC&L+~ zihPPNVsZ$dLq7Gu_CwK5wL8s7%xQ)cQg#cEw`Ti_-EQL~*O@lxc`ZC9ChRHRdF&}x zgj98_k;R)-$Fn%u>Ub6>TOH4mQVoTaUm=Cm-E*3$3@G<8IzzUXGNlyx%`g$wARu=y z=9h=m!lc4U01KcwM7K);lzH7ehKf>Hr7vH^j98dEb-7!8`2GB^G zWKj}MMp~HET|SyvBAtw@8VfkJhL}a`XQ-Pi0CQ^F%H?j9E=x-$r6qP5yqRKBn<*xh zHfC~W*jQ?B?%u}5 z*meKpl3S7T)>cP`PnPU#*BDh|0=mt$GM+VPn~Eck0jM&xkIUegX$4<6vv@G3r%C~6!XO$ z&+j<#6iE)BPJ|c2!A$*lE79gwqRp)~+T2Rti)bBd04R2jHN3tV8Txv}=1IBrvPA!v z^a@@w;iS|`;uT>`S=ni1Azo4KMG1E_vqYlxTK4q_Jx^bc(6eI=JX--jkXjkFh8cns z3elw{Uo56%60umxZ+voug#eb8#kjZ18Xi*!r{Xng@~E21;e#V)RmrE<@ul}Hx-6|c z;_xlDHRT`S;tnq04fvN)*vC3pXfq4?Qz_VkxiB+75oW$*_6(KebCvijwq(Wy_FFiA z=6}3DqYS2#PpE%`>!h*PXf5A#i=y~Dmq(QTCoG8K3E|sy*Mz^k?RANJ#9KkP7`+ve p#O7OJkND`V1+3^1TW>9>6W88~xN(7Bm4FA7mfH)wVjH{Z{{w52BG>=` literal 0 HcmV?d00001 diff --git a/utils/tests/visual_track_control/__pycache__/rate_limiter_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/rate_limiter_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000000000000000000000000000000000000..c204b9fc7a086cbb3f853670863a404f1747fca0 GIT binary patch literal 17673 zcmeHPYiu0V6`t9hea)<$H%=8rP8#7BK2>j%8s2TTJ;Ai@gu*0 z)@u|3Nj&nLIrq-FkG*r}p7Wi1#{Xz)QYAR%{_d6TWl>4`1QX5?;LN3NU|x|7X{Th! zMj$8e4A5(^M@a`{sVB5EEVDb2oe{Vb%td=*J7YcSP8IT%gVIjT2my^7VW3S$1ZcAn z1v<-!0iA8AK<5}5(7F2p$#{R8EJ-9~W%OLOCu?PhZe>hsb>F_ke+GUHe~GSKs;4ip zCb2!2dF%m@yPt_A#-4j=?5QWm|McwSdoO)5a6nZj-uhtt_;Zto-cFFr?hMKFrZab> zSFKvJH*5B%a=JxQ=|}Z+zSko8+!}1eTyuH5>ApU!Z1T;gCtiE`(^H2hjtou|pPu~a z!^x9xj}IK1{PTN>zrVHVlYu8Do_T+K;ED0q|2p~Z-zMLEBe8wMr>Bm5dg?b9j=c)` z7hWxn{pBeT8+)NNcJ$?m=MS-*@ppbd`O#~TuU>fb3|l~5W- zEX^M%y~Kb9;k@(VP%h-i8R6H%`@`^TB1XiB+L5eG zQ&DS{GXS1nFx$y1A#loY2H*_BslXX7gbcW2OE7O>btuH_7;r)0 zRL=~O?VzoS8KlA8xE%+%CR?+c=sXJ9p$6tr2DCUg5=V|%POSM5ctwFiC zBWI|59z_k!h}%&w{nLAXE!Q7eAV{A%XMSJL`)noglWb?KOHlFi4IU{ur%3lOFZ%)el zw=Qq#+t-ma)n9QYu{nX<+N9Zn*L6wr`-wY{Yex>5jY%`9780A8U@J%vG^Yo{l^~Qu zvJOc*l8s0@DoWqJ)^%UZN||PcShSK>sE%g0LnrRRYFnX6)(hl3-Jq?z{aEkxW0U9y zm1u2)beI5=kO-cj@#i4A+$2>owMGDd$)MeXd{hhR)kk;)uB zoyz5OlIbJqd{1ABWGZpEtv}qK%~{!Avl7nbLBgyo)Xhio{W*h*>U(nehXLUArg}1! zm~N&J{PmGa^WAh5viaU06Ot#DdHU`==}B37=JCEFhAprGYxO4DyovSR z#0GC-6YCwJV)BGP*(Xy_q|3)OYn;*_^qHqTyYBKzY~Y5V>z(x0BYCt+SXa10~vS?CLG~iO|0&;YHabD3Vb&Y5VNR=^bNK2HvhP9Pv z75qEHGgwN{VJym0BQ84%EzD!Nr?D}?mbwHB)eFANvss3qP~*9|3xMsh{_=_SI_KzOjK3*V? zzi5G{em_(sJUPTG+3eKrUR|FL*h3Ldt$wm%jHZwaIB0kZIoChrj+S4$u79Y$oa-No z3ja{dP(A*kInFQl^lYw^=U%VqAM)`HeTmruet;_P;>>KJL+^PeezV}lpDo~%_E}qM zG5pMIxoKt#;45t#!A%5?t!{Hb*2i0T&SzT)ZY>PT`yWJ{1F(g`9AG%g0dT{zfN_D( zYP$2J7AOuA=nEi=7k#w?f(e4)pa=O7G<6aAY#{TIL-f^JjlKXYkz24+D!~nhN{F^a z76MNeA^A3v?;!aulJ6l|jARLr!5GWB>=hrX7!|c0Y|}(k*V({`5t|L2EfwYAc);IRZ#2$(s^}JY1xRnc%bW? z*8D=pk&Yo{MG-9D6{R*HM>>i-N^K+B65z|2F{CXiw+(A6&MNqKhG(#ppu@_F!)+`z z;sIV~V+_6=*}SrFOwmfW#%rTUmAug|eg-=7hf$xffg$ zSJ8@`?npd9LG^Oq2UFd+RKwBv0f>XL!3j~(r}ya*nn<)vjnU?txU%2Qhm zd>J!_w8iCR!`kw*3jUqp87w8}u(BLia+DZx*->C&CCA*;3bY~E68MmKE>ti0GS6li zf=*5Wf=mgbjRR4&$38zLLolxt=z^H^M zoMCwWb$nlSXAqR0nL)5TdX9#5(hW6(T+B)p6lsSMag`J7$Nd~}s?Vn26gc%t+k|X; z4hl1shkn!QZ8+G{qabxX)qhFN7hNvHUfzaeIgqLyr#6Noo_P}1-Uf6cZl1v(R-N-#4jg_0WP@pyNvITdGfD|7@MAXF)(Sv-;sHF_r706Wv zMutE`xO2q8P!%u&g^SrWe*ngWI=!Cy{ja)!?s$FWir$Qx!+>!wA%v=k2%(WPVqk=hLt0Js)YC{dfN;NEV6B}xc&KWnHr~Y=6 zFt3a_cNAzwlhJHz+)#@*Y^aHV)F})#3?~g0R8XPnAGb$T%;2*fce1tLTo7Gh2ih+{6soPbM}v(t+?fokf& z+j{_%UPYJcbhYb@OBG%W_!I`-3_PZQs?!E8#x-}6{RDmm!4M1=gIsZOtnVOOMjd2y zS#jgTVEZVO*`G1>e81J#Z;_uuOY7h?-v`3HWHmu~4ZnKKwYr zuhe|@iI#!L8>JAym>X*FfLUa|Q-6;G;%kD`GX-U;3?9@N^yv19FmU<--(xNC)bIEG zkT<@@joI;wBoUG{zNWscyE9T>PWZaPLlmT6J}J~gq;SI{WG!%hr{{N|3%KU6MZ}Er z!h1YKK^SwX+vj;1;yar@NP9>nH%uka#f!@3V!dk@X~rl%TF%iyjfGoi7yh3 zW)3D)W0o=7R{1-vHT+JCx6^6a`%a^m7tRQru|gDp#GG0nL8!(=5CK@B4XyqwIp$Ij zOTpBh8?X<~Hfq-=gKn%mnFp2aZ}!CJLy$kg1(@$%#XC83ODEIK)%#Y-I1M8qo1sJ{ zMDUy7ik5R*Z#1w3>Tv^0l293qE@6S^%%{hL+*x9iU)+g{NKA~o^r8>u{+maw1lmd6 z%JT^E?I4Oxn}4j2I2isocppNKEl0KtDN9QbalQ2LmQs2c<07CQ=z zxa=s(vfR^Hmk<{W2%qDqCw*|+HEmby8-BC#6~x}ufh+f423L&0AB01&PAoB4G~mYZ zi}wpnGddLZU0^+q)Wi};J?o9nDqdyl2p#YHm7K6(rWH#ZodPh@U7*ta zrUh|tymr1!Jp^t32$!joKsb0Qo1^Ng>m~%wd0CV5DiK{b@@ZYKM4d0GXxL$8km6vV_$jsaFe}p&&bN~PV literal 0 HcmV?d00001 diff --git a/utils/tests/visual_track_control/__pycache__/staleness_checker_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/staleness_checker_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000000000000000000000000000000000000..228501eb88e1c98eab11fcfc6f3a3ba3361fe6c3 GIT binary patch literal 6043 zcmeHLOKcNI7~ZwlkJuzsJX$EwIu9DVKzM~m`k;lRw+d0grBS5Svb%u6FU@WOrgmj0 zk3gaIp%AN0g%7keAQjY7NXw%Vr%Jupanc~4P{EFKqJ={xPW}H~@53R0D|(<+Mzi1k z^Upv3%+5da|1LkJB>BgfFtPabOOyBpgL;=8=y1IXewSIU;>io+-}M6%Y72*d(yMe~SLynXV<(Ok_gyG_b*<3^^t0J}EHIDY$=%e#aZQ}T{1(lZ* z& zwZjZDa;^3~rm@s%JmVc>w6@xNKP^yuz)JAY@L1wj!4aool$!#pS>iUZoZVzOI%bR6 zb;~(o4zQf_e``6LHcO^ioopr8tCVwSazahna?YwP=hWJ2?+KQ(f%WWhN0jopHWKuz z<0aTmzq(B9T1{y^-izpXBJS$c z^EIr*p%;&keBF9wI{e|#JA{NuekQvuOoE)uihCjfF2u=D#iSVj68z%DkEo*!_a=c${j*gI+@z6lbK3I!2gY@ zzd8H)4AYQ&FS!9;DF%TDfFW#1F)2RhZh%tO-7v}+c?9cKM$RCIQH{zSLRALI>tv=< zk;>094fkO(C>}vedZ& zo`*0WVF7^InYWP!;7HiN>cUAQN-R)Z?QXxk!yjUMxPV{q%TaW;+lVObAg_YVbJ%W% z>TKuC%P`HUEdc420LS*Etf?*OmAUE7P|5(hk;uB6M;RlJprFdg8RRf1vZ~x6)Txu{ zl{yI;RoaZu!Oc1IGmJMS0HoUiU>r6!MN zZL&Q_Uc&8ydtKRP=3TZ`jD|x(#V%9MKx7F>&&FmS0^pJ6g$&b}nuQ<%zKDmx*JCsBadLc%E%eyFw>ZX<%?oNsLCKEoy=4!Qu&Wr(Kr6lik=EbU`r)Ed*R4a zxwe~Om=o4E@dEFmU{S9G3vmy}%K^B5$n4f|cSzufDPSyxc5|`qQGf(>>sbgGS*H0h z%X-RDgQb)d`;Yw&QXO>$T(fbun#@s?#qfC(U2>DMmi;04ODg##Sq#an><_}l74DnH zWpg>SR2;hMnQzUSlVO@tO#o>t0C?t(6p%SLva;@`QO3w4Sg$g2206?$B|B8^5USG2 zv{ffFm5NlpvV?tgc?Zi1VV-64PVILCnF*uJ&t=tr4K3>75`Gl45OFv0%kYCiXXPDb z9>I8%e`T_ZdCNl=bJaQE#alTLD)EZoPA51&1C%l}HPSiX2!@63fVhsdK^0!2B#dr< uSZSL6(_*8Uzv?KOK0u97uivv<=*D|43+=n-qUol4P7Ce55569x8lXAWXpBy z*8To-`kd1<)AIlm+j!MA^Y`DU|8x3u_v!OL&VT;%FXiQ42exBJpKtqBgTwJVIuM&? zA|F;c9ggQ6x?`(Dcj`s$&aFjkU)-U!6geFoC0kuie#X7k&CYnXday5UFYWMd^>+BS z`Z~(CmN6Qwy}YAhYX!qg+ABM%wpMja*gB!3dTVt@&DI*EyFz8HMY?;hwzXFGgeKy6 zLaU~i;&_to-RsyoS@!`>(aQkq^m4$ddIjJ#y%KP`UIjQqp8z;huLhi@*8uwUTEN-* zM8G-vB*3})WWek6DS+4Ob%68qsetqKX@EEA(*f)C8GsA)nSeLyvjA_>{eTO5it1%zY%H;8KKUW(887*Z(Ou9 z9N7_U53uWri!GuHMHXeRLrZrz6?)<2C&pg*{vS^FjqN`)mtV_!Ae7@Aqrw@%j-pBIoN8)JTOXvE3eDQ^U zxY&DiwD0u`Z@rAfwOzqT)PKJJ@v-BtjrKipuK(%t$M%iB^E?&OdvxsSeWRxjEIRkr zQ)6%a4A_OfpNt-S#(S~%2N(Bzk7{%9m9aNp9DVI|eq!`s-}%0SC}Z@6ACA5G+#gPV zXU!7a1zr1xkBz5i@!v{xSe;h?^Ucrkxf8>pGZ+&m< z@8A6G6W_gOU9pSjO;P*Mf$$(sV`3Y?hXZYG@>tc>7iuR zJzdS=NHmBSzosP`?&?fdSQ=1kcw4=`upm9Y$x1`0{Cw#_cG^|HBJ_&pV>%1NioexsT(U zk2%_iE^Q0QG`%Pb1(nP;y*LYn=zSczmW853mQezVGI6B#0k^>QY!n_z;mT4*XM>nX(0j4F+Zlo) zx=LVew+l?|v*h**imcCSwxp;+8O`EI?E`ZKu4ki=Z#pZ*^`eZJW3!`v!j3=TnDe92 zqi>Avd*b5b)cb-JjJ|Z7cXj99dWR#UhfiF1=b3Y-PhadkaQ@KK=y1=y{m!{Jo*sMh z)v;Hey!eyvTzGBI=)t|bQ$7FOsnMUJ&wtG8$2Rutn-^dFv43%6W8*(PPZ0eo*vROS zH!d9iu773YyQloB=;Xh{ z3LV{Z0PK?r^^bo0z}U}^u`*EX*c6(MmNu?j`R*xMC;lXs zHC_Dgz`}n-j~BiP;3D;a+t|t8uQZKust@ikk=vX2skVP_8vDUv+Flo@9OfyDAT{aHP0p8e>w(at-GPo|(IegXhw;~w z72(b(F)>J4bkXD_J!FJ;2BYDfp+IZ6J&H10x-qjM0_E@el z6}-*}1*10S4B|OS?e0w*o0HX@T>(*>K#()ynJH=F8mjufU`tC#RL&iIl^<~0a>?#TQFz&GyMIHX-S;-?xid;ZsH?)$#Pbo3fzn zb{O%jkp&AyA@fHTEfM%lBMplMe&hS|Rt(R(EivylkurB=$#Q`&8ktECWYeBaL)x^g zxUZu3iyz`KJCd%>u3h+2Tj))T%?*UUt`Bbku*y|YN@W}AP z5I0L0L9(3f0?}{>>y)ZwCxko^&?Z%iNmE~Bkl1CA+*I#0h-LC`!MF`*vYa*9h$*)k zcOz{jk<16^b)54}eZOMLvD#DXM*IzbEOu5l{(92%k-8Zpb+i8HbCtP1aJWib#%j>E z;4e4%)LpJ@PhH5hub6Ku?uaT0(+N6xA zXyV0DDYLdoDbR9WixtPT9gcWOtR(WMsFdK^suhjEAVrt7Iw2_|M{AL2CwfVgK6kdY zN!d)KI2_`RcRC^yE#*rIOi{*)#QhsqN-1aAQFc0UeN}5a+G0_v?kc#Zb(S)1YO0>8 zDBG~+u&G664%FObxr$Amm?lcs-AArdKTk(<}M}5UT2C3F}opdIP z!jY5C-J7nzp}S{EeZ>0>6M=$3SYv&pfn;BA+#imxlof&qh)J{zM9u5IaNnBdbFVeX z8lhwvP6r;|0U!L0RbNhM0>Ck>H(}>m1)n7JqBb=c{<4wcEk0h$zqsslN!k& z#+T@3d{jOD%obY@mRk>&@&kVLo2>^ctq02@^a@A`W*ZlJX7Xnv6u6FXBtlkYv(^!# z16k-nN2n$DI^OqAKk~H~9vbxC*t_<9-;_hOgT9%)%_Gxi_cjk{btB&L0}J*qIJo7| zvO%vO;=Qlpz%Bc48PFP^{l@U*#>C`CpnbOtVDD=j*64TEOyEZnCXl?89}OyWh=?T( zM|k0^EKuS)3w}b2haf|0P)>1tkLhRNa2SP zCbT=KVni~i&>==Gi8#W`7o@lpLy2^bc@b*{lr~aXeV}K5&ww_iucvP{cKcTkYg6R! zEYc(VXi%X;L@jCL=^T>i?;*|G%l|?y6TQ~BB#M@AZsU@ucoKX+E{Te-p z(Ps%si)%5Ew#vA)6}}&8vGGMTUSlHxVy{eKFzz92Gl4AxzD$5PWqnP$#PJ0>bw2?q zZWs^H;Z_1)Bk&-BhX{O~K!Cts5eO1^m;jY+kdfe|#|RPLN`Td88)4fCgbB0}cmyCq zb_={rEN#caYEKu&YGHAB;3`@izOsJv`c3zw*c(n}u<&*0sjdzS zPnKCPXmp`q8#XI+9VYykmN9qFVgZ0u^XI}*;Wrui2x0M3BvAmVVqEy0I(oVI1SCCCm|5n^ar zIwQ1IsdRy*4J}Kz(KhKGDk#xaY0NF5ByDKf6_swFc8;EkRLA*G*Lu zWj1CkBsZHO|a|n!9u7u^59r(k-D>_gdT%W{GEF zDakL=)IOhE0<;XDIecpA!I;gCSa@V>I{K6t!k z8)E*b-r|0j+XK${Dr#nsGxbU5M@6ew!(`r&qmnz>)DQUPt>zAd%j+X2idOipYPWjKnT~b*Z7s8&Sz$f6B|_?~7@ns+AEKYkG=cUmI3GrX#5+;xg z8#_@9Ds+g5B@IVVB6-u9lmZ1Mu98NILlj2dek=E{9MEd_uN>BDUx&1+Ox-W<5RFHi4m3138tzEz6fxzAC)-qJOB%2h23LPS1Ny8D8NOBJmgHoWN#8uKrISHFvFfHMm zHlPt4*3|E;HjS=I34;p7bl!nfI-E!5k1tClwAxG|fM$cz4AKt|m3%gYfM_{t!#2B_ zsFt%f146nSE)fP0)(zUsQjKd-X=t*Wj~Xx0wYaOu5u*Vk7v?)dET=8*j%hJBGhBIM zo`X)b-#UKA-c3w*#XOh|%W_#vSC)BWZ8kG8Q-+;X`F?RFwO4~lm<_8aZCIhA#Ndr5 z53MqqmZ}+Ud1_i`DbuE=>UoQIgJGwfHO|a|nwRFz6Dtv=>)wKEZhdk#HCM*(yw6y+ zKh)Hlr=~7Z(>!!i@Bt%qGxK06=1P{wO2gbd=#5@4+JmJe8$(~rE3PQ^rg>0Gyaz4g7seGpq}xY%;ijE(TC$9QZ6Gs?^VxMS?8|MQDufBxrFw_^fn zaq!`Y5kr%JJtm$*ypds35HLkqWfm_M^GOLaCzNKHW6ENiJnD_#Kv#j?=q5idy_ybI z)<^11Nhs;!vn`XJheOdIO(U##EBll2pHNZbJ_3&ckU45)QtR$nym)t|MOs?C6jmup zUdk+AxU)rh?hMyaHwB}Y@h!T>9&w?1d&|>WI!~4-M3@O8K8>4PRYDyJ!>QTiC5nea z@)F5Srp%;F3-v|RhdUgnp5i>(pqMLT&%HPAg0Er1?Z)?UB)Ic8^YAd%GpBNAIX1Ky z*ky2Mgmh5cc`>3$2=>gl*hzbiyajb-T(^1g%-7|4Z z<3|Lx639O_@}w`FL)tP&Gr9q1jw@3*<>E|Exip1SE|2(77Yl3Tqcq%Pq4;7;JKR^H z=_#hjgSZ*LK<*dtk09!>BkQP54%TR4f~y1*Tya-0Gq&XeCqI3ne#iWmO+^H-Dh<}CGLtDHE z5wnWy>blN0sR@a4;P{Hz)Y5KHZ0X%>bS2AK8!n*BzEs`` zqAJW;x4u^VJ@k6sclg_Q$VidgBZE?HYIc&(S|dA^trR*s0$ zW$au?VBhe;%Hg407T#JKt(SFKATYHT@1M<f zd7!HoTGKkqld!3&divr`!OD@d7MVGi?I35KnBs-G=`6N`g70vmrM@uR^qYeS&Efsno0W znpmZ26{(6A`hKcJNn&qWMcDg^S14y4JNSeoWx91wbY@Lb$w@a)$`C5@0@5sm2oH6rFg9z~2F+2!Ts^ z;EbQkbkk;XbGns;xzUT0oc5{+IJxQuo;}# zcn1aj4gQf4CKaCZPJ9xL_m@$=a%7>GdVYj4`p1=wI2`rH04~9ad%_9Gw#)0aUYXjg=ckvD(N#S?IEUvePojGnrZj*_@e{@mQLcq1NmW zc2&`v=Ze{z+p=Cr=muqHx3jBT&VB;^daz>`?=FFC;f7t?!@T$0F^|QrT?$_$KJC;S z^D?`(wA!w1gli|0?f zI1V#?@vg{wFP~4G)J^jEUP(;=c4fJ`h240F_!ih0gL=X?cc?}VAk8UCyLVxg=6rc* z<<#grbW#(Lc5NTCYx_)T&Z0?JO0toVV^RkY-@hr%(P%YE>1@OrduH3lMC>fyeo;AM zV>cp$Y1{C}z=|;Vj<6z(dR|jA+Ya;(`iiP3$2IvCO(?jH<@<`{GZpV9?^l$O0~*8x z%k#EtpYkixt4+Jp#-E zUrkts8GdgLJ%G>4R9<@@&kDbw5G#DLG}zv~op~i1zoZKH6L_7#7YRH?z)j#D0-Fht zbspj?*spcz31t#z@-f-PEkg1Wba?p&@VS(YUH)lc^_A&X@Bunc3I~r6ooWSV;<7S+ z8*dX`8-YgvB9#DXVk|dy^YtIZT6ux4)>cg0z%p=H+79t*b{h#~{6{Kb=n9=4-mj{C zzU-N@BU_F>aQJ~^kr%!`SannHdh(cK(>&K4nRIl<;TZ$knq#o!0-Si@N8bX{-wbeg z#<6epHxJdV0p54(K;4>zZ_ThqzxT`pek5T6$q+z^Vo;$&L@a4If)bBRl9U1k)ldWI1uBWTL&<&%(S2urc8|oiKW_|0VwHwgNZ>-_!DVx zol9S|vO=8`7@|W~3A~M@&9p79VG3Uf3;{Hak2{Ym2RApS^;xI!Ey=xb9}EFBjn6~5 zQdF3nicgC2IZ;%v&Bnn`srOJm6{XG8Kt;*=%-pn6QB9eQW}~86YFcNxzD-TlGZ!iW zdi$KU$jre8L2~9%PG-5wR@43R&`E;tC}4wxgxYNQx}b&GOy2+Vc;d) z=Jhg8rVQV%$P$x{J9kPhVT7Vs&o&w`wFb^CqFr4`fV-#huPF9^;~%Nb$ui7k)Cy%8 zD}+L^;zaR)25=11e^#8T1%O@=JyknYhjGajq*ts+_*M*S^n1@t;71ZBkPN+oCJ?E1T1ew`DxWIu_H}6_D&{dl%Lq-yJf#Fc$Nu@f&2i zh<{{BE^g$<^Z`;_vaQAanHhXQNG)k&`-?~Ww2g@=M>?KO({jYu_(@Mz4NyB|Tg=%e zzipA0KFJhLx3-or<-U<6*i|vKAdP832(kbzNCmv|z;>3Z1?9APVO)f?APRV8?dz^y zz$@7+U6z~1e}I5jmZ5w*dz7+(srP3`r?jBlbgI`EVHUhPg;`Kq(DkB~T52{=q^W(@ zDCwmAkQO8zRu~5`BRJk=)S`$q#m69H4xb>#>_}-N61Cj#6;h}pS-yGWrmqIp+$npp+PzwZ{3w-xKz8fGpYy zALOudgh+eWu7I#@8RwAo5@i?q3HK|Vw_MmbXCS1H$hDz0}WmVU*qCsS^auB+z2 zMV$`YgvBPW16M{4NZzhIHCGN?ZqtFwt$Q%h%xyYwc?x#mlD)L3uf>7O1t-2@`%r%c zI{y{kcD&bY+bP_^^pkP^%jaDNhLbPS`B9tqJjl+jB8D5rairZ3aJ5!%j<5OZ$=dP*M(Zr^vnT{q` zH0~KF?~Bxn^33E3sx%%Z`s%m?u9J?oBG9F60htz6 z=3?o5fmnzngt1MRGY%085esxGM@*>qMFBE?nRGZ(Wo0ux7s=QuSmyK!P0xt5Mh-ZL z$>Kp=LeFj==5Ln52wlo%a(Z<)i`9b2f{4}Pk=HAOLTQcFvNboqgYrzVT1XAG#%d{C zx%7k}S)R8YZkrRz-|Iysv%Yebbo z&D_}{s*stRB5X)AH*gkn5PLJK3Lp4np8qs$%k&auomRJ09ixDAw*>KBwF(gyW)JnG%+z*t0$TT)zv zmf%8^jJ;{jUqM-JCZJ6Bb}V!+k+5Nb9}!ld5?f!DM@LOF;=e^@|1&^}Ptx*~qnnwj zKgBu83|wpms#qMMcdN8qj&WUg4&&LaVT0C?41-VOeMZ_tyRfj5(bd6*h7B43m`J?( z%Zyi>5$Hx{BGlZzO?x(Bi6L9_e+Ki;mC%}p;N7HL&P+58Hb2aMdQST~z(AxnYyK)p!XmfN`i zws2WyTd2TlnX@gXiIVMRLtvf-&93g)X+xB2{?A^?Rd=?0Oe_F0!UvYBmxfCmdyPJtEBzALYC`>q;q z-!^NG zx?$u+R6kkN9p-V!uAYo%JWkIg`#hrAOyug?5$)a)4H#WfF_)M0}qtmgKc&);N{79lST1%@?usQ zPw{yh6Mu?~r^>f83tMG8Md=rHQu{O+FE_1K#w)a@b(V#jY-*~WxkbjyS&PgZOvcNZ z$0Fn9rc;KS+;mFD%T1SG#-q_?wppH&oh+WqJfF546J)%3qExjnpnVrzhK#4~dy2Y- zz|8htnIYp9+G-|Ro`y}UQLPp_jH-!WTwm=KCDUPY(~h@q^TAn`SjqjwK9%;ZW~wYpr_da^T>FAL=M=|h<9+8kh=*xO<GFAEM! z*sh?V2Z9ldLK)HR#=oOJi%G)zO%4fbq-N6PNLHU8VdOuMWOc8W>f#|}f=_RFOwDT2 zi*(q)ULiXmwr0me^%%^;&=>~O3g77~m^O3BUy%M2ART$KTqMcYuvhhRB8U~ey*=SfzF7FjD{@HXqW5nY-;N9;9<%F}IX&;n2D&~IRb(DDat>3eL?*QK+S>Gir3TH(=iy-ewTK1oz+~4BO6w-DgTO8V zJp`U4aFD=p0>2PXK>|qv|4QID1kMq-K;ZWTXk?Wc`}I=1LT+FhFJcGg|0bvc`y zm#SUP+fCr6<0+-i8JDKJof9ujaXV*Tsx5J@zEtIN&b>6<=e*&P-|bvRX)7+(x;eO+ m%9(MgnjMy+*eM-S#ozvkq4IS3EG_EN+D&f&NZF`@^LhFqU# z$1@&?pilIaWs0GvTC!mG;ggJ()dQ9DZ4hi zDW^84DYrJ)#@V@WR(RGKpp2oRl-|woj23AqGpABqbd~^8n;*^>T^$0)iEaWt1ZE+u zEfBro!aBPs9OG+?#O!dE=m{5!IpH~wKBvwj=0f^han>=ewph#qSR&2_I8V$6IA1IP zxIiofxKJztxJaA>aIrWSpie9YSSpqPENi#>=e5qab6lGe3WTD~b&^cdT@1PV6JrZd?Tuq*C-rN`sih(*Q+7xJzqs_jN=Uy23?srCi^~|NWU-+==39onT zjdw>+KX>V=pZKJ3U04b?hr;EdRjVr7B64f6F`ythtV)rhjg?rJT$x#UsHFu-UHb8p zW3RmU(Yfxi zFb+6B-g>(x^c7p=h{H zogZvzk)pv+eV{Ge7!5^~cB47!JeZ79IH*`X%WsYbMD2?R1nDQSC@oDV%wA7x`u}W) zkINlAiv6$#kaivEAc)_kx;4HDUrMkkECp3}J^d6_Pg_`0BB5ZT>d`A#gMQo7RqDz&_S;kseE(78u<(_6k4O>ttd#IkMdb-6bc(GoK~#sVNPb@02Gpc11a|{a zd1T%)Hpd4++Jb4bbP~YZk%F`rQg_3@Tmhhq8x{(B#N*q$b`A5Mr}jO*@8qtRzViLA zoN7xIF8kTsu6=|2rVoVNZsnOx3BHhYVhD<{fN~b;w63|>X$tEoqk9zAJ^Bml_fF{K z1{b5nkWO37)_{~VLMO;>FdX)naa|m{fyQU zkymi>AfX{o$Bg?K4Q=D(A`|_5BZrQ~IQ?BDbIwBjxp6mgHiJZIqH2|wO`b{e+4b5d z$;YM|E{+}0H1^QSBkmY90XLLxMJUzd<1a|sJ+Z+r=m5c5s7Sd@vmb?Rb4uan4d z9#Ci$#XOosmo;UHE=#AR-^WRGZjk71Gz0H6W|octSD7;{t}=IqtI$5YO%fgM|1tvE zd9BV!geZ(@@wCRYWM)ihZ~m3XbdBYh&YRYl&Yv06+d8J}EXEF3|MeE>c5%V9#&qG# zn0_wCbfeXnDyX|8p#gQP8DUxy-69z8FSbOt_?Ag@toL;ZX;O)9TnUiq7JtqpI#d{E z1hV??$Vh}Jtp3wmDSR_rh4$v{SpC~Me`$?`o49`4C-!aI{I-s--?5^leVt$SKB^(U zZ9c@-=t+QRNvr*Gg=Sy!c2=0}kls$;Hd<_h9}Ewny2Jgv>S$?SjbIIebqF?mk{gs| zutSj=HeuG5_Dxa_vF)njbg5xA zoiJ|Ypuu08CShiyO`{4nBYQ;dw{?VWj;v-qgpNR4E|84KWb8KTvjY%qcC3flabV}c z-QG?KZ7BUt3GL$2J_P##;8?FmujP;q zrT@Uq(bw&!&<7S~==2(Vw1xwprUqv8O((sClD^4lp__W4@tUnWjVkoEbaQPr5lk~# z6Eil@o_K!j-FNRUm>+0Hg{p7z+c6od;dL}Cc>3#rq_v**kA zNNX`U7n8j^s546-D5iW zkG3|7Bx~SsWAtHg4?yd%niY_P=)}|_sku7|Zvzh(;-#Y&1nQzv6Zimtk3nOw89WHU zvWiAU38i(Z2m$z{aS|HVq`MHzM}S|C%HyXbEyOs=@KPxP)Xz#A5TI61!p%A9OO?lt zl~6F0aA)rKO83G|74T)~w^n72tTjj0nTe120<^=EW-aI6fCWTT!;|BpGgwpuIq~IUo@j9<1&=`YBC; z94P}GB-BXi04Yav03JWciZTR9vsSFcR+J@$vIM^}E&@mhWpObntQ_L;Ist-u^9104huT7jtpq#VQJ>nLi5K$^{qN}Lz-lfwK2U)lTU(BjJE;z~dhkN}8K zImF|2K~JGk29taxwg$7L40I4sm>-xrKnhZvL0Xkw3Th=Nl!7W*CWf5nD_Lu+u(f`e zFA2V?59Uu*Lh$#&{HYq^@j9<1&=`YBz6x83IZ_5X2q?4?QwK;nhWEkz(I_$mNMkFu z9^aba*K}_k5>~+%3}~MWAR(;klas=lAs(;uS^|wRn1piB3e1r*&_P0tqz;gBGzZ{) zGAqguAkA8_3R|%>DJ%t+(&r$2Woe%yov%Ow;|3JDF<;qAGZ4PAl;PtDz!QzHkiJ-s zt+<7=7+P_2vzWA^8e6dvzTgC3Jy>z~P(^jJq8ji7qyQpR5Ak?i&{JrX!6aXeLxWjT z2092RGzU`$NI{A-sMVzx!+Hr0<)BiQjUnfGvM6jnz8x9icAW+J;~~Nb%i^JQMhFRv zeNp7bjBq>6Kp0^e!^aVTCl`1>^><=EO|XP9f2WsWB{%y!#Y_%iy8n=9JEJ*?b=nk^ zi%2kB!#eFTyQTjVx|~`0Kk*9jctE`UQVJt<|4jPP$tc` zhr1sV8Z0bN`nwU|D1$!AIvF9xypzB;%E)QuRm5@<->8Z6GGC@iX!5$YMszAueWMZ+ zO^Y`&dS9mvnxF|=JH7yIgF{e&lbZNFdH@Y*&+!5-n)2Pc8U&!Hlt6dhp;3^l6otZk@cC)tLpeEQ@&H!8`*GJfjElOj$Kl^paU7V3{xS zEHm|j)eL7=Rxw-i!1IDrdTz|Cjj1r@F%|T&)bHcQGz&CAvuZkK6F=R$!vXZoC2sW6 zK^PS2egwFMAe#~C8;I2+co4x-1YZFFd$_&R-abCklD2n~zJ^o}Ah-`n&n8ZH@T z>e4qzQW$f73&BGO0to&NK@h>i2+#{v5)qI+({jN4+0-BQ9!w=Kh_2OAJ%T0#4G5YM zL=i*~Jc6JRfSe6L-`C)s?rm0*y$x}1M(^2x6m4x5;T%f@T+4$>s6H&?7BC!A!lEPr z1?)umURW}^xQoKPQyWh|_``1{*4>j5_IB-pjnApVly{M7&+|7URTAl;w0r9bvMQIJ zf~B~$d;2L^iA#rgyw1}&jWC#mR1g?2L&`u00R@2pQwOjZLr=j%uF+%21-^8Gbyx-0 zYV$g*uTPye5o~0l9nV^c4m6t>9?9U^4L-PRZMK`%X0{T670qF}HiOr(<=X5tc)jJ? z?20*KE?Ar0G4~YKW+zx@Y$7RR;+pO$uVm&G*J33zoX;m+o7s8`E0=o`F2~l~N%9#h zoM*~!WL{}4-pJ^Eoi9#nZT3vka_0XmW--4TuPB)OZl=06vlT&aug1r5U3S91yVC`H zJiA8qv_$0y(JwJB*-@nPcKw;b-+^x*zB#^qlX~F&`|kVl!7uNt0bSpXRuTU!%G>wC zXJ^IWRgX?&s$3qlUdCC6DKdCVUDbTT&-HE#pynIS>4d^9$iUm6y|}Q*{{a9LRXN4a ze5-5Euy1A8p6*RazNn`z$uIl>*?Ro`MES0iu-lTUuQgN!Yf)9OHo16hf?rEi1pt8` z;&nk!p-~2t{90t0c%=+<5K!2JW9k4YNO1;%KI#XwQaTEy5{orrEGr;T_XKA-KFHvy z4G_-^<4<+m#dVsFx4_;<+c&{kTJmQcf)VYVwr_KmT-0t@Nsavj@M2rc(PsE8qEU&2 z22}fI1UT)1XFhl=WAIMJC}Sa|8hf&v+ASC~r^=Cx5*Do(&%N#QY(*O$`}KDEft3=o zh9`V96o@p}Mf;s-Oshh$mKfNiZ$ckP4ATeM1szcs+mCTcT0~#J!Xr*YAQN6WhAdLglX9g&ot?o3eIJO(Kbb78IH1%P7Nsa zQuK%n$4`}da?DuzE)+rmT>duzz;Cl=omhB$;mMA{tg?9h2c9{JxmyN3TR(Pi-X(uQ zCG@7g6>nGcR{(N4)K`&O24~VXfl4|7h_Gph$LoTgLZc}Xr8tA6JbQi`WKOLxMFy}Q z_E<;ZAY5Ms%K%kol!7SK{jn3;n#?NuL>AzcUe4ZK>iBc1bN4ED$gD*IGYCC3cNP#l zYAD*sQA70VGUtP*81P{8o4mn44N2DO=%IqtNzgg)T`>C+xitH*yw z?VR=%IcqxdbUEbVph&-&{6~FCJs1k%YK|&Ca^i3Y)GO@-;CD&Yh<_VF7lOwTV1vmN z_1lddoQ#W(z6+a2CsVKy?Nwos9Awm{lk^ni`+NA8zXX7sXH3jlkrY<26OD<&6+hd6 zCmFY}33fiK45rsf`P91S+Xp?RWQGl8m9c^QMbXYY#~<^YJ4@0Bj;a6VIBvAMRiKp1$&Fk1+hmN^eoWU!;%EQI$r$7 zqSqGz(tBTgQEK68z`E~Fz+G59#N%~AOQ5k7#wf}lDa;=C-p3wLWB@B+k9BkqQWvod zP+>+Xh(gWji{Wpwci-*!^WDxpo*VGgya{>gIBsgcE!-3o<)0xpbr3ZVsFbF~O`TKb zU1r{B!Mn!u#%Xa=+^S5KzkfE|)Mesrw}zYQTNMx4su;(sZWlN8))3d9jyXCbZH~&I zZ3cHEs`8nfc}mBTi#?5u#|uCc(&0wsbgB^1<#dgj)KEPJeQ`To5g~(A~YTRHqyD(JK4LpIR3oFxyN|}ZZ+?Q+-f?C=UXlX zIzLmsxX!}F6b#Vnj%q(Gf2v_=cmCQr4^>g0M^4E z>*ye)KE^UYl^LZV3UxD~P5M5(cbbxw>FQ=AHTbEo>FqFQBsG{-2HyjH-4{_(LpLc1 zSKc>{(ipN@bNDcx2BflDp>{+t?SSB?m)Yn65~xZ0B~JD$sG7v~BDAg-{^e@`4Cc1t zPF-SaPV#p!-Zlk^4USc8>??kIQ9tO;HlDt(Z&B)wjex-+iv%FT#vvZB3wjETrbv|H z43hHf`Dsv}X@w~=fc3D)IywlcpgzN#kO8WsxEVs-GrY|tWmwrFW_F?QIov`??F-D? zB0K28JXBA16zIA%5Z2&GEoE(a^hb3o*csbT&(;i#G)1BmXONU<&rcVy2NW5=de~zf9fZ_}SO%yvqZCA;?sTT+U!uAL z0Wlg11XPcHGL!iGswDyH+fYHzIHJN+3X|WmRkK)<>ekYzbHgs^PSuS+g4-B*SONtd zH8rdAt$xQCkGsP!-cv7VJe@&3COiS^tPnuja=kgY2prhGe_!ChzI_L! zLs%D&KebM6Q^a<)7s<3BKwn_uPez;^h>@BYfFx9@5*37mBD;i3n}kz={6aHv%A$H% z&sNZfv^IwCmT;1wSAhH{09PC~o9(JS%f^2^hqKv^ao-XCoh$y3%lVkM**0B;q)k^{ zTyB2Pif0}g%2|@kS(3`RV<@LQnN!{uOy;ca@($+~pW2?xU70AaPUcqsp+1>gllXc# znH%mBuDDj(Ts;S`b8x#-YjfC&PKj4Ixb^KKx3dm%`-9^;CSJ?Vv8}x3bK1(TG>yyO1?znGRM literal 0 HcmV?d00001 diff --git a/utils/tests/visual_track_control/frame_transform_test.py b/utils/tests/visual_track_control/frame_transform_test.py new file mode 100644 index 0000000..ef3f938 --- /dev/null +++ b/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) diff --git a/utils/tests/visual_track_control/line_of_sight_test.py b/utils/tests/visual_track_control/line_of_sight_test.py new file mode 100644 index 0000000..34a8aab --- /dev/null +++ b/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) diff --git a/utils/tests/visual_track_control/mavsdk_gateway_test.py b/utils/tests/visual_track_control/mavsdk_gateway_test.py new file mode 100644 index 0000000..1221f4b --- /dev/null +++ b/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 diff --git a/utils/tests/visual_track_control/pid_controller_test.py b/utils/tests/visual_track_control/pid_controller_test.py new file mode 100644 index 0000000..adb2f4d --- /dev/null +++ b/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 diff --git a/utils/tests/visual_track_control/rate_limiter_test.py b/utils/tests/visual_track_control/rate_limiter_test.py new file mode 100644 index 0000000..4540d9f --- /dev/null +++ b/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 diff --git a/utils/tests/visual_track_control/staleness_checker_test.py b/utils/tests/visual_track_control/staleness_checker_test.py new file mode 100644 index 0000000..856b16e --- /dev/null +++ b/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) diff --git a/utils/tests/visual_track_control/tracking_control_law_test.py b/utils/tests/visual_track_control/tracking_control_law_test.py new file mode 100644 index 0000000..55704f2 --- /dev/null +++ b/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) diff --git a/utils/tests/visual_track_control/vtc_config_test.py b/utils/tests/visual_track_control/vtc_config_test.py new file mode 100644 index 0000000..4335252 --- /dev/null +++ b/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) diff --git a/utils/visual_track_control/README.md b/utils/visual_track_control/README.md new file mode 100644 index 0000000..25979a3 --- /dev/null +++ b/utils/visual_track_control/README.md @@ -0,0 +1,41 @@ +# visual_track_control（Python 版） + +视觉跟踪控制律：像素视线角 → 水平航向 / 垂直速度 / 距离 PID 三通道 + 统一限幅 + +状态机（正常 / Coast / 判丢刹车 / 降级保持）+ MAVSDK offboard 指令输出。 +移植自 `reference-c++/visual_track_control/`（规格：docs/物理追踪思路.md）。 + +## 接口清单 + +| 名称 | 说明 | +| --- | --- | +| `TrackingConfig` / `load_from_json(path)` | 配置加载（启动期校验，错误携带字段名，抛 `ValueError`） | +| `HeadingMode` / `NoDistanceAction` | 配置枚举（rate/position；hold/slow_approach/exit） | +| `TrackingControlLaw(cfg)` | 主控律 | +| `.update(snapshot, now_ms) -> ControlOutput` | 单周期八步流程（姿态/视觉超龄 → 判丢 → Coast → 三通道 → NED 旋转） | +| `.reset()` | 停止闭环时清全部内部状态 | +| `ControlSnapshot` | 输入快照（自带 `TrackState`，track_time_ms 由调用方打戳） | +| `ControlOutput` | 输出：mode / valid / vx·vy·vz（NED）/ vx_body_mps（机体系前向）/ yaw_deg / yaw_rate_dps / degraded_reason / coast_active | +| `PidController` / `PidParams` | 距离 PID（anti-windup 条件积分） | +| `SlewRateLimiter` / `clamp` | 变化率限幅（初值 0 → 首条指令渐变，S6） | +| `PixelToAngle` / `is_stale` / `rotate_body_to_ned` | 针孔视线角 / 超龄判定 / 机体→NED 旋转 | +| `CommandSender`（Protocol） | 指令输出抽象（供注入 mock） | +| `MavsdkCommandGateway(drone)` | MAVSDK offboard 网关（async：start / send_velocity_heading / send_brake_hover / stop） | + +## 与 C++ 版的差异 + +- **MAVLink → MAVSDK 语义映射**（替代 C++ 的 ID 84 手工编码）： + - 角速率非零 → `set_velocity_body(forward, 0, down, yawspeed)`； + - 航向角非零（角速率为零）→ `set_velocity_ned(north, east, down, yaw)`； + - 两者均零 → `set_velocity_body` 配零角速率（保持航向，A2 回归语义）； + - 刹车 → `set_velocity_body(0,0,0,0)`（MAVSDK 无刹车掩码的行为等效适配）； + - 降级保持不发送（由调用方判断模式）。 +- **异步**：MAVSDK Python 为 asyncio 接口，网关方法均为协程；传输异常捕获后返回 + `False`（与 C++ 传输失败传播一致）。 +- **解耦**：C++ 的 ControlSnapshot 内嵌 `target_tracker::TrackResult` 是唯一耦合点； + Python 版自带字段对齐的 `TrackState`（box/raw 为 plain tuple），两包互不 import， + 转换在胶水层（`utils/demo_track_control.py` 的 `track_result_to_state`）。 +- **新增字段** `ControlOutput.vx_body_mps`：旋转前的机体系前向速度，供速率式 + 机体设定点直接使用（免反旋）。 +- **日志**：spdlog → 标准 `logging`，仅状态切换点打印（热路径零日志）不变。 +- 行为语义（姿态超龄优先于视觉、判丢进刹车清限幅器、Coast 清距离积分、 + dt 兜底 1/frequency_hz 上限 0.5s、WrapDeg180 回绕）与 C++ 一致，由移植测试锁定。 diff --git a/utils/visual_track_control/__init__.py b/utils/visual_track_control/__init__.py new file mode 100644 index 0000000..59b1654 --- /dev/null +++ b/utils/visual_track_control/__init__.py @@ -0,0 +1,55 @@ +"""视觉跟踪控制包（visual_track_control Python 版）。 + +对应 C++ 参考实现 reference-c++/visual_track_control/。本包与 target_tracker +包互不依赖（TrackState 为字段对齐的独立值类型，胶水层负责转换）。 + +分层： + 纯算法构件 rate_limiter / staleness_checker / line_of_sight / + frame_transform / pid_controller + 配置 config（TrackingConfig + load_from_json） + 主控律 tracking_control_law（TrackingControlLaw） + 指令输出 command_sender（抽象 Protocol）+ mavsdk_gateway（MAVSDK offboard） +""" + +from .command_sender import CommandSender +from .config import (AccelLimitConfig, CameraGroupConfig, ControlGroupConfig, + DistanceGroupConfig, HeadingGroupConfig, HeadingMode, + NoDistanceAction, TrackingConfig, VerticalGroupConfig, + load_from_json) +from .frame_transform import rotate_body_to_ned +from .line_of_sight import PixelToAngle +from .mavsdk_gateway import MavsdkCommandGateway +from .pid_controller import PidController, PidParams +from .rate_limiter import SlewRateLimiter, clamp +from .staleness_checker import is_stale +from .tracking_control_law import TrackingControlLaw +from .types import (ControlMode, ControlOutput, ControlSnapshot, DegradedReason, + TrackState) + +__all__ = [ + "CommandSender", + "MavsdkCommandGateway", + "TrackingControlLaw", + "TrackingConfig", + "load_from_json", + "HeadingMode", + "NoDistanceAction", + "ControlGroupConfig", + "CameraGroupConfig", + "HeadingGroupConfig", + "VerticalGroupConfig", + "DistanceGroupConfig", + "AccelLimitConfig", + "ControlMode", + "DegradedReason", + "TrackState", + "ControlSnapshot", + "ControlOutput", + "PidController", + "PidParams", + "SlewRateLimiter", + "clamp", + "is_stale", + "PixelToAngle", + "rotate_body_to_ned", +] diff --git a/utils/visual_track_control/__pycache__/__init__.cpython-312.pyc b/utils/visual_track_control/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..f4b882a39acc3c27ef6c3cec093c5ddbbea6f644 GIT binary patch literal 1865 zcmZuxNpBQK6z(2-?D33eyt3GYP(V;L84+{f6h)CNJ0Vgqu`uFLvQ%oiYsL*#U8CwA zh;Qte7)X@O5(=P@NQi_OP=GjIIOUdq&=TUnY@D0~A*7u0s%C6F34Qqb)$4xS_ui{N zlF7IM&&-vX!zWq|~|1m49CERV>%IL@(7NYN^W9Jycsw1D30v z>vN24;2ZHq-iQ12xsCPnO?Z=*h`zx8v=mku#k3 z>n46lMxdzbjkDDo=c~V7tIqvczI3BB^`La&%<|&YQP(e6j0DoMj}TjUffNjl zP6iXgLzVMW%Zulq96uS4mu~-7n*Rz7WiqJr*~#khQ>EFfl}mS!q**GdXVaZ_CNuCV zX9Hkg{`M9`E6!a)fhBV^sD-04uC()C6z47$A6}{6{|53opb{Aitbn58+x{G^UCGZmFZiRpQcO47b|xcN>`_s7cW##Jt*I~SGs$$dhc5G-uKm|Tjh(_SCgJP z3tyHOFUJw8%-^oe&Xj-ox_tF?ap3_%NJ0$4T1G5a`)=}R%LUO>)F>4Zj8W}t|HvAD1+3X=< zNk{p9YQl1)%`iDs^Ejp*rsHAfplwsOSCLw)EbJNrj-<6mibB4w>V~&Ubgiyu*o6kb z9IaJ$e@HC{>Rqc#)O91mp<(Afv0^u92d?nKl%AheV#A(;R0ghXH3UmFk`0A)0|ROt z4hd(H1cG?f0d4!J`w3+S#GsdB6wbXPnwxsjduj}&BVlsXb?P-i*TXhAj#@AqK78vK zWAxa5_*IhvU!yj&cpkjPT%YKnQ}NfNUGHH!8Yb#ab*;d)X6@rbkS_#bNBubFS$Tg# z1Yv5J=A`9NXFs)Ig2K3VsW&OzC!3&QvQxo&1qlU71#Jq{btqdDL>0sov@3wy(U8pw zHYn&NIt87K zj;@n=nWfHL=cdvRJN~nSI$)S=SX`Y+m}(qsO-7h%T-cYzgzamuhnJ;=UH`i$bhTmc ztCmxf9Y%x6JoRe~YRWWut6VB>EXD}AX*`3HjDOxAerFt361vHUt-u<$VbqBug-3S= z)DOlBfy?~ySKYPagt(p?5b}0@GK{^;g;QYkJ(&ToYGeHm0RC(;P4llv!fbxtZwGIl*Sc0V?1^6nQA(?rh=fWKS%6Oj|GG7YZ&1+t`y1poj5 literal 0 HcmV?d00001 diff --git a/utils/visual_track_control/__pycache__/command_sender.cpython-312.pyc b/utils/visual_track_control/__pycache__/command_sender.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..bb2691a9084800d9bcd41bfa864524335300a6d2 GIT binary patch literal 2049 zcmb7FOKcNI7@pnrE5@NbO4as&R8NtgCtI} zh3sL6-@zc+#-g1_vL}(`khxo&<>6M{k>43}dR@^!7=-lH4Kg;b9~&(_yh_GTRN}+> zkw?bdrKS9sF?*-9a9B@WBjZ<=^2f;R0$Du8w=^~JyIPcRSd2(rSd6HH3fA~0zWnF} z8M#|YPgkZcFXazEjVIi0y0?6Itb8`P$`O5XmfT4jM-uw|;ZpvhQ8@V`@;*5tebgJ% zWYr6Mn}u1rg=ED zueF1(!awV%zrYxv0=qVL4#-VvVdymW|N`)!o{u%x5G#R;8%BA?Q5*Xn1`$vu0(1ZUP}T7au_#y#*CCxxTN<5MJa zp8rM+sWOzR&c=gZ^y8X#zhB~wJ44RXnHFj z3xh-Ot9hX5Knx@kWA1Q2GF~>?3?}1o2B_zizP?^X#1gMhUn2Q&;NIf!W%j`6yNPZx zMVl&z`cxW)|EHsMdk;{xAB*8uQ4@OtiX;mXRwD9N5T(`=)q)|l=U`BciXlP6VqicB zSggTTGkHNZvu_@Z7451fVr^*SE3leP<9dprfhLUpj-x+aYo6C^FiWsD##eMGW(A5K zvrfH=q6uL&P`}Es3!yjBun*R)3tfp{AdA^6j+rvpQFpQtsvOTIldb+Pn>oO4H(b9=`>BaT5hE9F#*!AgINJep!@) zk^Ms-yvD$4n>JBrYoIY1sX9=D=Q-X0^8f|A^1TUo7i_8n&&*ToV++Qk>mTch+hq9M zs_?)5ZbwI((B9eI_T7IYXD#F=Xqv_f=5;3KUMvpCLcelQ#zUJ|f&0?>`*0Xj0a)<< zkGw1KI2m1hUEcH^cz9Bux}c}$jqD+K0xdK?vt+mtLRs>*ieytYWpHT2Ye|A_^DBvY z8K0NM^v}r$che;Z5fNSuq39Nbup&i6bnFp?AK)g`S8O2F5MD>U&<6ukX%x92JSy+d z>ASQs!BZvG;D^kFFuez6Lp7v+3r!MLZLHlh*;PZ(Ro$qC*~u((oZXY%T1C)3`E+~k zv$?HRYI@SqS)={38}??ml^8y+tLPL2NeMvy4lOna0yQ(m z^a5h|3r0mdm}S)hVJ1ftwp^GpcYR?o@D=_L7H9$0PoSxC48tt5PR91v2E=^+JK9DN a4@zb?K4aH0o{9Dvg09AaVVN3CkN*O(`tr^I literal 0 HcmV?d00001 diff --git a/utils/visual_track_control/__pycache__/config.cpython-312.pyc b/utils/visual_track_control/__pycache__/config.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..c32081733931b5d182ed23e0f06796d40dd5b65c GIT binary patch literal 14488 zcmd5jX>b!)n%z>%YD>0k`NB3n*lT9Q zm6!w)&x8a`Ac?b+31)KOY%+-xNQTPHZc(*=8roARU7p%(jpgIVR=f+f*_!#Y-}m~^ zVUf(SH8t(h_ul)y@4bHSd*AVn_P?d4TPS#jFPz(NTu4#B$Ab0{3KVV&28udIaa1eC z(VU@!ZlxhLb{IO1t;SBKm7x*ObeK9*T2n~g)M4&qTUp4baORHGPD`tWrVP|(iepbu zTq>XPj!}6@tCe6afVIY8(|9AnrvX0w9e6uF?HI`nlhl*`q!4a!TPT%ndTpj->(O0}E`<)u)r zQpr}W`I(bvrapiFo2vu61-Ivbx2wJ0*VX22cPw&r_}rYUP4IQP_WONZ-&{TQ``|H) zC35-G$Y*CA^@|ocA}=1B4jzxZ|N7JmBMyOY;|0FU!!PkHTD0__*Wcsra0ReUm&ex? z5PTg={eoxd!GH(L@P=AXcee%JJALuQ-_1aO}myXTeeE?SJ_wCWUHvTb-nq&5f9Y82nEK#Ka zt8I6CyUL*(-&{SfymIQwOOvl(m>7M*v9+bS$uWK6gPEUP`sV8CspId#;38j~m>4}1 zIr=ITC(nO0`N{|ic?RvX$!U;NId{PA>2Uk~vZ>9>cW_QxW;S;9bV6#Z>*}3>S*0Lv zg-JsZ@9=TFM>TGeARV9E`2fyQJOvJiYBg|ltC2IbGMus1)XO+cvb9m^KQ_?A1Ml>} zFCv3Ko(aA>bM&Rj;Wr~!Uxbl~ev!0GioVw|k?^?rd(^vU^K&( z&7A*a`in27KY4BH?JKAqFJ7KHI}GBVyzq<2$q%QGo?SZq@yVGlPQXk|p8I&})e{P< zGFdY}Ju~&$MbQ3<&o7WEyRUYIGG#Ke&)31p>HZGiA(y+mTkyF(`(%bc>G1(5M09;M}0 zq?J<<+!40gk2WY3kT*_bY6zolZvskGWP2bOwi;OpszJoub;puw|pgPhhT@K5)Ezv^}E>zla_NI64Q2$&Dh zxlX_L4p@(D6Wp9zaOud!h_t%{0dJrOv`WJ_IW372bYP>VHgKZ>Im6|0cXjy!Zk%Ml z%O#W}(dY%Mt$>_Ri6B9wrHDgN20#BI04J%h3rm9A2dgBeBs{+)*gV*Rxx(UL(_p>C z6o-o)B9lAlA7dPfq_Os2ENPUxCTTOzLMbX~EJY=4uLp}2+%bao2F)-&tGCnL&btnI zxxhZ}Y6dWA3T7YgZQmD|g5JT?v_Vd7^Br{Ub8);nL%})~cMx$&WD5%{N~lJ#009a| zSc0GmK`nv=X*v;y0Dpe057L}BKe%l$TVm#i^NT>P6%tbv2Hci`9>giRF=oCZOstq( zn5{sGRa@S;p9NIH(`w?4dKy(4@C+0G&!w_%1gYT z9V!UR5fCY_z#I|qO3dAhAVJE@5QhM3`R@S$QqHfqm0xoszb4o)ut;KR!dbbH10Bu{ zJ1RvcFJzLK%5Yhg$jl4njxkk=w6T%o(#E-qm39&*p{p+Ove}RDLcr^Bcf^RV{nel1 z^XYmy`(U37W#{Vb_Pe@y0X0q)D+PW6sEP!EoWxD8hR!H5AtG9f#RQqGLL7oM@bjbd z1DTY;$YHoRaxh;o=`p5Q88~t#ANcz~`BMiDzS2xQr=n6u@RcYO#e$8XZdf7;2AmdB zBKFLswpvNaLR8V(nnp@#SmM+77eSKnv}W+>&{jI(x3y*x{tPVjW;*S1wie)Gg|9SG zxj{B_F8*-0t5Xv=B^wTO!_NypPPT!66?`rdQ1Yoi}KE6%W zdUk_6UkhulgM5e2;|=siCr?0alnVtuAn@)$5(YV|%je?MaO`rEc&s-4^?JD^j;Kb@ zXzNW*d!h-ggZIU##(FGnKv0L^Aq4dZ8W3zmfVv^fM}UD)*o+`SnNY3*0&LFj0|3gD zTM*nlut#DF!kO8@hXzc?o0DXUN+C*Yl$g?RWwpo@h9FxVb}kl~(jid8#o?MoB2zL1 zwzw#~=>A)aHr!aW0gO}3uaTGy;l)curhI7Q7_(GSQtUx;B}Iey4ke`_grn+delJEz zdsCcdIkV2=;XAgY&r(9RfL0@$-G|XW?}1D&$*2?azv}_hM=(uJHgN{FLe-cV*oMXJ z2oi+41#t*=z|ZdhfI`hzgqj~t%#AVmiWIO)aw&WalvpVkDDeHvyYxG-dcX%_0R#S? zENckO97DrpjT#vUEj5N_;bG6Ogk?gr#?aDu*!e3lgwWDsXf`m?6vg}rEhC1O$!7p9 z8f|RxTS3@gWN>_&yQd@IY6G+I3B9rz*2%cxkW)M)^vh;-10=HtRc9-+nk~szH%_Wz zEJ+Ln76!^fN7#uu;z%*r3J)W|Kp^ZwfSyrkK|rivH|EIH?ZI5aoHij20kZk`0XRwh zM|rildh1yEw%}$ly;NehedVYU@7p})Xbf%=(~2dg@vAu;l}>T(j+0PSaBBy}!=@Gg->M;y(P!tZ{2*2ALfOAwo zwO>^cDC$qYt~f^w0puo6zafCN$)g>U_-z5RhU4gd`aJcTK~uV8bibj6avGX?Du6}V zJL7ivtq&pt&rDr8F*)!`?2dS?!^z0Tc0M3mynb(&vQ3d0&g%h8bZ_i53Xec@!lM9W z(?Q%b`>~4-heN=vft=~WrM6dq#j+v?!3F52;71k+K`NY<`C{X##=%FWwBq2#ack}% ze_{8SwS3%`FV25(^w6kRENGN$TSRtC*qRyKt_b)C(10gaRwh6YN#cy3qXIgup9<&_ zk4mh~pvmkoE!gRJ{uF3}JQ`0L)k0mOo1BdB1mKmWCoY9#<|&`A;}R`kcl`KDb<`G@ z*YARz`cu3h;M97NUJf8gDI@f}b#G{|WL zp*2G0ZbQ?$nq)Y0)C7kkxS=taz36~&P6Q8t*nwDaAS4jONZNqdi3}wGw2`z(*0P({ zx_1|j?77+?Evyq4)QLB)e z-yXoZCy%1@oT1+Un$Kt)$LOe4^y~G&7K}-3ft(J_u?3l~g~`X2KABG8Nz9=JTw;jo z3mB0E^fQVopvb`lRBg!6Z9r9t4j?}bKmYvz&;+ty+;(bP$S9>1sU|RHEm94jf3)j* z-F46PJz_zNWZNyWyT`4WgSpQ?0827_RrrTQtaiQtEEwoqZugZYl4rStFQe=MLTgH^(ToEP9&$^ zeS2c`pA)5t<}Dlj0YMRI5@|-26S!a#DWGEh2!3~0&I>JL)-oc_?br8SZxUOd5cj&o zC;mY!a7(tQME0quqn0h?yq+iU26W zQI2kFBELF0^Cp~`eKz^t&p-~~meEWv8Hh;==wtjC2{fy;>sG0pK*G}HAdsIyzA^x) zNA|(`=eCiNt{SzB^os=zl5L~NZj27_dj|!t6#j}q;St~$=U`CNAH9K-8-Sh(J=n+C zvE=Gw>GE~Cy7+doO%l*1P9v{69&_4&|JXl%=Xhj8hfg-_^Lo(cv9}bxFpA> z#FhmNw1NWxhKR(s5ci-gU}f4b)1LUHYr2bEf~hYRjzS9|`1$_{0IYu4{7_D)S7M#P zx^cEpVv8WnT`uG)?5!4BLmdh^rc<{4K${F(!9> zUiC0NyhglllQeHLVsgd{7Y;8U_KIt^ONBcSGcRmrZ<+INnDaw*H_gRE_FHAkZncB z22r)wFmbA##BA=`wr(6+OPonFDGdW{M@_=6_fc0*Vr05;aAuC>Qr}UH5$am>8h5K} z(TtDOwZ_zq8ZvIR(wdY#>_TX^OpQHm;mzW0Y$ry$QB$=*v|3 z^uuy;&>e>+=jF~c#R$kAx*Do0mVho%F#_z4a%PVHQZW&7dX5V>;fDa?%^*!R1G3c@ zjsyMY$nyaJ%?b`nYzjOx`)9W3R>vf z6rH;pjbg_{ZvI%o!Wh;j$#%cQJ|I5W0<495kTvfS#eT9JZDNM&2YWtBs` zxMV$WW!E8n+r;j)JxaiLgE7$$}T`CmkrpYfQHnw5chq%af;%I`T*_@pxarw)zS+RFdlvywB*HAIp$%P2#jEc12F4o(68^jyMZ*%MqZ&l~pz_ zyA->^o+$Q*9Uzw`u(Ba1Cb-0w>3yVb!UjhTJZfhIT#Y37{oI9?D?)DXFC!DoOZjJ=J0qV<*WznyX;F29r zzLay!sD@ypiiTia?Kn+Qj)U=z*rqsQt##oPRQMP8`B9%VSKKGD3(*zNm)K%Ri6{Hd(Tgrp^d7>RJWs0~ zO}WeixHU6#nV`%%?_KvNcKtnBF+Mu#+w~Zg(7$ZGUQSFs-M0sdwH}cOlB;o(_?V|g zx)P0NzeCGKQ|P@O$GYkuKF*s{t1`;1<>G2#RL!4LYk|faP2*}=1Gs!54}63ejjB;u z>e@oaR;Z1Qn;#IH2P&*bv{gqR=oU64V7%n+FqVfS z7m0oI^jh&(rsRgP{mg&f%cW^yveUq$=M3ImF%n3S-$daXbz-xe-k39j6JAB6BN6INyA zy!9F*m2JFsM6&Jybi??chn}ad10a&DlGxSaT3n&$?;(*s2SSOXR*y96C3ZuYwGXZk z^VdMQaomsLrf4>WVpDIKOK+G|<}T{%GimMsdW zW(=;snOaV;>u!{;)3Nv6Of87#g*8z)?MWD0hKru1=u{UyQT>44!Oare7-pA44{BCm z4=Q0)Y3a#!+xo^rhI#uN1g21OD0eg$@79tB%J|h0+a&CP5ei!Y^kv7~_13Dl z_QJNg6y+M9T*oKWQLgi~#nKjR8Bb-EV7FbK8(2Y!Ap z0L_SwNbC|^FXc;YA*8AiJ$Y@5RNfRWUxh}rh8WRbK#4A)WTx94*cW#igUbf8@yvlx zeU3_vyMn|+!*`8ZP`Q_iJ#gI5K+S|F_KKKe@J|ava3-q<OA~k!E5>@S?osg9rc|@n3%1hemU3A#GIVyo5B8AdUQcCU+aDxUGH0xnAIXaeUp(@}bH9vjGKvR!XH2j@Dd)W9mb%?H@0=j-z!iTURKYgLb znNU8_D|L=JW{6%c@3&~}M^k7aAFeHrFivkCo<>H%*>;DRMOk-v>;8^z*fjIb`Jn zca7XGX5QrjC88pU%Rk;L{(x>2&7GjIW#Tk{SH30-3=BR@vO# zLcX&nK~m1ZFXbC}{Aydl;uVeNz(qX>2)IF0~CM0Uu^5yB}fwjjWesVunA?un0b znuH%iUN*FKlRda>fOm`OwSSzjX8kusn;L0y`=PwN-~q|D?%GO`-8^p1xMeN9VJ$@- z7z}=HL3l>*8TE|rxmKoJc3lVF=0DCFk>?NoB{2MBwt5panD@{cA2BRcjB7KvsNr6T%-8jO%Psf^!Gq z(e!^A%rtX5i=xy1ovQgAmHRs?FG7{vNnz;1!OA-nBzJ5kI%_a+hk|4_lQLMpH86D9 zERf1>8=*K$iZ+!{goGlUP&BI%79e4PPFSE440OpXsBno&z@mjTWuceNQUFu}77Gah zj%sHq04f2CX@rnMZ-jiuQ%07JP71sa9Y=*B1|R7Frj zYE=^{U4z~M-*xC6X~9CLhZvM+x>yG9gLJmgN~aG7W+_NS$MV~l&K6}Te2@@6NM|cq zz~#(Rkcj2b9;CB*6$%a#f`fE+JDm#1hFJ;{u?iR=oz2goGeRIRNQO%8U_w4*_&);Y B(`*0$ literal 0 HcmV?d00001 diff --git a/utils/visual_track_control/__pycache__/frame_transform.cpython-312.pyc b/utils/visual_track_control/__pycache__/frame_transform.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..6352063365980f0d9b94bd6d5ae3e1b97f548603 GIT binary patch literal 1562 zcmZuxOHA8l9Jk4n08&@2swz#LRjnH-l+f+4?vY7TK|5@#TJ46AEH8EnU5LBS4h$3| z7bqlZAQ@##NPwnm)U>dYR3HW+l&RMp2HFK#JC%HnbApMUcG}-ItO)T%-{b%K`Txux zb#)E`)?6xnbI?o>+Zu7O>h>Rdm*Ej7SfY(!Nw&&Iwvnuft76TtEwHVyZ5(j4pVxE- zw7m}2K4fk)_f|Qp123xxLYXR**6x)TR(Faa93FMy=z^=Xe0Qf9emOi&;b&51@G*6@ zwM7$0;vkxG;i)``@pMc{k0|lG>e{c{(tyK(b1S$oL0xHRpm20Rl?L(rOnEF%A!@zEk;Jm1kC-Y+8F=j!x(d;yj_Ug$+k7pEEc1uu`fnmQcdi<%u$ z(#4%(lI1;us~mn(jt#5pYwGf>B2BBm<(j%!>P7bbrVqeV-Mfl(9~T~}^Uv{o5=%uK zovkFJYG@Y6epIvL)Sn>$EwBCrp{r|)MpR&W@JK4FNr=-`TF;eM^AM>4r$k0dtC`Yr zp;U-e5=#!8k5)#aJH<$O;#uY4j53zR!*|MKIh+{+|G*Ce4-Ay24@-p%jwY1RUmyw` z3mJ$wlLe5{@+gP^2dC1?)6BWbWDKv|!BdZ+S`MRJcs`PY8IR{Ja9;S7sw>?ly$4S2_8E8|n<7?sY@0 z;qG(m^-dYNf<#g3$SkN-9u$=LQe`}3L<7l!I2;sZG!(QFKvVUX8k?OII0ts_kD6aV z0uBn+q!Agd80#=t=%-s_xNR7FVjOKhLx>|69dTafr{B}fF#eHMZUl0l1h_rdJHxnSpX@5r>AMYmJ_R zggH&Jxr-4yWRr&%WRuY8ciNGb0Hj5Mz&!$4;gCs>ObW8a&m&MabBrJgcHmR{>))?h zK5oB;_!}_4EsV&tdw7=5O`NClN0)B1eDAv%R#) z(|(RcZ@}kH!WOQ>ArY^vM0H)%9=69$|7AZTeg3z-_7JsIdn!SOzfOFSW+uPftbI>v z-Ku#r_I=o$_%MBa@{`S)ccqrCniH{i64s5Hvm55KdjLu}EwYVuNczrx0 z$`&zj%g5;+JRy+g1UjLK?Von>Y{18ThE4;i^@7j}$F7Ma$$zS9N%P)Y1X;UlwUKY9 p&+ig&<&Eq2Q#!Rpo!>P9ck6;HZ^;LixsB#4n-^MMX*~Ti{{djMs`UT> literal 0 HcmV?d00001 diff --git a/utils/visual_track_control/__pycache__/line_of_sight.cpython-312.pyc b/utils/visual_track_control/__pycache__/line_of_sight.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..3553ad15bee8b53622f0d8319f7d0fce9046bb25 GIT binary patch literal 1901 zcmZuye@qis9DjGcwyc68YK)tj+osWm3Czt!aV}HQ8BL7I%$6{gOV_(%vGmxxs}a&a zDi(%ADH051IBXSd)d`&#DNt#`pIgFkY?-<2k7eHV%3rXtWXZBW_I7SRSf7m^pA z23l=4ZDd@V=w%O?5y;-y#gn?ILTdqO0IA=HQB&VCK^nKlMBf&c+n|U>KAV7-R+swqfFvXI`Cy? zY^0)*XQ!iYR=m!p;}5j)QFi7&TM3d~AkhX}^??WM%-72Sw#D>Vr}`+R_r~=5Q+i~$ zFb&q>t@jA*KGq>SyxT>WOA=&udW_vUJw^mGiOQXJur0Y;1Um$zbzOsMqyy1R>avSU`bcCy+~P@lNP?%oRJi_iBzgs>Nq7gcc8eL39~)jA&9^sW(bk{*p|Z8!Bh=d@7& zLUMq_%c_$%)X6a|G@_**=~r&Tg`@|A^MP>s>b3dn!OU<(AMP`<)ki1OSK?s#py)j# zU>T0Ksfj@PtB5+@uYDdScR5ecaNKEN!l0HMvs49(J|v)83ZO*ulwjg%_?vjfW#P?1 zrk3F?gcnRMhPMK5Zf5L7O7RKzS;2Es+)t{kSVg$w{S22bD3ty^K&Pdn+e^l3md-X!NRjZj1W4df5OOmw96 z1$IpT+M&?%2;}Z^H zEMkRebjYU^y2-7W05-}>uF35^C3*x-aymSM-J~#*;BjG6cuX>sNWv`3{&lpby6yxP z>#?J;1`t>06nTN;vFH_c$bux-`DC|8s(aq+u6y=nH#I9o9Ow4BWsYlESD1N4Uc8Dp z{sc$>{ZhKV|J9z7pUXDRZrVRrc3`&nz?0H7-St0|ZX05v)`<1{(rwwzqjP1)W{Zz4 zn$YU)g@PxWUmmK79*G=@eHfQ+e>}Hk@8ivT6ZMZbAL?L!EP81PyddQTI!fQ18mlhg@%Z4^% zG%q#L2`n^%|Mp)k8Pz6k!7;5HewPi57=6xe!sKJ1Fr+Ww5?I=4yb>l{21Fu}vn(bh zR;BWtk&g*iLOfJD2~SE1Pw~1cs+59~ zuF^T_QrgLsG8Ew%o~>e?`cgebbVQ&kQzA?zN+Iz*N^6xGbYwl@^^Js2cuyC`8NE7l zV$ErqkjJb~JyHB9=PoOI&TSWYZtTJVW$y>vSM?q6&-vR@3_S&in7vh{wyNkD# z+daY#yVv5Vt~SDq$abu`$>02va`yPx;K8x3-6+^bAmJV<=2kA}l%CGOiw*t{ci$L1 zyxHs7=60zB9p}b6x+80dF4?;IksE_8Mt@5)XI;uoUe0KLv(mKB-*Fn&bz|@iqmkpr z>)#%0+pi3^gB)J_4x0#~+4yRS`>MH%TLR-yOOoKR)e2Q^M}^00d(O>!3;b^mj9z$E zX>L_sK5#F$gn!xrT4?9zz*ZSTVVjlIzr zINAcs8|!KIzt%Kf-vHtuZ{zsutz!cxHBo@Pcu(i>_ zuE51tfkf%=H~L#&3$&e8UTzCq>Hw8->+DsMfNJe;Q#~U*_*h@y%yFpuU+Ed`uMg}$ z10upixUiQGD8~-^Up%ZaL|MntflJIfnYOyTa^j|HPlelMuNp^ZH&b$Qk>*94YCP37 z9*5@b8cYf|LLWh;lL!O?3n|s{R4L8t1cs-1rVIj0BU8%qw4g83LC9cA6AXl(!6yib ze4=0wjM+e|_Ud^Y==Dn zb<3>ERo07wr$%&@IKuH$l^p93KuKshfKIZLs#MWa!A@ce$6=6=f>xA+mzZ@W>&&#A z7?Dgi*rL@$o8+;J9)X9gW+b7iOf&#v38^{mKaUm{Zrv=p%SF4h*zU1!b+~y5La2a6 z9zpVKt?^V;Nn7KE*R7g*q7k(kjL2rIa8-D0wz}MS%C$tg??M4G0Mrv-cHY~0uje%} zEh)aN+$N?qIRtp(h%OZxiSXM4zt9%IK1wK1k%8nC(YqO1Pb8#13!g!LzVoQ3{WGcq4r!9Nw`XuaC$=5ik1y zX(@u=ym`cfdQiW+iOB7uCUHrQ^h}gIy_1R@(iC|bR<(5azTHIey}x|H#qk*{$qr4V*rU z!(KkEyx0x)J{FOrq{a-@-)i;{_OfT9b8s4gqDHPlE?-)=rDpE`9DnvN8j0B;;;Q9o zw;exn%G486W>p7}4Qrow2x^p*^;PcjazT_6cGyK%g{xe|yhmm#TxITebz&+uXyJKB z*h$1RK!`H|)Xj=(*D8Mw`lSv40rG7<2{L*^wl6)i({jKvlD>2}eW@=kV60zpw<*njZ3Vn$r;MJlKF5B)P^!CYaufxH&NG(8{c?p`vainE#Pj^ z$Ob}(zj!VTBcm=A8AmXBMjd4W3lWp5jJn+fK7Oo?6BNkQpx|YI(lo&*QVXx**&kU2 znpVsQBjW@C;GP~c5SoC+uo?3KHvY=u$oG?0_J7?0p1QM42I6HP2ws5S?O6AalB5tUrC6hLg;(sE)ggVhp5Egnt| zVCEFr6v@XipI76QI0LweD72hZ`@EXT+nm*stgo$8%LJr=OkGZ!63-wPmy;*L88-Dm zu2Fii6mj7qW>exU0A@xFUt%62W+TW2AmJg$!Ka2Yd4?pwRVaF`*e2L{2!eI@#1C%q z#x6ppE8sW|2fK6#cBwN)Qs)k*&h2u1o@&0HIjhrkz%`P2|8VC0zKpDqj0M9P3%d0q z1%<-}h5bbzXZ|&JDA!kD87X*hxZptugfkur(Yl!re?xTB9uBI2BWW4I9Fm^ZnRg(s zYuV@1=0~}%7|vXwa-Bb%F~6(!Wf&h)mVO^4-7mBrSVtFy3_a*ITZco}3ho z{|M@9pw>>K8m+m-`DADgRXm#+n#*8)KH`TKSQkL!atc*Ehq;`J9hawL$K?zwHm+n* z#q*de*=juxJFd*85D%OH&QoE&VQmV1#ge(!K!3`xFy>PO3z$z+XsAy~K%-&$8VD8* z)4yDXbz3k6r)I&a^)S8qsZOn%$apFvcaF#aD8Z?9F;1;}L8m%3lt1Fs_X1(8Q-^Pl z>U}ScJL_W{IM(B6&EqDV4G+MmuYo^4H#m`G&+G!N6U zKR=xa8qq|PAW8UcG&Vta$2mU8E|?Gx~1O%2%8pdc#Mjss`1;%XcXe(Xe#Naayd2_d}m}Hx8gh+)z!IDG^^Cr7F%$ z!DE_;iFk=Q9iKv6cDNMTWTVaIbn`V;SU1^hzlV3~a8H8G#=9Lhn}|j$;w3<4;1X4( zUNuFuW)aO>rYqn*8+TG%hycA?Ho=|KQ{iw5@FES)QC8))dsN#Hah>lHRU|DM;NdPJ zz$8(+4xo|zgLyW{=-9kN4%m4%uZssOVDo~BNo>ZUEg=F$@GeqBt*36%GuVvYML`0E zZ)suwvc5$@MEM@w6vFbRB?04C|%DM2GfW>J*Z>Z{4O>~py(FV zqJM>2_~y(H(t!0XwA{inL`Q}PQ`8#6ZlDf586;SCgNb6_)^hgUkh@T)3UwaenWousN;Hk1pVjq1~YxCY3yv|82Q zuds>!q3TplT&?Zt!{R2u;bWDw0szE4in^&wp_mXysMLQFP9fZN!>p#6BZgVShFOPJ Weqp$~UjL2$5sKPC-6CqCvi&dA%|NXH literal 0 HcmV?d00001 diff --git a/utils/visual_track_control/__pycache__/pid_controller.cpython-312.pyc b/utils/visual_track_control/__pycache__/pid_controller.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..7fb683deca38cd81d6bae29674b3f9595b89f891 GIT binary patch literal 4658 zcmb7HeQ*=U6~8;3J}k)}#bA)JvFsQd0WlfAT8I-`Y&X-Cy0n2##^dHj)twW8EU9-V zQhVeXh1f;aRt_+;`Foz%>ASV6kQX^eLKM#Zcv)sjg)wU9`_lc&y?QMXQ+^vW6XCH)4o6XAPtaAH; zy=KP_yV7?$9(q%m7>e~y*?F#s<2kR3d)l>Q$F5UuA>i~dBJXsaWL!S4$oo9I1m3mF z?`G4yhvTbUe!m&kijTId!}Aa4FS9^MM+=8lRr5YG&=9jqb17 zZI4f1js0ORrRS&T9)#XN?R)JtKBpks)tQdi=%f?a;!LlmlhWUf`u`ES06(+R-y08gz|!%_No9IM>FZJYrlAq^ zS)9MDt*=}eQhKHz&UY)5^YJq`HTBeqbFqtq=;RUz5Drq#Vx6MX<#7r^0?r?`=z(kY zyHbF`g)YS`H9=*7;D`ni07xAN2sY?gs)6S8oPpI%ggJeR%fg%?#hEc@ zOmP;>nGlBnDQ6vZvqzo0vsnn#KpR3NU`)L@1*f2Nj3wS2Q?Fi+pZOrsfj|p8U7Y)U zZ1|1EnI5HcQ2l6bac*w$&P5ongaAwkuQ8Yge=>E_4}UlOSvkk;6}eY=0I0{^>=xw$ zmgC*0oTB>_$27S;BF6(pxhC1_3yA)JsEw+3SY;E#c%980!^mcaY4))J52{-k=C^?M zRF9Ei0L2W$XTid5y?gbA^vQ$O&R&Ky-VA#z(nX;~E$9x9~4SQh4|(gF57~;Pf_gkJov|Dp1kD(PLHuEuXROA8WtQRkn4Pan%PyBe z9h$$*@ED2p%OlgZAc-V`26UQ_kwSrE%@SJ``2#)z8_Vu|pEIc=xK>4R?8$ z`r(K+7Tx;urjjO3R~`Q;1N@}z;-Qg-c?!B@t(DR$f=6(3wM z6-F(&ebr~HBbExuQZbf)&$9E&(u#%31JP|eC#~bw$hI13Tg^gcO|){yXmBJLsXQ!I z9$wN@WwqpjbqlOhU<;dJl8B|`o~0yOSTbB5DcdcF*cD(&vsY}f9hlBZBv1j^fruUQAJ z*K!AQ7q(YRo2tXv(VTUCp0l1l-&xoYN0$DG4A=H_Qw@s;aJ#Vd7yj>FO*KDw^Ql+1blj>4}!m97pr%W*r~=z>=**l zGL?hm@SMPjnRKVMXf<9}_x=Lp1axVKh_6s0(v$N)cKsrZTf6J;khGyREpy{S>PQFb zU_#AW+H*qb?0`o_tm~E@lI=L86l^g)#hE1uZL5&{o@O zHCJ@~8e|0&C_&k^;B`X0w=oaU9y17Pg}r&_-Ly-Jhw7;o%cAUide^3 zthX(`Fdv`3tNMrH4<^d%dG^=Q_n-P(Vx3c}zeOFKjgLQykKbae53`9oed^FHwW~e) za74dk>H-m}F+z);2z}ZWF=0XWVz<+fiYA#%@R5D897qVs6inKXGg?06qtdbT(dkt+ zvQge+pqd!E2HvE9dR>`pV;kATrTY+8WiZTsEqdN&PrJS$H=H~lyAoFWW}*v2@rf2z z^stp5flPPRMn??wE;kUz_cjA2$-~44%m9(die=e%!`7LBz=J`46{!V?(a zWdl-8plL~IF?~0H~Ry%)$gzpC1)rY|f_@M;a)!^;;!hmw& zr}$`Fe6%C};3lA1>Fu@<`|E8)A9g1Orqr$urE@spzp2dL(eb2C_kwLwh~z}X@;rr+ zESK9QyBs2hUV*AMV46UO;3dfd2?2V2h=Y|rd^GGbC~rbzR23jEhg)x&^LXV3uWU0! z7)cQ8@HRM#^MIsbh_hi6F>@Sog5#QZY|2h~T9Qu=y$r;UmW%cVm_N-smJ3P(OwfvR zhzXPIz^4Q`!tgRDZMa0HFc(1&E&<>=TL{MTeCP~tHV<8jtsqUcz4aQ^XzNoxx*5Xv zgFz-i)CLLGRu+7QW+;^ZmBHD2B;+vGhj(jzXdSfoIH;MD1!(<}WK9K^^gcm6+xR5a=SWz=TN>v@*9b z_*JrgjgSH$5i>L!PLhQNiF6qPGCj3Mtu;4q-<(&CO-@R7Ab#|BPG0YOogenU@uYCy zQsKU^^N+#<%lQSN${#Bit);MflD}suf6p+#l)o=v{;Qz4|M>7%qxM_&$zw}@#L|(P#|6g&OqX>zFTcmv;afBp>G&`O_PG2zXUb6GS{VdW3}ZroXM1^{*{6mlY6l`$P@E|PXQm!OMAlmM0@}&_wuE~%!lbHj2@o5l zxgwE+sr2QD4oOZ`adXiNNqjEc17{>F5O8Q?=)vkvV%$Q?-0i3rI4)x0xOxxo70KGl zao@t5pOR#896Z~=Hr9zAhm6Tvf{O?tSB{t)u%|)5aua?Xwqr<3u7)(6OOyy9UPvw= zt!RyAjOH~fZ7dx=w2EMw)Tdt#CdqEqY%&)2f3k{T3UAk@$wNu9TYHH%?&{Z+cZD_O zU8`nf+O|qlaH8fJf+=YL#u{PhUXXuRL1pZP8NW#gc@cHL-Xbf~s8S8DQosHYYy%#N ww{!@|s)3>?@R?#(tu$2{r~#j;(te{hX*Zxs!!_VKRr-%*Csq6m0nyC<2VPM=LI3~& literal 0 HcmV?d00001 diff --git a/utils/visual_track_control/__pycache__/staleness_checker.cpython-312.pyc b/utils/visual_track_control/__pycache__/staleness_checker.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..e4535a702b0c801a5fab61005104ca51932a01ba GIT binary patch literal 1033 zcmZuwO-vI(6rOD>t>rIj;(-{m2`6j-4JIZAy~vrEz=h2)5+b)^iVlX{a{wzvq z`7J*xm>29~DV(?%*`KARDm6x42@6Eh@Z{~a7d@V1xB9PhH z$m3EYLI>125zDc0#DWn)0%}77gBig9!wAEK4eb`;T%W1U*j-X%_Wpt}eS1vpU(nb3 zbK8At;GG(q$)^Hpa6(_{)#4wr(NQh3mK&YRrv?xFJyxq4-%yj|>^+-}RYN_xzgLaD z$quiu1h->?Jv^@EZMOO@r|flzoI)JDi{mAaLL{-?>ET7MfY08P6^DpDSe7}y1M@CS z>N>1&5Ix$b&8PCI8A0OZ`t0CRHawv3Z0U(*&Hq|okJq^c_Dl3u-3Cr_sG?JSRW|L7ZFsY8b&_6C7-y~fjJKUWj<`kzJb8b0Z;*p-vxva|}It8v> zB863g6)*Ad0QIRDz@q-qHBb@c1CU)+2oa{+;A4Eq1^XxVQ)rgY;A%ps1=Se6SD3Q^ zF96Ge{Cy&}@eX^P)uRKML^88Eti4)RLzCM4dOkIjon6(27oc6mJLglg#ly3W|KzaJ zw16XRVq{ede99z(nS@_|z6yX9JF6wbx$)^-f3RK~9D@jO^mrUlQo}<~?X<{u|2TZW z4EX<;9|dM)5-%YskfKlv@Xn3{TZYuwGc^=1VxQTVqjh)QW;QozY$&eHCn#}d`ifVg z2+)(}D8{Wz02QGi6N)l1fs&~O0l%A@Zrkq@=@D_bn;eS6&PxL31R{BGt%7C6?p2(k zY(I;C_Wvo?*`2mHWv;M6=(C~^NTyx&qr(bhxjc1Y&s?=%d1>EV9mAg!!SP#CdPDFQOew$YB@wOy{|4ziK6$p^(S2{B>Mx2*17rs literal 0 HcmV?d00001 diff --git a/utils/visual_track_control/__pycache__/tracking_control_law.cpython-312.pyc b/utils/visual_track_control/__pycache__/tracking_control_law.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..20b38954166d4ea76c6910f9e78b8c69189bcb2a GIT binary patch literal 12996 zcmbt4ZBSdswO2@hzCgkf<{M-%7#SmB{1Mx+jWLiIumNM5CM4}0MfYNa^+B#K*m!V@ z6FU)&ZE)fg$d^f)1e`Q6ZqtPLGq2N`*O_)Y-azMS?xRyrizN8*CbVSc&Ac~n=AFG) zSCX-alfE74?(W&MXLrw@J-d6({vjnrO+Zk+HPY2nP7r^`87U-%6Uzr=1TjKTL_0yr zs0631T_(f{?Fp2eQaI%+@+xPVYfbwaA+2^AT*h{zE4@A4wYGh&E2BL_MkElltgAxm zU^J$)K7u~t(4I|c+H)u^opVwHxm+63LMo3+Y0syPU3*2m*e{g|@O`lmF-_?JzCR9J z?~u0_QfYJ%wT3S0P*8?5iuU!Ckxqhq3gpuvUz|WZPEc#l5LCuT2_lX5lD-6UX3u}h z31VU5#f6E%h0iZ9On*2(eIs~bB5?cVuW$O=7@Pf=!+o^I<7OF;^Puhc*Ea|MKJdI+ z9k}{M;QA#~O<9>KF!cPwzzcy--VR-uGBI=q&CqT;U2ZQcv-Uc?JvJxFVlt#%Bx3b4 zcB=pZ!V-{l+KyZ7-Q8+vbYbF5aQx=iH(#ebcCR%wcqKIa;=-L<(6!*eTMM6GwYVtL zZzn1(TdcpG*b4t`@ZS#qN8rB${yQUmJpWqY;$-m6%Ym~W1wVd2_?y$ANf_1ZYLn@2 z7spKtpPgO2eJ0?0FYw9h&}?AhPH^D#{LRt8S>OEi3nELtzIjeWGyUzwS4@jz=R?;g z=I^`}Ja-xz5$L>Q0t)lf?}`X-e6~1nIpF(Z;noLuBzN8joP9qy@>y{DY-r+Y!1v<( z^o7N-;lLMX17nkc3oizSekIX)*Hq)Nd0D9C@RHpOO*!l=O+hJc80s&4{#NLN8zPm( z(O)gTe=YFJIiUx?Fa=&13Vk{rI6V}axFPfdIv=`zXJO(M)1Q82{nIr(y0eoo_{CRz zfsZc*C*KLY4U7;6@SZ6+^=aUXUoZGR3=CfcR2V_%!b^dhL)Q6Q=R>zXffTTzx)L}S z0R)D9LEmub##dkJ}U+yb`$f8W0GtuI3)LyN9*( zo*a;o@)0|Cv8sPQ(7EUcwA1J;Z=}ru`xE6 z7s_-;ob>TlpmEUQaAq0bfv`#BwWqMhG@i))m&V0#^9b5Bt?gNK*-t20kV0mr?R#fXgI8 zD&b@bK?pF}N!SOE5N1UW*7vZEVf_T&csq3CwSaHR^mO^=9Tl5Q@G*mwM%JXb8pVYe zUZ0mHL4sksL+prUWpel{RzCnWn(*eMTg5nnG+Zzpzt!vi+gV}{rBP8UC*>I z9^kC2*2dbNv4f0|6ytHz9Ss6?}IepF$pIY~3UxzLaW z{$9L`4iNK7?T~6vHC!~Bc(H7>XGU2(Q0-4v`nEiO+^JYr#-f{H4Nr8=4oRk)rj@U`*C*n;@7oNlwVHHg22oi(63Z^m!C7fIZQ#kP4kG%>e z7Tgtq)k%O=>gc8NXuJw0mXH-e64PJGJ_?T1uYGy zf#7SP`+Tq|TVT%^1U0>I`_;fp*TWm%(9eSx-e0(L(Ha=OusARvLP4`F++q)#e~R#DJ$!h%%XWg#k@g9)mv(yW4z^FEgB3BK4U3)H2!apvZ1P{V^K>VLQfm!UrT?WAtlMhjtI{9;zCw zS|yp0J<@-%pUbF38I}Hm!pr?*{anF*RIq<8J#*A>Id?3V%c@3M)&8tpF|!Y4?OT@1 z*6x>mlcLlnEfLDZBqk3Uw8K9VBynCo2%r&yCg3DZz$scr%PBZZ!^u$rCr1U9NGDQB zaGFe@lRJ{AWPm9tEv1B5MXBgyN=>W7Wg38K`sC&meof525k5K!Y+}-(ttLBGFj7c_ zgrG#<1n3CSFY70;h>3y4B|_+z#apRe(jX)>h*!akvnJWd>N^R&kmMcOk5GMR`>8Pv zhm)(cA7?GD+P+Gn%3^*Afz6Oo{W9r#A;r++h<0GH2nqrEf%WT((1oRSMTnsj>E-{E zPMXxjeRS}0TM@cSRsbLy(;vJg3E`o1@XA^dV(6@uiYP2!5ni%*;}JqvrId=sfSE1j z`at(JF<5ZTIlO{|U5CLE#V6TqE}F6N8i&hvlqQcmDA-U{9iCpY6K`{{<291Cb&r#6 z&17qJEuRX?08UCIoSdGeSsSkvt98b-JLcD%Y>jlfMl#@Vep2> zCwGeHINs1_J5GAxgcVj_RJXU1*OM5EcU?h)!kH^gfH3$ZCxJ9JyB&Ha>eyu439R`l zc?}6CQVAv5b}|Az8G&ly`sQUxHypNIUIwoSJ_Uvh!>y|&4>TXF<<%kvoHFp?P~oMW z9irC6{4|BtrjjHbZU;+}CvzUu9u}#v7HW6{@EIWd#tfh0oN6wA@7aUd7YrO zd8Jo6=q8Cwwnx+=Faj$pL4mG?;{aGjK)|~Iv@(3x!w|6c z$hsmbB#Oaoi&dvxnb8s{CG=d%#KdvA7*P^Y#E-)LXLiT*HI6!t?JFBm&0VG_B`F1% z5v2;;FP7lZxF2?GiHk>w`$iZ?$$qa9mPT8VQvc5p#!=VbX@rT<5yA$yN$O2B?!p&} z6~X&Q;8;Y}M+;--CXV{=kulFhy{%DvDb@9el#0fZbjk>*vm*+ZQV*@OOTvuC53RF1 ziYuiaS|^Up@!RWsJC|Z5m`Z}2N$Sa}QkwkGIovPNh{nviRpz2DiX)|>b0PGoNP@2- zF$bjbXiOz!K?Yikf+DG|hDb7!=)#BS6@gOXVganDkacLy2LPNO@<~UwY7^(eeyy_f($NDjI9FbPw zY;Uv$DRn}|JPtdOtm}jXiN=%~pVt6myX^kc0%re;@V!n8{VOU%R_d{qIF;I;%KRpN zk5-xApFBvfYv4hcplNkLBi@ev!h`tzx|rG3L%n#rX*{XW$JF~SQW3BJXCCa&s!|cJ z{)-RRmv{^-2If@^?*CndAK$ubevf(hnbcA=K9$y=#-5Y%T^FQOGzNWLM5$x@xeE8= zoU|~eJGYIkUkWt zsURa-?7N*Hp$CsiwWBeUL}{a=r&6M6b83_5{vejB1RIW++c&hYrx5Cke#nJ8FM+ez z8oGHs@E-U~&xJlX9sKxR>Q$7|6jW4n@IN&(Gg_vj-o61dp(3 z@!d1Q-@Gh3=LO$X@Z(Sf1>VSb4`8QQ{;iV>e~9I$H5~ccmi!K>tK7p-vhpAFAeU0 zd<*CG5isj;(O~W2(;^VTafb~+`0UPLLFH4c+uG{eeo$LS9%-vSSjX$Y0wuWVNHC^@ zpN`jo{od{Hc9J$XMLIoTEfX9^q(og8b}mV@dF>PRN9voK!t@daY(80tXo{)9jcWF{ zRyWpx-RX(CRtD=2J`oRrWw4AfkK)f>{MiQ|KC##4bWmW0i*dU1sRBM2qeQO*ueD>> zJZS@$XfF*0KVS#VFjh>Y0)Oy!!{D=)Fb%yl!wObEuzz8zA6UW|e1Ku_$%(;}&WF4h82Xhv{mBtZ^4HHvA+@2^^%uA1(fy)JOLogAh z968whL$bQ1rM0=b<^Z36A9mP9ip}|+{&n*-Tl=G=MFME9Q zIYT~Yup)!iCqJiLQW9xdoZkGE-aKc>89l(|ZbP}-rh5IkrpxtX^;~W_$}Q(|t59y$ zH*%S=S~j0i$YoTZj0#`PxdU^B6g?k>P`S^_Cz+LSD%%aSp zpA7zl(-nWEEB5OQLq8q->3hexl0681x;^*wMowRd^o5+h6zNN6^&9;dQjGM)oW2a{ z%VzcE0;B}#OE~=|q~A2Fw+Iju(wjIvhR^CtAB4=|(OXc41&`jJnF~?93G*`S0!B4a8UUvRv^O%{-E<@R6Ty_P@u9(_^vLE#|`qQ(x^fHuQ#-(pY>6@p@ zrrDYFTHj-H1?#c2io?PZ%UN2HrS)!ktH0Ph@zgs{amCwF@pi7b9u?PPOMauwSMz7! zj+TJ7vc|Hwtc@sZBbQ}GS=Omil(oy(;Llsf&zNESI?9uJxwG;L4)N|{%qxIXl^)+aH&E29Je@?;Wont$> zoN|;?&gJYvIlHh2cQ-69^^%$>*udpKit-=*Cm{z{hQDwlSNJF@e01853akGq$FTVl zm>14khpcs9_WG?`uJ&E&G64M97NZn@Ff~{-Gqa`S;f5b8D?= zt#xYs=jLl>uCf+Y)^e2x5&YI3^gS-j#0HeUflIfbbj#%NX~j(X9^YdEycneyi}N)3 z*i`LI`c9akto+elziIu1>Kzqlszjzr&U6Tw4&5y{LTYo4JBRsNm3Y zVnTK^OwsuU7-CA=3fYD>MD-=MzYC@Bf`)bJvsq>?%Yxviv)s!w4a+a7SFEsc@1ON$ z{b*Ia5Cm%pcm#gd){02Qbq zPFI9r=Ms^i+Ii3X2=FbO5xx2{(EC!r9yNU4T;2yz zaHq$=(O`@fA!;*)I{28fB6K~hPST1BkbP(!<-bq|?5Qimx3?p>d?US!v1aR6(!mzr zsP72g$I5#u0Vi^pS<8VgZRZ2o`xoPgMtsB z%+vT2=lCg{&xM1ycxbKxJa|unPb)F+NrdVrGvn;~!a4O182dT+3=sE}T27gbl-Zop zgp{UPWuYHK@{lr*Qx+j*(X6sqfaD`(KBrudl|`t2!C8=r$O`M136G!-0~j8DijP(UL!*InKgS{=Jp7D(4Jy9+1lbU;gZM@8 zOV|HCa9Vt)8h$bQF+7sOLeqoK%TI^jHo{};ADLoF?J^a6P33#>x3t)6HYYMofG*y7 zlHl%i6ukNhhr7cAcfCW+aOY~OK2&$aEEf;Ic$OG!lkwtZ4983HK-mVFlZA0aYsJOv z&$uSOu!E4nd!-$NJ4Ux&t{SUCnvHYXHA9VqjdwHm&t(?D-q!q1Gs@ih>9H@1{=M|y zO40U4e`fwSi3uqtP;u~hsdKE;S9`A6uUq3&3)&0b9SVF|g&WgsP*NKG5ifD9(;IfV%+Oyd^6LdPj`kRoUF$3ij>De^{t yvXrdQB>-k>qAY#5XNiDlJbf8QOM0~|X&j^lqN%=R9Q{}JQwcJ|KMDL52KIlQD^TG8 literal 0 HcmV?d00001 diff --git a/utils/visual_track_control/__pycache__/types.cpython-312.pyc b/utils/visual_track_control/__pycache__/types.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..528ab34a086175df65b98c66b1c968380171aaf5 GIT binary patch literal 4643 zcmbVQYj6|S6<$fJhxM?HAK1oV7E0Wz5FALF6v)d#0$f1ufMHqNFk&WRXF4T~R*zqpftk$oSI@b-Hn!vR zM|beiJ@?#u?zy|?eD}Nj!{M+}@Vq_Pe|W5fqJBVS<}qrG#}m-#r#Q+>aXL;P)P?k3 zeaPT7=ukHV>5$QD44J$pqSHZh$l|r=C_S~E;*1>>XX1^c2Gy6hi1eDF*D|Nq%Iisg z5%gO}_3GGBZF#Rvv)aIF|B2PESsh?4{)yEQFL9Q}?&v5gKYSuT+@1gSe180kY|pTg zxUNi{%$@1Tj9We`5WSuqZ@=4S}#f7~}ZGjqReJ72|Du zbp0W+PJZ~)ymTr5ViU zCmiHObzxrEytxG<*uzI-LGj__V0P%Ta;h^kK9RY7Hhb!*H8b9`UkDQ4l5(X#d-WUGec@ITqacp=iawEN6lpT|)lKExNeDcD?Rf6Pk?hAe z9!@6m$FFCvjVWIpdpOx`&5xbWkA0eb@4W>JuAdVjlfQL2d-~51-yHYgNL%}C7h2ka zzCf4}V{JjcA$y^#(A5j>@s$-YJ)C;Ei1Ud)f6y0=%Es0JALO8qc86mjnfArRh*Kw9 zn%l%cBDlZv&eKfuG;>_pEp2nSSc1QDm z_a52da&O<$=yJP2-{txX^%yt)pEq2a5By$;929&Z7reRyeh8c8gh-fwR^+4NftVNw zMh`5O(g)OxZHo&<;D`vo3@Ta+qJz3mzw&)mZD0Exy5fG>(!P3W=lzQ6zHLW$3uQB- z7yke|!2JJyfO((8LOw^ai-)~&JhzAUMI+(Zi{QTS$?4q0u+np08N2}BB6G8onfGoO z^)qFMy5ZBz`6xvK!lPhAVMpOWLEb&>Y4*6~;>MPirj~s!H@ml`ahF@R?{C`M)a=o; zQcpAMY66RAtDANIp{Ye!g7eAyD92XijHrS}bTtTkkFS4kD>?b1}m z42lJ5hy^U5VBjzM0flCG|W*~S!Io35#ob|u%O z=(==OwX`!?o}#PM6_wJCgg-@B{=AcW^cFSkG01soZ-5Sb9%n!-OnXEJ-gsF*3gk_DU~W`q6BavGJ& zD7Qy*ox?ySW{+rPXhcb#%8X0e$oEIGL+61<6uPFDfO5ODaPcNC*86ex!ne6|mti=N1m*Nc zFa~FTaNdgk)#a3nqd>eEQgo~sfa5O|1?rN&-dULHP!gYEagIws)iSqkgO}B+S2YM` zEz|A`1~|4gBEYoO7&>}}Grxr6TYbQS*j6aCh!BVBz_M0r(J^l|wR}tA$nXg)7l?{J zC}B3VO|Ae=5%C9r^Kh&fVO6drJJf+%sB)=K6aymgJS_MqfWn-fnK9s zp!gMv=TQ6_#Re4hC^n)%{vy19;x{Pp@DyG`@mmy|P#|FvwxC!5BkQma1+Ei42cnaD zMAJsgBRf@7H*)x1^(M)k*qov_rE8W+yOYnS=w%P;Rsms1@+o>%dR6@$x_SW6p?;2i z#cH&3DSCB!`3h_TQmjZ05hl&(BYRHMkrtK0K6=L5h@TF0A3K>$hjLYc9zHv4K$i# zVp~j%U0DbjT@wHng-?>ojZP)m1Dt;@RhJm%E(26u`ARAr zA7MOh7aqu``WrIiLk-H%HNXcYaRP|@zoif1pOWk-NM{O1`v4-cLlc=>eYvl10u1G^ zUd&zXC-n$Os1vi&FF`k+kpD+yR3p{fu?_R6Q3iO-bY0k~Pf zTqT((NavM_O9)=Q1KQmIL8AQ#`&NV_C56#z{<=@&&S%5w2E=<`jUE}p)F89FasOV| zPDXXA4$NPj0+h-fyP-l9*#d?%<0UgS3|_KF;hHtUJO_TVAHk&z!nuSqd%wmd$aM>F zkG#sy;Em)4#{sS9#4FQqCE&NoMgn;n3xQ1_M3~xDXfU-CNRVxFHYXGG;rN5HeHLgC zW&j~on#>A>C(Cw!#0L~bB_y#OqWtMYrd=AaJPDcS~i~o>1`fq|~IV zOSU|r;Agsw(%XL2+jK8aQy?B2K%FM)a$;DjW2Pw(ngP{~#89bQIZc5ehDUm6k=_3n DmGU|A literal 0 HcmV?d00001 diff --git a/utils/visual_track_control/command_sender.py b/utils/visual_track_control/command_sender.py new file mode 100644 index 0000000..61052bf --- /dev/null +++ b/utils/visual_track_control/command_sender.py @@ -0,0 +1,32 @@ +"""控制指令发送抽象（语义接口，对应 C++ ICommandTransport + 网关门面）。 + +C++ 版的 ICommandTransport 是字节流传输抽象（Send(bytes)），MAVLink 编码 +在网关内完成；Python 版采用 MAVSDK 语义映射后，"传输"被抽象为设定点级 +指令发送。本 Protocol 仅声明门面接口，供胶水层依赖注入与 mock 测试。 + +异常约定：实现不得向外抛传输层异常，失败一律返回 False（网关已捕获）。 +""" + +from typing import Optional, Protocol, runtime_checkable + +from .types import ControlOutput + + +@runtime_checkable +class CommandSender(Protocol): + async def start(self) -> bool: + """进入 offboard 模式。失败返回 False。""" + ... + + async def send_velocity_heading(self, output: ControlOutput, + time_boot_ms: Optional[int] = None) -> bool: + """发送速度+航向意图（ControlMode.VELOCITY_HEADING）。失败返回 False。""" + ... + + async def send_brake_hover(self, time_boot_ms: Optional[int] = None) -> bool: + """发送零速刹车/悬停意图（ControlMode.BRAKE_HOVER）。失败返回 False。""" + ... + + async def stop(self) -> None: + """退出 offboard 模式（结束时调用）。""" + ... diff --git a/utils/visual_track_control/config.py b/utils/visual_track_control/config.py new file mode 100644 index 0000000..95117cc --- /dev/null +++ b/utils/visual_track_control/config.py @@ -0,0 +1,278 @@ +"""视觉跟踪控制律配置（TrackingConfig + load_from_json）。 + +对应 C++ 参考实现 reference-c++/visual_track_control/src/vtc_config.cpp +（规格：docs/物理追踪思路.md §8：全部阈值集中配置，NFR-007）。 + +启动期一次性解析：缺字段 / 类型错误 / 非法取值抛 ValueError 并携带完整 +字段名（如 "heading.gain"）；解析结果与 JSON 脱钩，热路径不做热更新。 +""" + +import json +from dataclasses import dataclass, field +from enum import Enum +from typing import Any + +from .types import ControlMode # noqa: F401 (re-export 便利性) + + +class HeadingMode(Enum): + """航向通道模式（§4.2）。默认速率式：yaw_rate 天然是相对量，无坐标系歧义。""" + + RATE = "rate" # β → 偏航角速度，输出 yaw_rate_dps + POSITION = "position" # β → 修正量，与当前航向合成绝对航向输出 yaw_deg + + +class NoDistanceAction(Enum): + """雷达距离不可用（未关联/超龄）时的距离通道降级动作（§6.3）。""" + + HOLD = "hold" # 保持：前向速度指令清零 + SLOW_APPROACH = "slow_approach" # 限速接近：按 no_distance_approach_limit_mps 缓速 + EXIT = "exit" # 退出近距操作：输出降级保持 + + +@dataclass +class ControlGroupConfig: + frequency_hz: float = 20.0 + visual_stale_ms: int = 200 + radar_stale_ms: int = 500 + attitude_stale_ms: int = 300 + + +@dataclass +class CameraGroupConfig: + image_width: int = 1920 + image_height: int = 1080 + fov_h_deg: float = 60.0 + fov_v_deg: float = 35.0 + + +@dataclass +class HeadingGroupConfig: + mode: HeadingMode = HeadingMode.RATE + gain: float = 1.0 + yaw_rate_limit_dps: float = 30.0 + yaw_slew_limit_dps2: float = 60.0 + + +@dataclass +class VerticalGroupConfig: + vz_gain_mps_per_deg: float = 0.1 + vz_limit_mps: float = 2.0 + + +@dataclass +class DistanceGroupConfig: + d_exp_m: float = 10.0 + kp: float = 0.0 + ki: float = 0.0 + kd: float = 0.0 + integral_limit: float = 0.0 + derivative_filter_coef: float = 0.0 + approach_velocity_limit_mps: float = 3.0 + retreat_velocity_limit_mps: float = 1.0 + no_distance_action: NoDistanceAction = NoDistanceAction.HOLD + no_distance_approach_limit_mps: float = 0.5 + + +@dataclass +class AccelLimitConfig: + ax_mps2: float = 2.0 + ay_mps2: float = 2.0 + az_mps2: float = 1.5 + + +@dataclass +class TrackingConfig: + control: ControlGroupConfig = field(default_factory=ControlGroupConfig) + camera: CameraGroupConfig = field(default_factory=CameraGroupConfig) + heading: HeadingGroupConfig = field(default_factory=HeadingGroupConfig) + vertical: VerticalGroupConfig = field(default_factory=VerticalGroupConfig) + distance: DistanceGroupConfig = field(default_factory=DistanceGroupConfig) + accel_limit: AccelLimitConfig = field(default_factory=AccelLimitConfig) + + +# --------------------------------------------------------------------------- +# JSON 解析与校验（错误消息携带完整字段名，与 C++ 版文本一致） +# --------------------------------------------------------------------------- + +def _require_section(root: dict, section: str) -> dict: + value = root.get(section) + if not isinstance(value, dict): + raise ValueError(f"配置缺失分组或类型错误: {section}") + return value + + +def _is_number(value: Any) -> bool: + return isinstance(value, (int, float)) and not isinstance(value, bool) + + +def _is_integer(value: Any) -> bool: + return isinstance(value, int) and not isinstance(value, bool) + + +def _require_number(section: dict, section_name: str, key: str) -> float: + value = section.get(key) + if not _is_number(value): + raise ValueError(f"配置缺失字段或类型错误: {section_name}.{key}") + return float(value) + + +def _require_integer(section: dict, section_name: str, key: str) -> int: + value = section.get(key) + if not _is_integer(value): + raise ValueError(f"配置缺失字段或类型错误(需整数): {section_name}.{key}") + return int(value) + + +def _require_string(section: dict, section_name: str, key: str) -> str: + value = section.get(key) + if not isinstance(value, str): + raise ValueError(f"配置缺失字段或类型错误(需字符串): {section_name}.{key}") + return value + + +def _require_positive(value: float, field_name: str) -> float: + if not (value > 0.0): + raise ValueError(f"配置非法(必须为正): {field_name}") + return value + + +def _require_non_negative(value: float, field_name: str) -> float: + if value < 0.0: + raise ValueError(f"配置非法(不得为负): {field_name}") + return value + + +def _require_range(value: float, lo: float, hi: float, field_name: str) -> float: + if not (lo < value < hi): + raise ValueError(f"配置非法(超出范围): {field_name}") + return value + + +def _load_control(root: dict) -> ControlGroupConfig: + s = _require_section(root, "control") + c = ControlGroupConfig() + c.frequency_hz = _require_positive( + _require_number(s, "control", "frequency_hz"), "control.frequency_hz") + c.visual_stale_ms = _require_integer(s, "control", "visual_stale_ms") + c.radar_stale_ms = _require_integer(s, "control", "radar_stale_ms") + c.attitude_stale_ms = _require_integer(s, "control", "attitude_stale_ms") + if c.visual_stale_ms <= 0 or c.radar_stale_ms <= 0 or c.attitude_stale_ms <= 0: + raise ValueError("配置非法: control.*_stale_ms 必须为正") + return c + + +def _load_camera(root: dict) -> CameraGroupConfig: + s = _require_section(root, "camera") + c = CameraGroupConfig() + c.image_width = int(_require_positive( + float(_require_integer(s, "camera", "image_width")), "camera.image_width")) + c.image_height = int(_require_positive( + float(_require_integer(s, "camera", "image_height")), "camera.image_height")) + c.fov_h_deg = _require_range(_require_number(s, "camera", "fov_h_deg"), + 0.0, 180.0, "camera.fov_h_deg") + c.fov_v_deg = _require_range(_require_number(s, "camera", "fov_v_deg"), + 0.0, 180.0, "camera.fov_v_deg") + return c + + +def _load_heading(root: dict) -> HeadingGroupConfig: + s = _require_section(root, "heading") + h = HeadingGroupConfig() + mode = _require_string(s, "heading", "mode") + if mode == "position": + h.mode = HeadingMode.POSITION + elif mode == "rate": + h.mode = HeadingMode.RATE + else: + raise ValueError("配置非法: heading.mode 仅支持 position / rate") + h.gain = _require_non_negative(_require_number(s, "heading", "gain"), "heading.gain") + h.yaw_rate_limit_dps = _require_positive( + _require_number(s, "heading", "yaw_rate_limit_dps"), "heading.yaw_rate_limit_dps") + h.yaw_slew_limit_dps2 = _require_positive( + _require_number(s, "heading", "yaw_slew_limit_dps2"), "heading.yaw_slew_limit_dps2") + return h + + +def _load_vertical(root: dict) -> VerticalGroupConfig: + s = _require_section(root, "vertical") + v = VerticalGroupConfig() + v.vz_gain_mps_per_deg = _require_non_negative( + _require_number(s, "vertical", "vz_gain_mps_per_deg"), + "vertical.vz_gain_mps_per_deg") + v.vz_limit_mps = _require_positive( + _require_number(s, "vertical", "vz_limit_mps"), "vertical.vz_limit_mps") + return v + + +def _load_distance(root: dict) -> DistanceGroupConfig: + s = _require_section(root, "distance") + d = DistanceGroupConfig() + d.d_exp_m = _require_positive(_require_number(s, "distance", "d_exp_m"), + "distance.d_exp_m") + d.kp = _require_non_negative(_require_number(s, "distance", "kp"), "distance.kp") + d.ki = _require_non_negative(_require_number(s, "distance", "ki"), "distance.ki") + d.kd = _require_non_negative(_require_number(s, "distance", "kd"), "distance.kd") + d.integral_limit = _require_non_negative( + _require_number(s, "distance", "integral_limit"), "distance.integral_limit") + # 容许 -0.0：范围下界略低于 0，读入后归一化为 0 + d.derivative_filter_coef = _require_range( + _require_number(s, "distance", "derivative_filter_coef"), + -0.000001, 1.0, "distance.derivative_filter_coef") + if d.derivative_filter_coef < 0.0: + d.derivative_filter_coef = 0.0 + d.approach_velocity_limit_mps = _require_positive( + _require_number(s, "distance", "approach_velocity_limit_mps"), + "distance.approach_velocity_limit_mps") + d.retreat_velocity_limit_mps = _require_positive( + _require_number(s, "distance", "retreat_velocity_limit_mps"), + "distance.retreat_velocity_limit_mps") + action = _require_string(s, "distance", "no_distance_action") + if action == "hold": + d.no_distance_action = NoDistanceAction.HOLD + elif action == "slow_approach": + d.no_distance_action = NoDistanceAction.SLOW_APPROACH + elif action == "exit": + d.no_distance_action = NoDistanceAction.EXIT + else: + raise ValueError( + "配置非法: distance.no_distance_action 仅支持 hold / slow_approach / exit") + d.no_distance_approach_limit_mps = _require_non_negative( + _require_number(s, "distance", "no_distance_approach_limit_mps"), + "distance.no_distance_approach_limit_mps") + return d + + +def _load_accel_limit(root: dict) -> AccelLimitConfig: + s = _require_section(root, "accel_limit") + a = AccelLimitConfig() + a.ax_mps2 = _require_positive(_require_number(s, "accel_limit", "ax_mps2"), + "accel_limit.ax_mps2") + a.ay_mps2 = _require_positive(_require_number(s, "accel_limit", "ay_mps2"), + "accel_limit.ay_mps2") + a.az_mps2 = _require_positive(_require_number(s, "accel_limit", "az_mps2"), + "accel_limit.az_mps2") + return a + + +def load_from_json(path: str) -> TrackingConfig: + """从 JSON 文件一次性加载配置（启动期调用，调参重启生效）。""" + try: + with open(path, "r", encoding="utf-8") as fp: + root = json.load(fp) + except OSError: + raise ValueError(f"配置文件打开失败: {path}") from None + except json.JSONDecodeError as exc: + raise ValueError(f"配置文件解析失败: {path} ({exc})") from None + + if not isinstance(root, dict): + raise ValueError(f"配置文件解析失败: {path} (根节点必须为对象)") + + cfg = TrackingConfig() + cfg.control = _load_control(root) + cfg.camera = _load_camera(root) + cfg.heading = _load_heading(root) + cfg.vertical = _load_vertical(root) + cfg.distance = _load_distance(root) + cfg.accel_limit = _load_accel_limit(root) + return cfg diff --git a/utils/visual_track_control/frame_transform.py b/utils/visual_track_control/frame_transform.py new file mode 100644 index 0000000..af65a08 --- /dev/null +++ b/utils/visual_track_control/frame_transform.py @@ -0,0 +1,25 @@ +"""机体系（前/右/下）→ 局部 NED（北/东/地）坐标旋转。 + +对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/frame_transform.h +（规格：docs/物理追踪思路.md §2.3）。 + +速度设定值只随航向角 ψ（绕下轴旋转）旋转：滚转/俯仰对速度指令为二阶 +小量，由飞控内环处理。旋转为正交变换：合速度大小不变，因此"限幅在机 +体系完成、旋转在输出端执行"不破坏 §8 限幅约束。 + +正方向验证（NED：x=北，y=东，z=地；航向 ψ 自北顺时针为正）： + 机体前向 (1,0) 在 ψ=90°（机头朝东）时 → (0, 1) = 东； + 机体右向 (0,1) 在 ψ=0°（机头朝北）时 → (0, 1) = 东。 +""" + +import math + + +def rotate_body_to_ned(v_forward: float, v_right: float, v_down: float, + yaw_rad: float) -> tuple[float, float, float]: + """绕下轴按航向角 ψ 旋转，返回 (北, 东, 地)；垂直分量不变。""" + c = math.cos(yaw_rad) + s = math.sin(yaw_rad) + north = v_forward * c - v_right * s + east = v_forward * s + v_right * c + return north, east, v_down diff --git a/utils/visual_track_control/line_of_sight.py b/utils/visual_track_control/line_of_sight.py new file mode 100644 index 0000000..69efa52 --- /dev/null +++ b/utils/visual_track_control/line_of_sight.py @@ -0,0 +1,25 @@ +"""像素偏移 → 视线角（针孔模型）。 + +对应 C++ 参考实现 reference-c++/visual_track_control/src/line_of_sight.cpp +（规格：docs/物理追踪思路.md §4.1 水平 β / §5.1 垂直 α）。 + +全程使用精确针孔模型 angle = atan(offset · 2·tan(θ/2) / size)：原线性近似 +双路切换在边界两侧不连续（θ=60°、边界比例 0.2 处约 8% 台阶），会向控制 +环路注入突变；atan 代价可忽略。构造期预计算针孔系数，Convert 仅一次乘加。 +""" + +import math + + +class PixelToAngle: + def __init__(self, size_px: float, fov_rad: float) -> None: + # 构造期校验：非法参数直接抛异常（初始化抛异常规范） + if not (size_px > 0.0): + raise ValueError("PixelToAngle: size_px 必须为正") + if not (0.0 < fov_rad < math.pi): + raise ValueError("PixelToAngle: fov_rad 必须在 (0, π) 内") + self._pinhole_scale = 2.0 * math.tan(fov_rad / 2.0) / size_px + + def convert(self, offset_px: float) -> float: + """偏移（像素，水平右正 / 垂直下正）→ 视线角（弧度，同向为正）。""" + return math.atan(offset_px * self._pinhole_scale) diff --git a/utils/visual_track_control/mavsdk_gateway.py b/utils/visual_track_control/mavsdk_gateway.py new file mode 100644 index 0000000..6cf7bea --- /dev/null +++ b/utils/visual_track_control/mavsdk_gateway.py @@ -0,0 +1,98 @@ +"""MAVSDK offboard 网关 —— 控制意图 → MAVSDK 设定点（语义映射）。 + +对应 C++ 参考实现 reference-c++/visual_track_control/src/mavlink_command_gateway.cpp +（C++ 版手工编码 MAVLink ID 84 字节流；Python 版改用 MAVSDK offboard API， +按 A2 语义映射三分支）： + + 速率式（yaw_rate_dps ≠ 0）→ set_velocity_body(机体前向, 0, vz, yaw_rate) + 位置式（yaw_deg ≠ 0 且角速率为零）→ set_velocity_ned(NED 速度 + 绝对 yaw) + 两者均零 → set_velocity_body 配零角速率（保持当前航向，A2 回归语义） + 刹车 → set_velocity_body(0, 0, 0, 0)（MAVSDK 无刹车掩码的行为等效适配） + +降级保持（DEGRADED_HOLD）不发送任何指令，由调用方（胶水层）判断模式。 +MAVSDK Python API 为 asyncio 异步接口，本网关方法均为协程；传输异常 +捕获后返回 False（与 C++ 传输失败传播一致），不向外抛异常。 +""" + +import logging +from typing import Any, Optional + +from .types import ControlOutput + +logger = logging.getLogger(__name__) + + +class MavsdkCommandGateway: + """drone：mavsdk System（仅取 .offboard 属性，duck-typed 便于注入 mock）。""" + + def __init__(self, drone: Any) -> None: + self._offboard = drone.offboard + self._started = False + + async def start(self) -> bool: + """进入 offboard：PX4 要求先流式发送首条设定点再请求启动。""" + try: + await self._offboard.set_velocity_body( + self._velocity_body(0.0, 0.0, 0.0, 0.0)) + await self._offboard.start() + except Exception as exc: # OffboardError / 连接异常统一视为启动失败 + logger.warning("[MavsdkGateway] offboard 启动失败: %s", exc) + return False + self._started = True + logger.info("[MavsdkGateway] offboard 已启动") + return True + + async def send_velocity_heading(self, output: ControlOutput, + time_boot_ms: Optional[int] = None) -> bool: + try: + if output.yaw_rate_dps != 0.0: + # 速率式：机体系速度 + 偏航角速度（NED 分量不再使用） + await self._offboard.set_velocity_body( + self._velocity_body(output.vx_body_mps, 0.0, output.vz_mps, + output.yaw_rate_dps)) + elif output.yaw_deg != 0.0: + # 位置式：NED 速度 + 绝对航向（A2：绝对航向合成在控制律完成） + await self._offboard.set_velocity_ned( + self._velocity_ned(output.vx_mps, output.vy_mps, + output.vz_mps, output.yaw_deg)) + else: + # 两者均零：机体系速度 + 零角速率 = 保持当前航向（A2 回归语义） + await self._offboard.set_velocity_body( + self._velocity_body(output.vx_body_mps, 0.0, output.vz_mps, + 0.0)) + except Exception as exc: + logger.warning("[MavsdkGateway] 速度航向指令发送失败: %s", exc) + return False + return True + + async def send_brake_hover(self, time_boot_ms: Optional[int] = None) -> bool: + try: + await self._offboard.set_velocity_body( + self._velocity_body(0.0, 0.0, 0.0, 0.0)) + except Exception as exc: + logger.warning("[MavsdkGateway] 刹车指令发送失败: %s", exc) + return False + return True + + async def stop(self) -> None: + try: + await self._offboard.stop() + except Exception as exc: + logger.warning("[MavsdkGateway] offboard 停止失败: %s", exc) + self._started = False + logger.info("[MavsdkGateway] offboard 已停止") + + @staticmethod + def _velocity_body(forward: float, right: float, down: float, + yawspeed: float) -> Any: + from mavsdk import offboard as _mavsdk_offboard + return _mavsdk_offboard.VelocityBodyYawspeed( + forward_m_s=forward, right_m_s=right, down_m_s=down, + yawspeed_deg_s=yawspeed) + + @staticmethod + def _velocity_ned(north: float, east: float, down: float, + yaw: float) -> Any: + from mavsdk import offboard as _mavsdk_offboard + return _mavsdk_offboard.VelocityNedYaw( + north_m_s=north, east_m_s=east, down_m_s=down, yaw_deg=yaw) diff --git a/utils/visual_track_control/pid_controller.py b/utils/visual_track_control/pid_controller.py new file mode 100644 index 0000000..c537eec --- /dev/null +++ b/utils/visual_track_control/pid_controller.py @@ -0,0 +1,95 @@ +"""位置式 PID 控制器（纯算法构件）。 + +对应 C++ 参考实现 reference-c++/visual_track_control/src/pid_controller.cpp +（规格：docs/物理追踪思路.md §6.2 距离控制律、§6.4 Coast 清积分）。 + +anti-windup（条件积分）：输出饱和且本拍积分增量（error·dt）仍在推深饱和 +时冻结本步积分累加；判据刻意基于"增量方向"而非"积分项与输出同向"—— +后者在误差反号后仍会冻结回卷，导致饱和残留。 +""" + +from dataclasses import dataclass + +from .rate_limiter import clamp + + +@dataclass +class PidParams: + """PID 参数（全部来自配置，代码不写死任何数值）。""" + + kp: float = 0.0 + ki: float = 0.0 + kd: float = 0.0 + integral_limit: float = 0.0 # 积分项限幅（<=0 表示不限幅） + derivative_filter_coef: float = 0.0 # 微分一阶低通系数 ∈ [0,1)：0 = 不滤波 + output_limit: float = 0.0 # 输出限幅（<=0 表示不限幅） + + +class PidController: + def __init__(self, params: PidParams) -> None: + self._params = params + self._integral = 0.0 + self._prev_error = 0.0 + self._filtered_derivative = 0.0 + self._last_output = 0.0 + self._has_prev_error = False + + def update(self, error: float, dt_s: float) -> float: + """单步更新：输入误差与采样间隔（秒），返回限幅后的输出。 + + dt_s <= 0 时保持上次输出（异常节拍保护）。 + """ + if dt_s <= 0.0: + return self._last_output + + # 积分：候选值先钳位（积分限幅） + integral_candidate = self._integral + error * dt_s + if self._params.integral_limit > 0.0: + integral_candidate = clamp(integral_candidate, + -self._params.integral_limit, + self._params.integral_limit) + + # 微分：一阶低通滤波（系数 ∈ [0,1)，0 = 不滤波） + derivative = 0.0 + if self._has_prev_error: + derivative = (error - self._prev_error) / dt_s + coef = self._params.derivative_filter_coef + derivative = coef * self._filtered_derivative + (1.0 - coef) * derivative + self._filtered_derivative = derivative + self._prev_error = error + self._has_prev_error = True + + # 位置式合成 + integral_term = self._params.ki * integral_candidate + raw = self._params.kp * error + integral_term + self._params.kd * derivative + + # anti-windup（条件积分）：输出饱和且本拍积分增量仍在推深饱和时冻结 + saturated = (self._params.output_limit > 0.0 + and abs(raw) > self._params.output_limit) + increment_pushes_deeper = (error * dt_s) * raw > 0.0 + if not (saturated and increment_pushes_deeper): + self._integral = integral_candidate + + # 输出限幅 + if self._params.output_limit > 0.0: + self._last_output = clamp(raw, -self._params.output_limit, + self._params.output_limit) + else: + self._last_output = raw + return self._last_output + + def reset_integral(self) -> None: + """清空积分项（§6.4：Coast 时防止积分饱和）；保留微分历史与上次输出。""" + self._integral = 0.0 + + def reset(self) -> None: + """清空全部内部状态（积分、微分历史、上次输出）。""" + self._integral = 0.0 + self._prev_error = 0.0 + self._filtered_derivative = 0.0 + self._last_output = 0.0 + self._has_prev_error = False + + @property + def last_output(self) -> float: + return self._last_output diff --git a/utils/visual_track_control/rate_limiter.py b/utils/visual_track_control/rate_limiter.py new file mode 100644 index 0000000..8beb90b --- /dev/null +++ b/utils/visual_track_control/rate_limiter.py @@ -0,0 +1,37 @@ +"""限幅与变化率限制（纯算法构件）。 + +对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/rate_limiter.h +（规格：docs/物理追踪思路.md §8、状态机设计 S6 首条指令限幅）。 +""" + +def clamp(v: float, lo: float, hi: float) -> float: + """静态限幅：将 v 钳制在 [lo, hi]（调用方保证 lo <= hi）。""" + return min(max(v, lo), hi) + + +class SlewRateLimiter: + """变化率限幅器：限制输出每秒最大变化量（单位由调用方决定）。 + + 初值置 0 时天然实现"首条指令突变抑制"（S6）：切换进入视觉跟踪后 + 首条指令从零按加速度限幅渐变。 + """ + + def __init__(self, rate_limit: float, initial: float = 0.0) -> None: + self._rate_limit = rate_limit + self._output = initial + + def limit(self, target: float, dt_s: float) -> float: + """朝 target 推进一个节拍；dt_s <= 0 或 rate_limit <= 0 时保持当前输出。""" + if dt_s <= 0.0 or self._rate_limit <= 0.0: + return self._output + max_step = self._rate_limit * dt_s + self._output = clamp(target, self._output - max_step, self._output + max_step) + return self._output + + def reset(self, value: float = 0.0) -> None: + """复位输出值（刹车/重置时归零，保证恢复跟踪后指令从零渐变）。""" + self._output = value + + @property + def last_output(self) -> float: + return self._output diff --git a/utils/visual_track_control/staleness_checker.py b/utils/visual_track_control/staleness_checker.py new file mode 100644 index 0000000..5bfca18 --- /dev/null +++ b/utils/visual_track_control/staleness_checker.py @@ -0,0 +1,19 @@ +"""输入超龄判定（单调毫秒时钟）。 + +对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/staleness_checker.h +（规格：docs/物理追踪思路.md §7.1：超龄数据只能触发降级或告警）。 +""" + + +def is_stale(sample_time_ms: int, now_ms: int, valid_for_ms: int) -> bool: + """输入超龄判定。 + + sample_time_ms <= 0 视为从未取样，直接判超龄；now_ms < sample_time_ms + （时间戳不一致：时钟源错配/打戳错误）同样判超龄——单一单调时钟下不应 + 发生，宁可降级也不信任不可核实的取样。 + """ + if sample_time_ms <= 0: + return True + if now_ms < sample_time_ms: + return True + return (now_ms - sample_time_ms) > valid_for_ms diff --git a/utils/visual_track_control/tracking_control_law.py b/utils/visual_track_control/tracking_control_law.py new file mode 100644 index 0000000..fc9dff7 --- /dev/null +++ b/utils/visual_track_control/tracking_control_law.py @@ -0,0 +1,220 @@ +"""视觉跟踪主控律（TrackingControlLaw）。 + +对应 C++ 参考实现 reference-c++/visual_track_control/src/tracking_control_law.cpp +（规格：docs/物理追踪思路.md §2.3/§4/§5/§6/§7/§8/§9）。 + +单周期八步流程： + ① 超龄判定（姿态优先于视觉，超龄 → 降级保持） + ② 判丢 → 零速刹车（进入时清积分与限幅器历史） + ③ Coast（is_predicted）→ 清距离积分防饱和 + ④ 像素偏移 → 视线角 β/α（全程针孔模型） + ⑤ 水平航向（速率式/位置式，A2：位置式合成绝对 NED 航向） + ⑥ 垂直统一速度支路（A1：不切姿态微调模式） + ⑦ 距离通道 PID → 前向速度；无距离按配置降级（hold/slow_approach/exit） + ⑧ 机体系 → 局部 NED 旋转（A4） + +日志纪律与 C++ 版一致：仅状态切换点打印，热路径零日志。 +Python 扩展：ControlOutput.vx_body_mps 记录旋转前的机体系前向分量， +供 MAVSDK 机体速度设定点使用。 +""" + +import logging +import math + +from .config import HeadingMode, NoDistanceAction, TrackingConfig +from .frame_transform import rotate_body_to_ned +from .line_of_sight import PixelToAngle +from .pid_controller import PidController, PidParams +from .rate_limiter import SlewRateLimiter, clamp +from .staleness_checker import is_stale +from .types import ControlMode, ControlOutput, ControlSnapshot, DegradedReason + +logger = logging.getLogger(__name__) + +_K_PI = 3.14159265358979323846 +_K_RAD_TO_DEG = 180.0 / _K_PI +_K_DEG_TO_RAD = _K_PI / 180.0 +_K_MAX_DT_S = 0.5 # 单步 dt 上限（秒）：异常节拍保护 + +_MODE_NAMES = { + ControlMode.VELOCITY_HEADING: "速度航向", + ControlMode.BRAKE_HOVER: "零速刹车", + ControlMode.DEGRADED_HOLD: "降级保持", +} + + +def _wrap_deg_180(deg: float) -> float: + """角度回绕到 [-180, 180)（位置式绝对航向合成用）。""" + d = math.fmod(deg + 180.0, 360.0) + if d < 0.0: + d += 360.0 + return d - 180.0 + + +def _build_distance_pid_params(cfg: TrackingConfig) -> PidParams: + """由距离配置构造 PID 参数（§6.2）；输出限幅取接近/后退限幅的较大者。""" + return PidParams( + kp=cfg.distance.kp, + ki=cfg.distance.ki, + kd=cfg.distance.kd, + integral_limit=cfg.distance.integral_limit, + derivative_filter_coef=cfg.distance.derivative_filter_coef, + output_limit=max(cfg.distance.approach_velocity_limit_mps, + cfg.distance.retreat_velocity_limit_mps), + ) + + +class TrackingControlLaw: + def __init__(self, cfg: TrackingConfig) -> None: + self._cfg = cfg + self._pixel_to_beta = PixelToAngle(cfg.camera.image_width, + cfg.camera.fov_h_deg * _K_DEG_TO_RAD) + self._pixel_to_alpha = PixelToAngle(cfg.camera.image_height, + cfg.camera.fov_v_deg * _K_DEG_TO_RAD) + self._distance_pid = PidController(_build_distance_pid_params(cfg)) + self._yaw_deg_limiter = SlewRateLimiter(cfg.heading.yaw_slew_limit_dps2) + self._yaw_rate_limiter = SlewRateLimiter(cfg.heading.yaw_slew_limit_dps2) + self._vx_limiter = SlewRateLimiter(cfg.accel_limit.ax_mps2) + self._vz_limiter = SlewRateLimiter(cfg.accel_limit.az_mps2) + self._last_now_ms = 0 + self._last_mode = ControlMode.DEGRADED_HOLD + + def update(self, snapshot: ControlSnapshot, now_ms: int) -> ControlOutput: + # ---- 节拍推算（§7.1 固定周期；异常节拍用配置频率兜底） ---- + # 时钟回拨或 now_ms 未前进（同值）时不信任差值，回退配置频率。 + dt_s = 1.0 / self._cfg.control.frequency_hz + if self._last_now_ms > 0 and now_ms > self._last_now_ms: + dt_s = min((now_ms - self._last_now_ms) / 1000.0, _K_MAX_DT_S) + self._last_now_ms = now_ms + + out = ControlOutput() + out.mode = ControlMode.VELOCITY_HEADING # 正常路径默认模式，分支可覆盖 + + # ---- ① 超龄判定（§7.1 / §9.2） ---- + # 姿态过期或缺失：禁止空间转换和控制，立即退出正常控制（§9.2） + if (not snapshot.attitude_present + or is_stale(snapshot.attitude_time_ms, now_ms, + self._cfg.control.attitude_stale_ms)): + out.mode = ControlMode.DEGRADED_HOLD + out.degraded_reason = DegradedReason.ATTITUDE_STALE + return self._finish_and_log(out, "姿态过期/缺失，禁止控制") + # 视觉追踪结果超龄：不得继续生成正常指令（§7.1） + if is_stale(snapshot.track_time_ms, now_ms, + self._cfg.control.visual_stale_ms): + out.mode = ControlMode.DEGRADED_HOLD + out.degraded_reason = DegradedReason.VISION_STALE + return self._finish_and_log(out, "视觉追踪结果超龄") + + # ---- ② 判丢 → 零速刹车（§6.4：tracked == false 执行 BRAKE_HOVER） ---- + if not snapshot.track.tracked: + if self._last_mode is not ControlMode.BRAKE_HOVER: + # 进入刹车：清积分与限幅器历史，恢复跟踪后指令从零渐变（S6 首条限幅） + self._distance_pid.reset() + self._yaw_deg_limiter.reset() + self._yaw_rate_limiter.reset() + self._vx_limiter.reset() + self._vz_limiter.reset() + out.mode = ControlMode.BRAKE_HOVER # 速度分量保持默认全零 + out.valid = True + return self._finish_and_log(out, "判丢，零速刹车") + + # ---- ③ Coast：is_predicted → 清积分防饱和（§6.4） ---- + if snapshot.track.is_predicted: + self._distance_pid.reset_integral() + # 航向通道为纯比例增益映射（§4.2），无积分项，无需清理 + out.coast_active = True + + # ---- ④ 像素偏移 → 视线角（§4.1 水平 β / §5.1 垂直 α） ---- + # x_offset = cx − W/2（右正），y_offset = cy − H/2（下正） + x_offset = snapshot.track.center[0] - self._cfg.camera.image_width * 0.5 + y_offset = snapshot.track.center[1] - self._cfg.camera.image_height * 0.5 + beta_deg = self._pixel_to_beta.convert(x_offset) * _K_RAD_TO_DEG + alpha_deg = self._pixel_to_alpha.convert(y_offset) * _K_RAD_TO_DEG + + # ---- ⑤ 水平航向通道（§4.2：增益 → slew → 角速度限幅） ---- + if self._cfg.heading.mode is HeadingMode.RATE: + # 速率式：β → 偏航角速度指令，先变化率限幅再角速度限幅（FR-033） + target_rate = self._cfg.heading.gain * beta_deg + slewed = self._yaw_rate_limiter.limit(target_rate, dt_s) + out.yaw_rate_dps = clamp(slewed, -self._cfg.heading.yaw_rate_limit_dps, + self._cfg.heading.yaw_rate_limit_dps) + else: + # 位置式：β → 航向修正量，变化率限幅（FR-033）后与当前航向合成 + # 绝对 NED 航向——ID 84 的 yaw 字段是绝对值，直接发修正量会让 + # 机体转向"修正角"方向（A2 修复）。姿态已在 ① 通过超龄校验。 + target_yaw = self._cfg.heading.gain * beta_deg + correction_deg = self._yaw_deg_limiter.limit(target_yaw, dt_s) + out.yaw_deg = _wrap_deg_180(snapshot.yaw_rad * _K_RAD_TO_DEG + + correction_deg) + + # ---- ⑥ 垂直速度通道（§5：统一速度支路） ---- + # 符号约定：α 下正（目标偏低）→ NED 下正 → vz 为正（下降趋近目标高度）。 + # 输出模式保持 VELOCITY_HEADING，距离通道前向速度不会被丢弃（A1 修复）。 + vz_target = self._cfg.vertical.vz_gain_mps_per_deg * alpha_deg + slewed = self._vz_limiter.limit(vz_target, dt_s) # 加速度限幅（§8） + out.vz_mps = clamp(slewed, -self._cfg.vertical.vz_limit_mps, + self._cfg.vertical.vz_limit_mps) + + # ---- ⑦ 距离通道（§6.2 / §6.3） ---- + # 雷达距离不得直接当目标距离：仅关联成立且未超龄才可用（§6.3、FR-041） + distance_available = (snapshot.associated_to_target + and snapshot.radar_distance_m > 0.0 + and not is_stale(snapshot.radar_time_ms, now_ms, + self._cfg.control.radar_stale_ms)) + if distance_available: + # Δd = d − d_exp：d > d_exp 前向接近，d < d_exp 减速/后退（§6.2） + delta_d = snapshot.radar_distance_m - self._cfg.distance.d_exp_m + vx_raw = self._distance_pid.update(delta_d, dt_s) + slewed = self._vx_limiter.limit(vx_raw, dt_s) # 加速度限幅（§8） + out.vx_mps = clamp(slewed, + -self._cfg.distance.retreat_velocity_limit_mps, + self._cfg.distance.approach_velocity_limit_mps) + else: + # 无可用目标距离：清积分防饱和，按配置降级动作（§6.3） + self._distance_pid.reset_integral() + action = self._cfg.distance.no_distance_action + if action is NoDistanceAction.HOLD: + # 保持：前向指令清零（安全占位语义） + out.vx_mps = 0.0 + self._vx_limiter.reset(0.0) + elif action is NoDistanceAction.SLOW_APPROACH: + # 限速接近：缓速前向，同样经加速度限幅 + out.vx_mps = self._vx_limiter.limit( + self._cfg.distance.no_distance_approach_limit_mps, dt_s) + else: # NoDistanceAction.EXIT + # 退出近距操作：整体降级保持 + out.mode = ControlMode.DEGRADED_HOLD + out.degraded_reason = DegradedReason.NO_DISTANCE_EXIT + return self._finish_and_log(out, "距离未关联，退出近距操作") + + # ---- ⑧ 机体系 → 局部 NED 旋转（§2.3，A4 修复） ---- + # 各通道限幅均在机体系完成（旋转为正交变换，合速度不变，限幅约束 + # 不被破坏）；输出端随快照航向旋转为北/东/地。旋转前记录机体系前向 + # 分量，供 MAVSDK 机体速度设定点直接使用（免反旋）。 + out.vx_body_mps = out.vx_mps + out.vx_mps, out.vy_mps, out.vz_mps = rotate_body_to_ned( + out.vx_mps, out.vy_mps, out.vz_mps, snapshot.yaw_rad) + + out.valid = True + return self._finish_and_log(out, + "Coast 预测跟踪" if out.coast_active + else "正常跟踪") + + def reset(self) -> None: + """停止视觉闭环时清全部状态（§6.4）。""" + self._distance_pid.reset() + self._yaw_deg_limiter.reset() + self._yaw_rate_limiter.reset() + self._vx_limiter.reset() + self._vz_limiter.reset() + self._last_now_ms = 0 + self._last_mode = ControlMode.DEGRADED_HOLD + + def _finish_and_log(self, out: ControlOutput, reason: str) -> ControlOutput: + """仅模式变化时打印状态切换日志（刹车进出、降级切换等）。""" + if out.mode is not self._last_mode: + logger.info("[ControlLaw] 状态切换: %s -> %s (%s)", + _MODE_NAMES[self._last_mode], _MODE_NAMES[out.mode], + reason) + self._last_mode = out.mode + return out diff --git a/utils/visual_track_control/types.py b/utils/visual_track_control/types.py new file mode 100644 index 0000000..01353f3 --- /dev/null +++ b/utils/visual_track_control/types.py @@ -0,0 +1,95 @@ +"""视觉跟踪控制律的中立输入/输出结构。 + +对应 C++ 参考实现 reference-c++/visual_track_control/include/visual_track_control/vtc_types.h。 + +解耦说明：C++ 版的 ControlSnapshot 直接内嵌 target_tracker::TrackResult；本包为保持 +与 target_tracker 包互不依赖，自带字段对齐的 TrackState 值类型。调用方（胶水层） +负责把 target_tracker.TrackResult 转换为 TrackState（字段均为 plain tuple/标量）。 +""" + +from dataclasses import dataclass, field +from enum import Enum, auto +from typing import Optional + + +class ControlMode(Enum): + """控制输出模式（§2.2 控制意图语义）。""" + + VELOCITY_HEADING = auto() # 速度与航向意图 + BRAKE_HOVER = auto() # 零速刹车/悬停：判丢等场景 + DEGRADED_HOLD = auto() # 降级保持：超龄/退出近距操作，禁止正常控制 + + +class DegradedReason(Enum): + """降级原因（仅 DEGRADED_HOLD 模式有意义）。""" + + NONE = auto() + ATTITUDE_STALE = auto() # PX4 姿态过期：禁止空间转换和控制（§9.2） + VISION_STALE = auto() # 视觉追踪结果超龄（§7.1） + NO_DISTANCE_EXIT = auto() # 无距离关联且配置为退出近距操作（§6.3） + + +@dataclass +class TrackState: + """追踪结果视图（字段语义与 target_tracker.TrackResult 对齐，但独立定义）。""" + + tracked: bool = False + primary_id: Optional[int] = None + center: tuple[float, float] = (0.0, 0.0) + box: Optional[tuple[float, float, float, float]] = None + confidence: float = 0.0 + is_predicted: bool = False + lost_frames: int = 0 + n_active: int = 0 + raw: Optional[tuple[float, float, float, float]] = None + + +@dataclass +class ControlSnapshot: + """控制律输入快照：调用方在同一控制周期内采集并打戳。 + + TrackState 本身不含时间戳（§7.2），调用方必须在取样瞬间用单调时钟 + 打戳填入 track_time_ms；超龄判定一律使用"取样时间 + valid_for_ms"。 + """ + + track: TrackState = field(default_factory=TrackState) + track_time_ms: int = 0 # 取样单调时钟（毫秒），<=0 视为从未取样 + + radar_distance_m: float = 0.0 # 机头前向单点距离（米） + associated_to_target: bool = False # 相机—雷达空间关联成立标志（未关联不得当目标距离） + radar_time_ms: int = 0 + + attitude_present: bool = False # 姿态源当前是否可用（过期 → 禁止控制） + attitude_time_ms: int = 0 + yaw_rad: float = 0.0 # 航向（弧度，NED）；参与 NED 旋转与位置式航向合成 + roll_rad: float = 0.0 # 以下为预留字段（机体补偿未实现，§4.1） + pitch_rad: float = 0.0 + yaw_rate_radps: float = 0.0 + velocity_x_mps: float = 0.0 + velocity_y_mps: float = 0.0 + velocity_z_mps: float = 0.0 + + +@dataclass +class ControlOutput: + """控制律输出：限幅后的控制意图。 + + 坐标语义：vx/vy/vz 为局部 NED（北/东/地）分量——各通道在机体系内计算 + 并限幅后，随 snapshot.yaw_rad 旋转得到（旋转为正交变换，合速度不变）。 + vx_body_mps 为旋转前的机体系前向分量，供 MAVSDK 速度机体设定点使用 + （机体系侧向恒为 0，故不另设右向字段；vz 两系相同）。 + """ + + mode: ControlMode = ControlMode.DEGRADED_HOLD + valid: bool = False + + vx_mps: float = 0.0 # 北向（§6.2） + vy_mps: float = 0.0 # 东向（机体系恒 0，航向旋转后可非零） + vz_mps: float = 0.0 # 垂直（NED 下正，§5.2） + vx_body_mps: float = 0.0 # 机体系前向分量（旋转前，MAVSDK 机体设定点用） + + yaw_deg: float = 0.0 # 位置式目标绝对航向（度，NED，右转为正，回绕 [-180,180)） + yaw_rate_dps: float = 0.0 # 速率式偏航角速度指令（度/秒，右转为正） + + degraded_reason: DegradedReason = DegradedReason.NONE + coast_active: bool = False