first commit
This commit is contained in:
Generated
+5
@@ -0,0 +1,5 @@
|
||||
# 默认忽略的文件
|
||||
/shelf/
|
||||
/workspace.xml
|
||||
# 基于编辑器的 HTTP 客户端请求
|
||||
/httpRequests/
|
||||
Generated
+8
@@ -0,0 +1,8 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<module type="PYTHON_MODULE" version="4">
|
||||
<component name="NewModuleRootManager">
|
||||
<content url="file://$MODULE_DIR$" />
|
||||
<orderEntry type="inheritedJdk" />
|
||||
<orderEntry type="sourceFolder" forTests="false" />
|
||||
</component>
|
||||
</module>
|
||||
+27
@@ -0,0 +1,27 @@
|
||||
<component name="InspectionProjectProfileManager">
|
||||
<profile version="1.0">
|
||||
<option name="myName" value="Project Default" />
|
||||
<inspection_tool class="PyPep8NamingInspection" enabled="true" level="WEAK WARNING" enabled_by_default="true">
|
||||
<option name="ignoredErrors">
|
||||
<list>
|
||||
<option value="N801" />
|
||||
<option value="N806" />
|
||||
</list>
|
||||
</option>
|
||||
</inspection_tool>
|
||||
<inspection_tool class="PyStubPackagesAdvertiser" enabled="true" level="WARNING" enabled_by_default="true">
|
||||
<option name="ignoredPackages">
|
||||
<list>
|
||||
<option value="scipy" />
|
||||
</list>
|
||||
</option>
|
||||
</inspection_tool>
|
||||
<inspection_tool class="PyUnresolvedReferencesInspection" enabled="true" level="WARNING" enabled_by_default="true">
|
||||
<option name="ignoredIdentifiers">
|
||||
<list>
|
||||
<option value="moviepy.*" />
|
||||
</list>
|
||||
</option>
|
||||
</inspection_tool>
|
||||
</profile>
|
||||
</component>
|
||||
+6
@@ -0,0 +1,6 @@
|
||||
<component name="InspectionProjectProfileManager">
|
||||
<settings>
|
||||
<option name="USE_PROJECT_PROFILE" value="false" />
|
||||
<version value="1.0" />
|
||||
</settings>
|
||||
</component>
|
||||
Generated
+8
@@ -0,0 +1,8 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<project version="4">
|
||||
<component name="ProjectModuleManager">
|
||||
<modules>
|
||||
<module fileurl="file://$PROJECT_DIR$/.idea/code_drone-test.iml" filepath="$PROJECT_DIR$/.idea/code_drone-test.iml" />
|
||||
</modules>
|
||||
</component>
|
||||
</project>
|
||||
@@ -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 → 汇总报告。
|
||||
+371
@@ -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）。
|
||||
+1029
File diff suppressed because it is too large
Load Diff
@@ -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<double,double> last_center;
|
||||
std::optional<BoxXYXY> 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<int> primary_id;
|
||||
std::pair<double,double> center; // tracked=false 时无效
|
||||
std::optional<BoxXYXY> box;
|
||||
double confidence = 0.0;
|
||||
bool is_predicted = false;
|
||||
int lost_frames = 0;
|
||||
int n_active = 0;
|
||||
std::optional<BoxXYXY> 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<Detection>& dets,
|
||||
std::optional<FrameShape> frame_shape);
|
||||
TrackResult last_result() const; // 需要跨线程时加锁/原子快照
|
||||
void reset();
|
||||
};
|
||||
```
|
||||
|
||||
### 9.2 STL 容器选择
|
||||
|
||||
| 代码中的结构 | C++ 建议 |
|
||||
|--------------|----------|
|
||||
| 轨道哈希表 `{id: Tracklet}` | `std::unordered_map<int, Tracklet>`；若需按 ID 有序遍历可用 `std::map` |
|
||||
| 关联中的已分配集合 | `std::unordered_set<int>`，或目标数量少时直接用 `std::vector<bool>` 位标记 |
|
||||
| 按置信度排序的轨道视图 | 复制 ID/指针到 `std::vector` 后 `std::sort`（不直接排哈希表） |
|
||||
| 检测列表 | `std::vector<Detection>`，`Detection` 为 POD 结构体 |
|
||||
| 结果缓存 | `TrackResult` 值类型；跨线程用互斥锁保护的拷贝，或双缓冲原子交换 |
|
||||
|
||||
### 9.3 数值计算
|
||||
|
||||
- 矩阵规模极小（8×8、4×8、4×4），**无需引入 Eigen/OpenCV**：
|
||||
用固定大小二维数组或 `std::array<double, 64>` 手写矩阵乘法即可。
|
||||
- 唯一需要通用实现的是 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()`，
|
||||
则需整体加锁。
|
||||
@@ -0,0 +1,85 @@
|
||||
cmake_minimum_required(VERSION 3.16)
|
||||
|
||||
# =============================================================================
|
||||
# target_tracker —— 视觉追踪库（独立动态库工程）
|
||||
#
|
||||
# 轻量级多目标追踪过滤器：恒定速度卡尔曼滤波 + 贪婪关联 + 主目标锁定。
|
||||
# 规格见 docs/视觉追踪技术路线.md；实现文档见 target_tracker.md。
|
||||
# 零第三方依赖（不引入 spdlog / Eigen / OpenCV），仅链接 C++ 标准库。
|
||||
# =============================================================================
|
||||
|
||||
project(target_tracker VERSION 1.0.0 LANGUAGES CXX)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
set(CMAKE_CXX_EXTENSIONS OFF)
|
||||
|
||||
# ---- 动态库目标 ----
|
||||
add_library(target_tracker SHARED
|
||||
src/kalman_box_filter.cpp
|
||||
src/tracklet.cpp
|
||||
src/target_tracker.cpp)
|
||||
target_include_directories(target_tracker PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
|
||||
target_compile_features(target_tracker PUBLIC cxx_std_17)
|
||||
|
||||
# 版本与符号：VERSION 1.0.0 / SOVERSION 1；默认符号可见性（不加
|
||||
# visibility=hidden 与导出宏），全部公共类直接对外导出。
|
||||
set_target_properties(target_tracker PROPERTIES
|
||||
VERSION 1.0.0
|
||||
SOVERSION 1)
|
||||
|
||||
# 编译告警：与主工程一致（非 MSVC：-Wall -Wextra -Wpedantic）
|
||||
if(MSVC)
|
||||
target_compile_options(target_tracker PRIVATE /W4 /permissive-)
|
||||
else()
|
||||
target_compile_options(target_tracker PRIVATE -Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# ---- 单元测试（GTest；缺失时仅提示不失败，与主工程策略一致） ----
|
||||
include(CTest)
|
||||
if(BUILD_TESTING)
|
||||
find_package(GTest QUIET)
|
||||
if(GTest_FOUND)
|
||||
include(GoogleTest)
|
||||
|
||||
# 测试可执行文件直接链接共享库 target_tracker；
|
||||
# BUILD_RPATH 指向库输出目录，保证 ctest 无需 LD_LIBRARY_PATH 即可运行
|
||||
set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR};$ORIGIN")
|
||||
|
||||
# 卡尔曼滤波器测试：初始化状态、静止收敛、匀速速度收敛与预测超前、未初始化行为
|
||||
add_executable(kalman_box_filter_test tests/kalman_box_filter_test.cpp)
|
||||
target_link_libraries(kalman_box_filter_test PRIVATE
|
||||
target_tracker GTest::gtest_main)
|
||||
if(MSVC)
|
||||
target_compile_options(kalman_box_filter_test PRIVATE /W4 /permissive-)
|
||||
else()
|
||||
target_compile_options(kalman_box_filter_test PRIVATE -Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
gtest_discover_tests(kalman_box_filter_test)
|
||||
|
||||
# 轨道与几何工具测试：IoU 边界、IsDead/IsCoasting、Predict/Update/MarkMiss 计数
|
||||
add_executable(tracklet_test tests/tracklet_test.cpp)
|
||||
target_link_libraries(tracklet_test PRIVATE
|
||||
target_tracker GTest::gtest_main)
|
||||
if(MSVC)
|
||||
target_compile_options(tracklet_test PRIVATE /W4 /permissive-)
|
||||
else()
|
||||
target_compile_options(tracklet_test PRIVATE -Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
gtest_discover_tests(tracklet_test)
|
||||
|
||||
# 追踪器测试：逐条覆盖文档 §9.4 六条行为一致性验收用例
|
||||
add_executable(target_tracker_test tests/target_tracker_test.cpp)
|
||||
target_link_libraries(target_tracker_test PRIVATE
|
||||
target_tracker GTest::gtest_main)
|
||||
if(MSVC)
|
||||
target_compile_options(target_tracker_test PRIVATE /W4 /permissive-)
|
||||
else()
|
||||
target_compile_options(target_tracker_test PRIVATE -Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
gtest_discover_tests(target_tracker_test)
|
||||
else()
|
||||
message(STATUS "GoogleTest not found; target_tracker tests will not be built")
|
||||
endif()
|
||||
endif()
|
||||
@@ -0,0 +1,87 @@
|
||||
#pragma once
|
||||
|
||||
// =============================================================================
|
||||
// kalman_box_filter.h —— 单目标 8 维恒定速度卡尔曼滤波器
|
||||
//
|
||||
// 对应规格：docs/视觉追踪技术路线.md §4（卡尔曼滤波算法细节）、§8.2（内部常量）
|
||||
// 状态向量 x[8] = [cx, cy, w, h, vx, vy, vw, vh]（§4.1）
|
||||
// 观测向量 z[4] = [cx, cy, w, h]（§4.1）
|
||||
// 时间步长 dt = 1 帧，与帧率无关（§1.3）
|
||||
//
|
||||
// 实现约束（§9.3）：矩阵规模极小（8×8 / 4×8 / 4×4），手写定长矩阵运算，
|
||||
// 全程 double 精度，不引入 Eigen/OpenCV 等线性代数库。
|
||||
// =============================================================================
|
||||
|
||||
#include <array>
|
||||
#include <optional>
|
||||
|
||||
namespace drone {
|
||||
namespace tracker {
|
||||
|
||||
/// 轴对齐边界框（中心点 + 宽高表示，卡尔曼观测空间）
|
||||
struct BoxCXCYWH {
|
||||
double cx = 0.0; // 框中心 x（像素）
|
||||
double cy = 0.0; // 框中心 y（像素）
|
||||
double w = 0.0; // 框宽（像素）
|
||||
double h = 0.0; // 框高（像素）
|
||||
};
|
||||
|
||||
/// 单目标恒定速度卡尔曼滤波器（§3.1 / §4）
|
||||
class KalmanBoxFilter {
|
||||
public:
|
||||
KalmanBoxFilter() = default;
|
||||
|
||||
/// 是否已用首次观测初始化（§3.1 initialized 字段）
|
||||
bool Initialized() const { return initialized_; }
|
||||
|
||||
/// 用首次观测 [cx, cy, w, h] 初始化（§4.3）：
|
||||
/// 状态速度置零；P = diag(2,2,2,2,100,100,100,100)。
|
||||
void Initiate(const BoxCXCYWH& z);
|
||||
|
||||
/// 预测推进一帧（§4.4）：未初始化返回 std::nullopt；
|
||||
/// 否则 x ← F·x，P ← F·P·Fᵀ + Q，返回状态前 4 维 [cx,cy,w,h] 副本。
|
||||
std::optional<BoxCXCYWH> Predict();
|
||||
|
||||
/// 观测更新（§4.5）：未初始化时直接返回；
|
||||
/// S = H·P·Hᵀ + R，K = P·Hᵀ·S⁻¹，x ← x + K(z−Hx)，P ← (I−KH)P。
|
||||
void Update(const BoxCXCYWH& z);
|
||||
|
||||
/// 状态向量只读访问（[cx, cy, w, h, vx, vy, vw, vh]，测试与排查用）
|
||||
const std::array<double, 8>& State() const { return x_; }
|
||||
|
||||
// ---- 内部常量（§4.2 / §8.2，经验值，针对 640×480 画面调优） ----
|
||||
static constexpr double kStdPos = 1.0 / 50.0; // STD_POS：位置噪声比例系数
|
||||
static constexpr double kStdVel = 1.0 / 200.0; // STD_VEL：速度噪声比例系数
|
||||
static constexpr double kIniPos = 2.0; // INI_POS：初始位置协方差
|
||||
static constexpr double kIniVel = 100.0; // INI_VEL：初始速度协方差
|
||||
static constexpr double kRScale = 2.0; // R 额外缩放因子（增强平滑，§8.2）
|
||||
|
||||
private:
|
||||
std::array<double, 8> x_{}; // 状态向量（§3.1）
|
||||
std::array<std::array<double, 8>, 8> P_{}; // 状态协方差 8×8（§3.1）
|
||||
bool initialized_ = false; // 是否已初始化（§3.1）
|
||||
|
||||
// 状态转移矩阵 F（常量，§4.1）：I4 + 右上 I4，即 F[i][i]=1、F[i][i+4]=1
|
||||
static constexpr std::array<std::array<double, 8>, 8> kF = {{
|
||||
{{1, 0, 0, 0, 1, 0, 0, 0}},
|
||||
{{0, 1, 0, 0, 0, 1, 0, 0}},
|
||||
{{0, 0, 1, 0, 0, 0, 1, 0}},
|
||||
{{0, 0, 0, 1, 0, 0, 0, 1}},
|
||||
{{0, 0, 0, 0, 1, 0, 0, 0}},
|
||||
{{0, 0, 0, 0, 0, 1, 0, 0}},
|
||||
{{0, 0, 0, 0, 0, 0, 1, 0}},
|
||||
{{0, 0, 0, 0, 0, 0, 0, 1}},
|
||||
}};
|
||||
|
||||
// 观测矩阵 H（常量，§4.1）：[I4 | 0]（4 行 × 8 列），
|
||||
// 直接观测位置与尺寸、不观测速度
|
||||
static constexpr std::array<std::array<double, 8>, 4> kH = {{
|
||||
{{1, 0, 0, 0, 0, 0, 0, 0}},
|
||||
{{0, 1, 0, 0, 0, 0, 0, 0}},
|
||||
{{0, 0, 1, 0, 0, 0, 0, 0}},
|
||||
{{0, 0, 0, 1, 0, 0, 0, 0}},
|
||||
}};
|
||||
};
|
||||
|
||||
} // namespace tracker
|
||||
} // namespace drone
|
||||
@@ -0,0 +1,123 @@
|
||||
#pragma once
|
||||
|
||||
// =============================================================================
|
||||
// target_tracker.h —— 多目标追踪过滤器（TargetTracker）对外接口
|
||||
//
|
||||
// 对应规格：docs/视觉追踪技术路线.md
|
||||
// §2 输入输出接口定义（Detection / TrackResult）
|
||||
// §3.3 TargetTracker（管理器）
|
||||
// §5 检测-轨迹关联（贪婪最邻近，§5.3）
|
||||
// §6 轨迹生命周期与 ID 管理（主目标锁定与选举，§6.3）
|
||||
// §7 主流程（每帧 Update 六步）
|
||||
//
|
||||
// 线程模型（§9.5）：Update() 由单线程（推理主循环）调用；库内仅对
|
||||
// last_result 缓存加一把 std::mutex，LastResult() 返回受保护拷贝。
|
||||
// 本库零第三方依赖（不依赖 spdlog / Eigen / OpenCV）。
|
||||
// =============================================================================
|
||||
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "tracklet.h"
|
||||
|
||||
namespace drone {
|
||||
namespace tracker {
|
||||
|
||||
/// 单个检测框（§2.2 输入 detections 元素：[x1,y1,x2,y2,conf,cls_id]）
|
||||
struct Detection {
|
||||
double x1 = 0.0; // 边界框左上角 x（像素坐标）
|
||||
double y1 = 0.0; // 边界框左上角 y
|
||||
double x2 = 0.0; // 边界框右下角 x
|
||||
double y2 = 0.0; // 边界框右下角 y
|
||||
double conf = 0.0; // 检测置信度 ∈ [0,1]
|
||||
int cls_id = 0; // 类别编号
|
||||
};
|
||||
|
||||
/// 画面尺寸（height, width，§2.2）：仅在"无实测轨道、只有 Coast 轨道"
|
||||
/// 时用于选取离画面中心最近的轨道；缺省按 640×480 中心 (320,240)。
|
||||
struct FrameShape {
|
||||
int height = 0;
|
||||
int width = 0;
|
||||
};
|
||||
|
||||
/// 每帧追踪输出（§2.3）
|
||||
struct TrackResult {
|
||||
bool tracked = false; // 是否存在有效主目标；false 时调用方应刹车
|
||||
std::optional<int> primary_id; // 主目标轨道 ID；无目标时为空
|
||||
std::pair<double, double> center{0.0, 0.0}; // 平滑后的目标中心点（tracked=false 时无效）
|
||||
std::optional<BoxXYXY> box; // 平滑后的边界框
|
||||
double confidence = 0.0; // 当前置信度（Coast 期间继承最近实测值）
|
||||
bool is_predicted = false; // true = Coast（仅预测、无实测）
|
||||
int lost_frames = 0; // 主目标连续未命中帧数
|
||||
int n_active = 0; // 当前活跃轨道总数
|
||||
std::optional<BoxXYXY> raw; // 关联到的原始检测框；Coast 时为空（§2.3）
|
||||
};
|
||||
|
||||
/// 多目标追踪过滤器（§1.1 / §3.3）
|
||||
class TargetTracker {
|
||||
public:
|
||||
/// 追踪器配置参数（§8.1，默认值取自规格）
|
||||
struct Config {
|
||||
int max_lost_frames = 8; // 最大连续丢失帧数（严格大于才判死，§6.1）
|
||||
double max_association_dist = 200.0; // 中心距离软阈值 Dmax（像素，§5.1）
|
||||
int min_hits = 3; // 轨道"确认"所需累计匹配次数（§6.1）
|
||||
double dist_weight = 0.5; // 关联代价距离项权重（§5.1）
|
||||
double iou_weight = 0.5; // 关联代价 (1−IoU) 项权重（§5.1）
|
||||
};
|
||||
|
||||
/// 默认配置构造（等价于 Config{}，§8.1 默认值）
|
||||
TargetTracker();
|
||||
|
||||
/// 指定配置构造
|
||||
explicit TargetTracker(const Config& cfg);
|
||||
|
||||
/// 每帧调用一次（§7 六步主流程）：输入检测列表与可选画面尺寸，
|
||||
/// 输出主目标追踪结果并缓存（供 LastResult 跨线程读取）。
|
||||
TrackResult Update(const std::vector<Detection>& detections,
|
||||
std::optional<FrameShape> frame_shape = std::nullopt);
|
||||
|
||||
/// 上一帧结果快照（线程安全：返回受互斥锁保护的拷贝，§9.5）
|
||||
TrackResult LastResult() const;
|
||||
|
||||
/// 清空所有轨道、ID 计数器、锁定状态与缓存结果（§2.1）；next_id 归零（§6.2）
|
||||
void Reset();
|
||||
|
||||
/// 当前存活轨道数量（§2.1）
|
||||
int ActiveCount() const;
|
||||
|
||||
/// 当前所有轨道的只读快照（§2.1；值拷贝）
|
||||
std::unordered_map<int, Tracklet> ActiveTracks() const;
|
||||
|
||||
/// 配置只读访问
|
||||
const Config& GetConfig() const { return config_; }
|
||||
|
||||
private:
|
||||
/// 贪婪关联结果（§5.3）：matches 元素为 (track_id, 检测索引)
|
||||
struct AssociationResult {
|
||||
std::vector<std::pair<int, int>> matches;
|
||||
std::vector<int> unmatched_track_ids;
|
||||
std::vector<int> unmatched_det_indices;
|
||||
};
|
||||
|
||||
/// 贪婪最邻近关联（§5.3）：按轨道 confidence 降序逐一匹配，
|
||||
/// cost = w_dist·dist/max(Dmax,1e-3) + w_iou·(1−IoU)，仅 cost 严格小于
|
||||
/// best_cost（初始 1.0）才匹配，即 cost ≥ 1.0 判为不可匹配（§5.1）。
|
||||
AssociationResult AssociateDetections(const std::vector<Detection>& detections);
|
||||
|
||||
/// 主目标选择/锁定延续（§6.3 SELECT_PRIMARY）
|
||||
TrackResult SelectPrimary(std::optional<FrameShape> frame_shape);
|
||||
|
||||
Config config_; // 配置参数（§3.3）
|
||||
int next_id_ = 0; // 下一个可分配 ID，从 0 递增不复用（§6.2）
|
||||
std::unordered_map<int, Tracklet> tracks_; // 所有存活轨道（§3.3）
|
||||
long long frame_count_ = 0; // 已处理帧数（仅统计用，§3.3）
|
||||
std::optional<int> locked_primary_id_; // 当前锁定的主目标 ID（§3.3）
|
||||
TrackResult last_result_; // 上一帧结果缓存（§3.3）
|
||||
mutable std::mutex last_result_mutex_; // 仅保护 last_result_（§9.5）
|
||||
};
|
||||
|
||||
} // namespace tracker
|
||||
} // namespace drone
|
||||
@@ -0,0 +1,93 @@
|
||||
#pragma once
|
||||
|
||||
// =============================================================================
|
||||
// tracklet.h —— 轨道（Tracklet）与基础几何工具
|
||||
//
|
||||
// 对应规格：docs/视觉追踪技术路线.md
|
||||
// §3.2 Tracklet（轨道）字段定义与派生状态
|
||||
// §5.2 IoU 计算
|
||||
// §7 主流程伪代码末尾 TRACKLET_PREDICT / TRACKLET_UPDATE
|
||||
//
|
||||
// 几何量说明：BoxCXCYWH（卡尔曼观测空间 [cx,cy,w,h]）定义于
|
||||
// kalman_box_filter.h，本文件通过 include 复用并同时提供 BoxXYXY
|
||||
// （像素坐标 [x1,y1,x2,y2]）与两种表示的互转函数。
|
||||
// 本库只依赖 C++ 标准库，无任何第三方依赖。
|
||||
// =============================================================================
|
||||
|
||||
#include <optional>
|
||||
#include <utility>
|
||||
|
||||
#include "kalman_box_filter.h"
|
||||
|
||||
namespace drone {
|
||||
namespace tracker {
|
||||
|
||||
// ---- 基础几何量（§9.1 模块/类划分草案） ----
|
||||
|
||||
/// 轴对齐边界框（像素坐标，左上/右下角表示）
|
||||
struct BoxXYXY {
|
||||
double x1 = 0.0;
|
||||
double y1 = 0.0;
|
||||
double x2 = 0.0;
|
||||
double y2 = 0.0;
|
||||
};
|
||||
|
||||
/// [x1,y1,x2,y2] → [cx,cy,w,h]（§7 TRACKLET_UPDATE 中的转换）
|
||||
BoxCXCYWH ToCenterSize(const BoxXYXY& box);
|
||||
|
||||
/// [cx,cy,w,h] → [x1,y1,x2,y2]（§7 TRACKLET_PREDICT 中的 center_size 逆变换）
|
||||
BoxXYXY ToXYXY(const BoxCXCYWH& box);
|
||||
|
||||
/// 标准轴对齐 IoU（§5.2）：
|
||||
/// 任一框为空、交集面积 ≤ 0 或并集 ≤ 0 时返回 0。
|
||||
double ComputeIoU(const BoxXYXY& a, const BoxXYXY& b);
|
||||
|
||||
// =============================================================================
|
||||
// Tracklet —— 单条轨道：生命周期计数 + 辅助状态 + 卡尔曼滤波器（§3.2）
|
||||
// =============================================================================
|
||||
struct Tracklet {
|
||||
/// 构造即创建轨道（§7 步骤 5 / §6.1 新建阶段）：
|
||||
/// 用首次观测初始化滤波器，age=1、hits=1、lost_count=0，
|
||||
/// last_center / last_box 立即由观测重建（保证创建当帧即可参与输出）。
|
||||
Tracklet(int id, const BoxCXCYWH& measurement, double conf, int cls,
|
||||
int max_lost_frames);
|
||||
|
||||
int track_id = 0; // 全局唯一轨道 ID（§3.2）
|
||||
KalmanBoxFilter kf; // 本轨道的滤波器（§3.2）
|
||||
int lost_count = 0; // 当前连续未匹配帧数（§3.2）
|
||||
int max_lost = 0; // 最大允许连续丢失帧数（构造时注入，§3.2）
|
||||
double confidence = 0.0; // 最近一次匹配的置信度（§3.2）
|
||||
int cls_id = 0; // 最近一次匹配的类别 ID（§3.2）
|
||||
int age = 1; // 轨道已存在的总帧数（创建时为 1，§3.2）
|
||||
int hits = 1; // 累计成功匹配次数（创建时为 1，§3.2）
|
||||
std::pair<double, double> last_center{0.0, 0.0}; // 最近一次中心点（实测或预测）
|
||||
std::optional<BoxXYXY> last_box; // 最近一次边界框（实测或预测，§3.2）
|
||||
std::optional<BoxXYXY> last_raw_box; // 最近一次实测关联到的原始检测框；
|
||||
// Coast 期间保留不更新，仅内部参考；
|
||||
// 对外输出语义按 §2.3：Coast 时 raw 置空
|
||||
|
||||
// ---- 派生状态（§3.2 只读属性） ----
|
||||
|
||||
/// is_dead := lost_count > max_lost（严格大于，§6.1 注记 2）
|
||||
bool IsDead() const { return lost_count > max_lost; }
|
||||
|
||||
/// is_coasting := lost_count > 0（处于纯预测状态，§3.2）
|
||||
bool IsCoasting() const { return lost_count > 0; }
|
||||
|
||||
// ---- 生命周期方法（§7 末尾伪代码） ----
|
||||
|
||||
/// TRACKLET_PREDICT：卡尔曼预测推进一帧。
|
||||
/// 预测成功时更新 last_center / last_box 且 age+1。
|
||||
void Predict();
|
||||
|
||||
/// TRACKLET_UPDATE：框 [x1,y1,x2,y2] → [cx,cy,w,h] → kf.update；
|
||||
/// lost_count 清零、hits+1、age+1，更新
|
||||
/// last_center / last_box / last_raw_box / confidence / cls_id。
|
||||
void Update(const BoxXYXY& box, double conf, int cls);
|
||||
|
||||
/// 未匹配计丢失：lost_count+1、age+1（§7 步骤 4）。
|
||||
void MarkMiss();
|
||||
};
|
||||
|
||||
} // namespace tracker
|
||||
} // namespace drone
|
||||
@@ -0,0 +1,235 @@
|
||||
// =============================================================================
|
||||
// kalman_box_filter.cpp —— 单目标 8 维恒定速度卡尔曼滤波器实现
|
||||
//
|
||||
// 对应规格：docs/视觉追踪技术路线.md §4（算法细节）、§8.2（内部常量）、
|
||||
// §9.3（数值计算：定长数组手写矩阵乘法，4×4 高斯-约当求逆 + 1e-9 正则化）。
|
||||
// 关键公式逐行注释并标注对照文档小节号。
|
||||
// =============================================================================
|
||||
|
||||
#include "target_tracker/kalman_box_filter.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <utility>
|
||||
|
||||
namespace drone {
|
||||
namespace tracker {
|
||||
namespace {
|
||||
|
||||
// ---- 定长矩阵基础运算（§9.3：无需引入 Eigen/OpenCV） ----
|
||||
|
||||
/// 8×8 矩阵乘法：C = A·B
|
||||
std::array<std::array<double, 8>, 8> MatMul8(const std::array<std::array<double, 8>, 8>& a,
|
||||
const std::array<std::array<double, 8>, 8>& b) {
|
||||
std::array<std::array<double, 8>, 8> c{};
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
for (int k = 0; k < 8; ++k) {
|
||||
if (a[i][k] == 0.0) continue; // 稀疏常量矩阵加速，结果不变
|
||||
for (int j = 0; j < 8; ++j) {
|
||||
c[i][j] += a[i][k] * b[k][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
return c;
|
||||
}
|
||||
|
||||
/// 8×8 矩阵转置：R = Aᵀ
|
||||
std::array<std::array<double, 8>, 8> MatTranspose8(
|
||||
const std::array<std::array<double, 8>, 8>& a) {
|
||||
std::array<std::array<double, 8>, 8> r{};
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
for (int j = 0; j < 8; ++j) {
|
||||
r[i][j] = a[j][i];
|
||||
}
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
||||
/// 4×4 矩阵求逆：高斯-约当消元 + 部分主元（§9.3）。
|
||||
/// 输入矩阵先对角加 1e-9 正则化防奇异（§9.3 建议）。
|
||||
/// 返回的 bool 表示是否求逆成功（主元退化时返回 false）。
|
||||
bool Invert4x4(const std::array<std::array<double, 4>, 4>& m,
|
||||
std::array<std::array<double, 4>, 4>& out) {
|
||||
constexpr double kRegularization = 1e-9; // 正则化极小量（§9.3）
|
||||
|
||||
// 增广矩阵 [M + εI | I]，消元后左半变为单位阵、右半即为逆矩阵
|
||||
std::array<std::array<double, 8>, 4> aug{};
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
for (int j = 0; j < 4; ++j) {
|
||||
aug[i][j] = m[i][j] + (i == j ? kRegularization : 0.0);
|
||||
aug[i][j + 4] = (i == j) ? 1.0 : 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
for (int col = 0; col < 4; ++col) {
|
||||
// 部分主元：选取当前列绝对值最大行交换，提高数值稳定性
|
||||
int pivot = col;
|
||||
for (int r = col + 1; r < 4; ++r) {
|
||||
if (std::abs(aug[r][col]) > std::abs(aug[pivot][col])) {
|
||||
pivot = r;
|
||||
}
|
||||
}
|
||||
if (std::abs(aug[pivot][col]) < 1e-12) {
|
||||
return false; // 主元退化（正则化后理论上不可达）
|
||||
}
|
||||
if (pivot != col) {
|
||||
std::swap(aug[pivot], aug[col]);
|
||||
}
|
||||
// 主元行归一化
|
||||
const double d = aug[col][col];
|
||||
for (int j = 0; j < 8; ++j) {
|
||||
aug[col][j] /= d;
|
||||
}
|
||||
// 消去其余行（高斯-约当：一次消成对角单位阵）
|
||||
for (int r = 0; r < 4; ++r) {
|
||||
if (r == col) continue;
|
||||
const double f = aug[r][col];
|
||||
if (f == 0.0) continue;
|
||||
for (int j = 0; j < 8; ++j) {
|
||||
aug[r][j] -= f * aug[col][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
for (int j = 0; j < 4; ++j) {
|
||||
out[i][j] = aug[i][j + 4];
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
void KalmanBoxFilter::Initiate(const BoxCXCYWH& z) {
|
||||
// §4.3 初始化：状态 = [cx, cy, w, h, 0, 0, 0, 0]（速度置零）
|
||||
x_ = {{z.cx, z.cy, z.w, z.h, 0.0, 0.0, 0.0, 0.0}};
|
||||
|
||||
// §4.3 协方差：P = diag(2,2,2,2,100,100,100,100)
|
||||
// （位置置信度中等，速度置信度低，允许前几帧快速收敛速度估计）
|
||||
for (auto& row : P_) row.fill(0.0);
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
P_[i][i] = kIniPos; // INI_POS = 2.0（§4.2）
|
||||
P_[i + 4][i + 4] = kIniVel; // INI_VEL = 100.0（§4.2）
|
||||
}
|
||||
initialized_ = true;
|
||||
}
|
||||
|
||||
std::optional<BoxCXCYWH> KalmanBoxFilter::Predict() {
|
||||
// §4.4：未初始化返回空
|
||||
if (!initialized_) {
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
// §4.2 动态过程噪声：Q = diag(σ_pos², σ_vel²)，σ 按状态中 w/h 缩放
|
||||
// σ_pos = [w/50, h/50, w/50, h/50]
|
||||
// σ_vel = [w/200, h/200, w/200, h/200]
|
||||
// （Q 直接使用状态值，不做 abs，与规格 §4.2 注记保持一致）
|
||||
const double w = x_[2];
|
||||
const double h = x_[3];
|
||||
const double std_pos[4] = {w * kStdPos, h * kStdPos, w * kStdPos, h * kStdPos};
|
||||
const double std_vel[4] = {w * kStdVel, h * kStdVel, w * kStdVel, h * kStdVel};
|
||||
std::array<std::array<double, 8>, 8> q{};
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
q[i][i] = std_pos[i] * std_pos[i]; // 前 4 对角元来自 σ_pos（各分量先平方）
|
||||
q[i + 4][i + 4] = std_vel[i] * std_vel[i]; // 后 4 对角元来自 σ_vel
|
||||
}
|
||||
|
||||
// §4.4 状态转移：x ← F·x（恒定速度模型：位置/尺寸各自加上对应速度）
|
||||
std::array<double, 8> x_new{};
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
double sum = 0.0;
|
||||
for (int j = 0; j < 8; ++j) {
|
||||
sum += kF[i][j] * x_[j];
|
||||
}
|
||||
x_new[i] = sum;
|
||||
}
|
||||
x_ = x_new;
|
||||
|
||||
// §4.4 协方差传播：P ← F·P·Fᵀ + Q
|
||||
P_ = MatMul8(MatMul8(kF, P_), MatTranspose8(kF));
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
for (int j = 0; j < 8; ++j) {
|
||||
P_[i][j] += q[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
// §4.4 返回预测的位置/尺寸（状态向量前 4 维的副本）
|
||||
return BoxCXCYWH{x_[0], x_[1], x_[2], x_[3]};
|
||||
}
|
||||
|
||||
void KalmanBoxFilter::Update(const BoxCXCYWH& z) {
|
||||
// §4.5：未初始化直接返回
|
||||
if (!initialized_) {
|
||||
return;
|
||||
}
|
||||
|
||||
// §4.2 动态观测噪声：R = diag(2(w/50)², 2(h/50)², 2(w/50)², 2(h/50)²)
|
||||
// （R 使用 abs(w)/abs(h) 防御负尺寸，并额外乘 2 增强平滑；
|
||||
// 该差异与规格 §4.2 注记保持一致）
|
||||
const double w = std::abs(x_[2]);
|
||||
const double h = std::abs(x_[3]);
|
||||
std::array<std::array<double, 4>, 4> r{};
|
||||
const double rw = kRScale * (w * kStdPos) * (w * kStdPos);
|
||||
const double rh = kRScale * (h * kStdPos) * (h * kStdPos);
|
||||
r[0][0] = rw;
|
||||
r[1][1] = rh;
|
||||
r[2][2] = rw;
|
||||
r[3][3] = rh;
|
||||
|
||||
// §4.5 新息协方差：S = H·P·Hᵀ + R（4×4）
|
||||
// H = [I4 | 0]，故 H·P·Hᵀ 即 P 的左上 4×4 子块
|
||||
std::array<std::array<double, 4>, 4> s{};
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
for (int j = 0; j < 4; ++j) {
|
||||
s[i][j] = P_[i][j] + r[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
// §4.5 / §9.3：S⁻¹（4×4 高斯-约当求逆 + 对角加 1e-9 正则化防奇异）
|
||||
std::array<std::array<double, 4>, 4> s_inv{};
|
||||
if (!Invert4x4(s, s_inv)) {
|
||||
return; // 正则化后理论不可达；防御性跳过本次更新
|
||||
}
|
||||
|
||||
// §4.5 卡尔曼增益：K = P·Hᵀ·S⁻¹（8×4）
|
||||
// H = [I4 | 0]，故 P·Hᵀ 即 P 的前 4 列（8×4）
|
||||
std::array<std::array<double, 4>, 8> k{};
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
for (int j = 0; j < 4; ++j) {
|
||||
double sum = 0.0;
|
||||
for (int l = 0; l < 4; ++l) {
|
||||
sum += P_[i][l] * s_inv[l][j];
|
||||
}
|
||||
k[i][j] = sum;
|
||||
}
|
||||
}
|
||||
|
||||
// §4.5 残差（innovation）：y = z − H·x（H·x 即状态前 4 维）
|
||||
const double z_vec[4] = {z.cx, z.cy, z.w, z.h};
|
||||
double y[4];
|
||||
for (int i = 0; i < 4; ++i) {
|
||||
y[i] = z_vec[i] - x_[i];
|
||||
}
|
||||
|
||||
// §4.5 状态更新：x ← x + K·y
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
double sum = 0.0;
|
||||
for (int j = 0; j < 4; ++j) {
|
||||
sum += k[i][j] * y[j];
|
||||
}
|
||||
x_[i] += sum;
|
||||
}
|
||||
|
||||
// §4.5 协方差更新：P ← (I8 − K·H)·P
|
||||
// K·H（8×8）= K 的前 4 列铺到前 4 列上（H 选通前 4 维）
|
||||
std::array<std::array<double, 8>, 8> ikh{};
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
for (int j = 0; j < 8; ++j) {
|
||||
ikh[i][j] = (i == j ? 1.0 : 0.0) - (j < 4 ? k[i][j] : 0.0);
|
||||
}
|
||||
}
|
||||
P_ = MatMul8(ikh, P_);
|
||||
}
|
||||
|
||||
} // namespace tracker
|
||||
} // namespace drone
|
||||
@@ -0,0 +1,312 @@
|
||||
// =============================================================================
|
||||
// target_tracker.cpp —— 多目标追踪过滤器（TargetTracker）实现
|
||||
//
|
||||
// 对应规格：docs/视觉追踪技术路线.md
|
||||
// §5 检测-轨迹关联（贪婪最邻近，§5.3）
|
||||
// §6 轨迹生命周期与 ID 管理（主目标锁定与选举，§6.3）
|
||||
// §7 主流程六步（UPDATE 伪代码）
|
||||
// §9.5 线程模型（单线程写 Update，仅 last_result 缓存加锁）
|
||||
// =============================================================================
|
||||
|
||||
#include "target_tracker/target_tracker.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
|
||||
namespace drone {
|
||||
namespace tracker {
|
||||
|
||||
TargetTracker::TargetTracker() : TargetTracker(Config{}) {}
|
||||
|
||||
TargetTracker::TargetTracker(const Config& cfg) : config_(cfg) {}
|
||||
|
||||
TrackResult TargetTracker::Update(const std::vector<Detection>& detections,
|
||||
std::optional<FrameShape> frame_shape) {
|
||||
// §7：frame_count += 1（仅统计用）
|
||||
++frame_count_;
|
||||
|
||||
// ---- 入口防御：过滤坐标或置信度含 NaN/Inf 的检测 ----
|
||||
// 防御性增强：上游检测器理论上已过滤，但库自身必须免疫——
|
||||
// 非有限值一旦进入卡尔曼状态将永久污染轨道并传导到主目标输出
|
||||
std::vector<Detection> valid_detections;
|
||||
valid_detections.reserve(detections.size());
|
||||
for (const Detection& det : detections) {
|
||||
if (std::isfinite(det.x1) && std::isfinite(det.y1) &&
|
||||
std::isfinite(det.x2) && std::isfinite(det.y2) &&
|
||||
std::isfinite(det.conf)) {
|
||||
valid_detections.push_back(det);
|
||||
}
|
||||
}
|
||||
|
||||
// ---- 步骤 1：所有存活轨道先行预测（推进一帧）----
|
||||
// （§7 步骤 1：t.predict() 更新 last_center / last_box、age += 1）
|
||||
for (auto& kv : tracks_) {
|
||||
kv.second.Predict();
|
||||
}
|
||||
|
||||
// ---- 步骤 2：构造检测中心表并贪婪关联（§7 步骤 2 / §5.3） ----
|
||||
AssociationResult assoc;
|
||||
if (!valid_detections.empty()) {
|
||||
assoc = AssociateDetections(valid_detections);
|
||||
} else {
|
||||
// 无检测：全部轨道视为未匹配，全部进入 Coast 计数（§7 步骤 2 否则分支）
|
||||
for (const auto& kv : tracks_) {
|
||||
assoc.unmatched_track_ids.push_back(kv.first);
|
||||
}
|
||||
}
|
||||
|
||||
// ---- 步骤 3：用匹配到的检测更新轨道（§7 步骤 3） ----
|
||||
for (const auto& match : assoc.matches) {
|
||||
const int track_id = match.first;
|
||||
const Detection& det = valid_detections[static_cast<size_t>(match.second)];
|
||||
// §7 步骤 3：由检测重建原始框 raw_box = [cx−w/2, cy−h/2, cx+w/2, cy+h/2]
|
||||
// （等价于检测框本身：cx=(x1+x2)/2、w=x2−x1 的逆运算，浮点上精确还原）
|
||||
const BoxXYXY raw_box{det.x1, det.y1, det.x2, det.y2};
|
||||
tracks_.at(track_id).Update(raw_box, det.conf, det.cls_id);
|
||||
}
|
||||
|
||||
// ---- 步骤 4：未匹配轨道计丢失，死亡者回收（§7 步骤 4） ----
|
||||
for (const int track_id : assoc.unmatched_track_ids) {
|
||||
auto it = tracks_.find(track_id);
|
||||
if (it == tracks_.end()) {
|
||||
continue;
|
||||
}
|
||||
it->second.MarkMiss(); // lost_count+1、age+1
|
||||
if (it->second.IsDead()) { // lost_count > max_lost（严格大于，§6.1）
|
||||
// 锁定的唯一解除途径之一：锁定轨道死亡被回收时清锁（§6.3）
|
||||
if (locked_primary_id_.has_value() && *locked_primary_id_ == track_id) {
|
||||
locked_primary_id_.reset();
|
||||
}
|
||||
tracks_.erase(it);
|
||||
}
|
||||
}
|
||||
|
||||
// ---- 步骤 5：未匹配检测 → 创建新轨道（§7 步骤 5 / §6.2） ----
|
||||
for (const int det_idx : assoc.unmatched_det_indices) {
|
||||
const Detection& det = valid_detections[static_cast<size_t>(det_idx)];
|
||||
// §7 步骤 5：以 [cx,cy,w,h] 创建；构造函数内重建 last_box，
|
||||
// 保证创建当帧即可参与输出；next_id 从 0 递增永不复用（§6.2）
|
||||
tracks_.emplace(next_id_,
|
||||
Tracklet(next_id_, ToCenterSize({det.x1, det.y1, det.x2, det.y2}),
|
||||
det.conf, det.cls_id, config_.max_lost_frames));
|
||||
++next_id_;
|
||||
}
|
||||
|
||||
// ---- 步骤 6：选取/延续主目标，组装输出并缓存（§7 步骤 6 / §6.3） ----
|
||||
TrackResult result = SelectPrimary(frame_shape);
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(last_result_mutex_);
|
||||
last_result_ = result; // 缓存供跨线程读取（§9.5）
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
TrackResult TargetTracker::LastResult() const {
|
||||
// §9.5：LastResult() 返回受互斥锁保护的拷贝
|
||||
std::lock_guard<std::mutex> lock(last_result_mutex_);
|
||||
return last_result_;
|
||||
}
|
||||
|
||||
void TargetTracker::Reset() {
|
||||
// §2.1：清空所有轨道、ID 计数器、锁定状态与缓存结果
|
||||
tracks_.clear();
|
||||
next_id_ = 0; // §6.2：reset() 时计数器归零
|
||||
frame_count_ = 0;
|
||||
locked_primary_id_.reset(); // 锁定的唯一解除途径之二（§6.3）
|
||||
std::lock_guard<std::mutex> lock(last_result_mutex_);
|
||||
last_result_ = TrackResult{};
|
||||
}
|
||||
|
||||
int TargetTracker::ActiveCount() const {
|
||||
return static_cast<int>(tracks_.size());
|
||||
}
|
||||
|
||||
std::unordered_map<int, Tracklet> TargetTracker::ActiveTracks() const {
|
||||
return tracks_; // 值拷贝只读快照（§2.1）
|
||||
}
|
||||
|
||||
TargetTracker::AssociationResult TargetTracker::AssociateDetections(
|
||||
const std::vector<Detection>& detections) {
|
||||
AssociationResult result;
|
||||
|
||||
// §5.3：按轨道 confidence 降序排序（复制 ID 到 vector 后 sort，
|
||||
// 不直接排哈希表，§9.2）
|
||||
std::vector<int> sorted_ids;
|
||||
sorted_ids.reserve(tracks_.size());
|
||||
for (const auto& kv : tracks_) {
|
||||
sorted_ids.push_back(kv.first);
|
||||
}
|
||||
std::sort(sorted_ids.begin(), sorted_ids.end(), [this](int a, int b) {
|
||||
const double raw_a = tracks_.at(a).confidence;
|
||||
const double raw_b = tracks_.at(b).confidence;
|
||||
// NaN 安全：非有限置信度按 −∞ 处理（排最后），避免 NaN 参与比较
|
||||
// 破坏 std::sort 要求的严格弱序（未定义行为）；映射后为全序，
|
||||
// 等价传递性恒成立
|
||||
const double ca = std::isfinite(raw_a)
|
||||
? raw_a
|
||||
: -std::numeric_limits<double>::infinity();
|
||||
const double cb = std::isfinite(raw_b)
|
||||
? raw_b
|
||||
: -std::numeric_limits<double>::infinity();
|
||||
if (ca != cb) return ca > cb; // 置信度从高到低
|
||||
return a < b; // 平局按 ID 升序作次级键，保证确定性
|
||||
});
|
||||
|
||||
std::vector<bool> det_assigned(detections.size(), false);
|
||||
|
||||
for (const int track_id : sorted_ids) {
|
||||
const Tracklet& track = tracks_.at(track_id);
|
||||
// §5.3：每条轨道最优代价初始化为 1.0（仅 cost 严格小于 1.0 才记录，
|
||||
// 即 cost ≥ 1.0 判为不可匹配，§5.1 匹配有效性判据）
|
||||
double best_cost = 1.0;
|
||||
int best_det = -1;
|
||||
for (size_t d = 0; d < detections.size(); ++d) {
|
||||
if (det_assigned[d]) continue; // 跳过已分配检测
|
||||
const Detection& det = detections[d];
|
||||
// §5.1：中心点欧氏距离（像素）
|
||||
const double det_cx = (det.x1 + det.x2) / 2.0;
|
||||
const double det_cy = (det.y1 + det.y2) / 2.0;
|
||||
const double dist =
|
||||
std::hypot(track.last_center.first - det_cx,
|
||||
track.last_center.second - det_cy);
|
||||
// §5.1：归一化距离，取 max(Dmax, 1e-3) 防除零
|
||||
const double norm_dist =
|
||||
dist / std::max(config_.max_association_dist, 1e-3);
|
||||
// §5.2：预测框与检测框的 IoU
|
||||
const double iou = track.last_box.has_value()
|
||||
? ComputeIoU(*track.last_box,
|
||||
{det.x1, det.y1, det.x2, det.y2})
|
||||
: 0.0;
|
||||
// §5.1：cost = w_dist·norm_dist + w_iou·(1−IoU)
|
||||
const double cost = config_.dist_weight * norm_dist +
|
||||
config_.iou_weight * (1.0 - iou);
|
||||
if (cost < best_cost) { // 严格小于才更新（§5.1 / §5.3）
|
||||
best_cost = cost;
|
||||
best_det = static_cast<int>(d);
|
||||
}
|
||||
}
|
||||
if (best_det >= 0) {
|
||||
result.matches.emplace_back(track_id, best_det);
|
||||
det_assigned[static_cast<size_t>(best_det)] = true;
|
||||
} else {
|
||||
result.unmatched_track_ids.push_back(track_id);
|
||||
}
|
||||
}
|
||||
|
||||
// §5.3：未分配检测收集
|
||||
for (size_t d = 0; d < detections.size(); ++d) {
|
||||
if (!det_assigned[d]) {
|
||||
result.unmatched_det_indices.push_back(static_cast<int>(d));
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
TrackResult TargetTracker::SelectPrimary(std::optional<FrameShape> frame_shape) {
|
||||
TrackResult result;
|
||||
result.n_active = static_cast<int>(tracks_.size());
|
||||
|
||||
// §6.3：轨道表为空 → 清除锁定，返回 tracked=false
|
||||
if (tracks_.empty()) {
|
||||
locked_primary_id_.reset();
|
||||
return result;
|
||||
}
|
||||
|
||||
// §6.3 ① 锁定延续：锁定轨道仍存活则直接输出（Coast 中照样输出，
|
||||
// is_predicted=true；不被其他更高置信度目标抢占）
|
||||
Tracklet* chosen = nullptr;
|
||||
bool is_predicted = false;
|
||||
if (locked_primary_id_.has_value()) {
|
||||
auto it = tracks_.find(*locked_primary_id_);
|
||||
if (it != tracks_.end()) {
|
||||
chosen = &it->second;
|
||||
is_predicted = it->second.IsCoasting();
|
||||
}
|
||||
}
|
||||
|
||||
// §6.3 ② 重新选举（无锁定 / 锁定目标已死亡被回收）
|
||||
if (chosen == nullptr) {
|
||||
// measured ← 所有 lost_count == 0 的轨道；coasting ← 其余存活轨道
|
||||
std::vector<Tracklet*> measured;
|
||||
std::vector<Tracklet*> coasting;
|
||||
for (auto& kv : tracks_) {
|
||||
if (kv.second.IsCoasting()) {
|
||||
coasting.push_back(&kv.second);
|
||||
} else {
|
||||
measured.push_back(&kv.second);
|
||||
}
|
||||
}
|
||||
|
||||
if (!measured.empty()) {
|
||||
// confirmed ← measured 中 hits ≥ min_hits 者；为空则退回 measured
|
||||
// （min_hits 只影响候选池优先级，不阻止输出，§6.1 注记 1）
|
||||
std::vector<Tracklet*> pool;
|
||||
for (Tracklet* t : measured) {
|
||||
if (t->hits >= config_.min_hits) {
|
||||
pool.push_back(t);
|
||||
}
|
||||
}
|
||||
if (pool.empty()) {
|
||||
pool = measured;
|
||||
}
|
||||
// chosen ← pool 中 confidence 最大者；平局取 unordered_map 遍历序
|
||||
// 中首个最大者，不做确定性承诺（规格 §6.3 未规定平局规则）
|
||||
chosen = pool.front();
|
||||
for (Tracklet* t : pool) {
|
||||
if (t->confidence > chosen->confidence) {
|
||||
chosen = t;
|
||||
}
|
||||
}
|
||||
is_predicted = false;
|
||||
} else if (!coasting.empty()) {
|
||||
// 无实测轨道：选 coasting 中离画面中心最近者（欧氏距离平方排序）
|
||||
// §2.2：frame_shape 缺省按 640×480 画面中心 (320, 240)
|
||||
const double center_x =
|
||||
(frame_shape.has_value() && frame_shape->width > 0)
|
||||
? frame_shape->width / 2.0
|
||||
: 320.0;
|
||||
const double center_y =
|
||||
(frame_shape.has_value() && frame_shape->height > 0)
|
||||
? frame_shape->height / 2.0
|
||||
: 240.0;
|
||||
double best_dist_sq = std::numeric_limits<double>::infinity();
|
||||
for (Tracklet* t : coasting) {
|
||||
const double dx = t->last_center.first - center_x;
|
||||
const double dy = t->last_center.second - center_y;
|
||||
const double dist_sq = dx * dx + dy * dy;
|
||||
if (dist_sq < best_dist_sq) {
|
||||
best_dist_sq = dist_sq;
|
||||
chosen = t;
|
||||
}
|
||||
}
|
||||
if (chosen == nullptr) {
|
||||
// NaN 残余的防御兜底：若所有 coasting 轨道中心均为 NaN，
|
||||
// dist_sq 恒为 NaN 无人当选，取首个避免空指针解引用；
|
||||
// 正常输入已在 Update 入口经有限性过滤，本分支理论上不可达
|
||||
chosen = coasting.front();
|
||||
}
|
||||
is_predicted = true; // Coast 兜底：仅预测、无实测
|
||||
} else {
|
||||
// 理论上不可达（轨道非空则 measured/coasting 必居其一）；防御清锁
|
||||
locked_primary_id_.reset();
|
||||
return result;
|
||||
}
|
||||
locked_primary_id_ = chosen->track_id; // 选举后锁定（§6.3）
|
||||
}
|
||||
|
||||
// ---- 组装输出（§2.3 九字段） ----
|
||||
result.tracked = true;
|
||||
result.primary_id = chosen->track_id;
|
||||
result.center = chosen->last_center;
|
||||
result.box = chosen->last_box;
|
||||
result.confidence = chosen->confidence; // Coast 期间继承最近实测置信度
|
||||
result.is_predicted = is_predicted;
|
||||
result.lost_frames = chosen->lost_count;
|
||||
// §2.3：raw = 关联到的原始检测框；Coast 时为空（锁定延续分支同样遵守）
|
||||
result.raw = is_predicted ? std::nullopt : chosen->last_raw_box;
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace tracker
|
||||
} // namespace drone
|
||||
@@ -0,0 +1,101 @@
|
||||
// =============================================================================
|
||||
// tracklet.cpp —— 轨道（Tracklet）与基础几何工具实现
|
||||
//
|
||||
// 对应规格：docs/视觉追踪技术路线.md
|
||||
// §5.2 IoU 计算
|
||||
// §7 TRACKLET_PREDICT / TRACKLET_UPDATE 伪代码
|
||||
// §3.2 Tracklet 字段语义
|
||||
// =============================================================================
|
||||
|
||||
#include "target_tracker/tracklet.h"
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
namespace drone {
|
||||
namespace tracker {
|
||||
|
||||
BoxCXCYWH ToCenterSize(const BoxXYXY& box) {
|
||||
// §7 TRACKLET_UPDATE：cx=(x1+x2)/2；cy=(y1+y2)/2；w=x2−x1；h=y2−y1
|
||||
return BoxCXCYWH{(box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0,
|
||||
box.x2 - box.x1, box.y2 - box.y1};
|
||||
}
|
||||
|
||||
BoxXYXY ToXYXY(const BoxCXCYWH& box) {
|
||||
// §7 TRACKLET_PREDICT：center_size → [cx−w/2, cy−h/2, cx+w/2, cy+h/2]
|
||||
return BoxXYXY{box.cx - box.w / 2.0, box.cy - box.h / 2.0,
|
||||
box.cx + box.w / 2.0, box.cy + box.h / 2.0};
|
||||
}
|
||||
|
||||
double ComputeIoU(const BoxXYXY& a, const BoxXYXY& b) {
|
||||
// §5.2：任一框为空（宽或高 ≤ 0）返回 0
|
||||
const double area_a = (a.x2 - a.x1) * (a.y2 - a.y1);
|
||||
const double area_b = (b.x2 - b.x1) * (b.y2 - b.y1);
|
||||
if (area_a <= 0.0 || area_b <= 0.0) {
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
// 交集宽高与 0 取最大值（§5.2）
|
||||
const double inter_w = std::min(a.x2, b.x2) - std::max(a.x1, b.x1);
|
||||
const double inter_h = std::min(a.y2, b.y2) - std::max(a.y1, b.y1);
|
||||
if (inter_w <= 0.0 || inter_h <= 0.0) {
|
||||
return 0.0; // 交集面积 ≤ 0 返回 0（§5.2）
|
||||
}
|
||||
|
||||
const double inter_area = inter_w * inter_h;
|
||||
const double union_area = area_a + area_b - inter_area;
|
||||
if (union_area <= 0.0) {
|
||||
return 0.0; // 并集 ≤ 0 返回 0（§5.2）
|
||||
}
|
||||
// IoU = area(A∩B) / (area(A)+area(B)−area(A∩B))（§5.2）
|
||||
return inter_area / union_area;
|
||||
}
|
||||
|
||||
Tracklet::Tracklet(int id, const BoxCXCYWH& measurement, double conf, int cls,
|
||||
int max_lost_frames)
|
||||
: track_id(id),
|
||||
max_lost(max_lost_frames),
|
||||
confidence(conf),
|
||||
cls_id(cls) {
|
||||
// §4.3：用首次观测初始化滤波器（速度置零、P 取默认对角）
|
||||
kf.Initiate(measurement);
|
||||
// §7 步骤 5：创建当帧即赋 last_center / last_box（保证当帧可参与输出）
|
||||
last_center = {measurement.cx, measurement.cy};
|
||||
last_box = ToXYXY(measurement);
|
||||
// 创建即源于实测检测：同步记录原始检测框（Coast 期间保留，
|
||||
// 对外输出按 §2.3 在 Coast 时置空）
|
||||
last_raw_box = last_box;
|
||||
// age / hits / lost_count 由类内默认值保证（1 / 1 / 0，§3.2）
|
||||
}
|
||||
|
||||
void Tracklet::Predict() {
|
||||
// §7 TRACKLET_PREDICT：卡尔曼预测推进一帧
|
||||
const auto pred = kf.Predict();
|
||||
if (pred.has_value()) {
|
||||
// 预测成功：更新 last_center / last_box，age += 1
|
||||
last_center = {pred->cx, pred->cy};
|
||||
last_box = ToXYXY(*pred);
|
||||
++age;
|
||||
}
|
||||
}
|
||||
|
||||
void Tracklet::Update(const BoxXYXY& box, double conf, int cls) {
|
||||
// §7 TRACKLET_UPDATE：框 → [cx,cy,w,h] → kf.update
|
||||
kf.Update(ToCenterSize(box));
|
||||
last_center = {(box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0};
|
||||
last_box = box;
|
||||
last_raw_box = box; // 实测关联到的原始检测框（Coast 期间保留不更新）
|
||||
confidence = conf;
|
||||
cls_id = cls;
|
||||
lost_count = 0; // 匹配成功：连续丢失计数清零
|
||||
++hits; // 累计匹配次数 +1
|
||||
++age; // 帧龄 +1
|
||||
}
|
||||
|
||||
void Tracklet::MarkMiss() {
|
||||
// §7 步骤 4：未匹配计丢失 lost_count+1、age+1
|
||||
++lost_count;
|
||||
++age;
|
||||
}
|
||||
|
||||
} // namespace tracker
|
||||
} // namespace drone
|
||||
@@ -0,0 +1,217 @@
|
||||
# target_tracker 视觉追踪库实现文档
|
||||
|
||||
> 独立动态库（`libtarget_tracker.so`），实现轻量级多目标追踪过滤器：
|
||||
> 恒定速度卡尔曼滤波平滑 + Coast 抗短暂丢失 + ID 稳定与主目标锁定。
|
||||
> 算法行为的唯一权威规格为 `docs/视觉追踪技术路线.md`（下称"技术路线"），
|
||||
> 本文档不重复抄录算法全文，仅描述实现结构与对照关系。
|
||||
|
||||
---
|
||||
|
||||
## 一、功能职责与边界
|
||||
|
||||
**做什么**：
|
||||
|
||||
- 每帧接收 YOLO 检测框列表，输出**一个主目标**的平滑中心点/边界框/状态标志
|
||||
（`TrackResult`），供下游速度闭环（PID）消费。
|
||||
- 平滑：8 维恒定速度卡尔曼滤波（位置/尺寸 + 速度），抑制检测框逐帧抖动。
|
||||
- Coast 预测：目标短暂消失时用滤波器预测值继续输出（`is_predicted=true`），
|
||||
避免控制输出突变；连续丢失超过 `max_lost_frames`（严格大于）才判丢。
|
||||
- ID 稳定：贪婪最邻近关联（中心距离 + IoU 加权代价）维持轨道 ID；
|
||||
主目标锁定后不被高置信度新目标抢占，仅在轨道死亡或 `Reset()` 时解除。
|
||||
|
||||
**不做什么**（边界）：
|
||||
|
||||
- 不做光流估计、不做激光/惯导等多源融合（属 perception 模块职责）；
|
||||
- 不做 Topic 订阅/发布接线、不依赖 spdlog/Eigen/OpenCV，零第三方依赖；
|
||||
- 不做画面边界钳制与负尺寸约束（与技术路线 §4.5 注记保持一致）；
|
||||
- 不要求检测置信度下限过滤（上游检测器已完成阈值过滤）。
|
||||
|
||||
## 二、接口与数据流
|
||||
|
||||
命名空间 `drone::tracker`，头文件保护 `#pragma once`。
|
||||
|
||||
| 头文件 | 内容 |
|
||||
|--------|------|
|
||||
| `include/target_tracker/kalman_box_filter.h` | `BoxCXCYWH`、`KalmanBoxFilter` |
|
||||
| `include/target_tracker/tracklet.h` | `BoxXYXY`、`ToCenterSize`/`ToXYXY`/`ComputeIoU`、`Tracklet` |
|
||||
| `include/target_tracker/target_tracker.h` | `Detection`、`FrameShape`、`TrackResult`、`TargetTracker` |
|
||||
|
||||
主要接口签名：
|
||||
|
||||
```cpp
|
||||
class KalmanBoxFilter {
|
||||
void Initiate(const BoxCXCYWH& z); // 首次观测初始化（§4.3）
|
||||
std::optional<BoxCXCYWH> Predict(); // 未初始化返回 nullopt（§4.4）
|
||||
void Update(const BoxCXCYWH& z); // 观测更新（§4.5）
|
||||
bool Initialized() const;
|
||||
const std::array<double, 8>& State() const; // [cx,cy,w,h,vx,vy,vw,vh]
|
||||
};
|
||||
|
||||
struct Tracklet {
|
||||
Tracklet(int id, const BoxCXCYWH& measurement, double conf, int cls,
|
||||
int max_lost_frames);
|
||||
bool IsDead() const; // lost_count > max_lost（严格大于）
|
||||
bool IsCoasting() const; // lost_count > 0
|
||||
void Predict(); // kf.predict 非空时更新 last_center/last_box、age+1
|
||||
void Update(const BoxXYXY& box, double conf, int cls); // 匹配更新
|
||||
void MarkMiss(); // lost+1、age+1
|
||||
// 字段：track_id / kf / lost_count / max_lost / confidence / cls_id /
|
||||
// age / hits / last_center / last_box / last_raw_box
|
||||
};
|
||||
|
||||
class TargetTracker {
|
||||
struct Config { // §8.1 默认值
|
||||
int max_lost_frames = 8;
|
||||
double max_association_dist = 200.0;
|
||||
int min_hits = 3;
|
||||
double dist_weight = 0.5;
|
||||
double iou_weight = 0.5;
|
||||
};
|
||||
TargetTracker(); // 默认配置
|
||||
explicit TargetTracker(const Config& cfg);
|
||||
TrackResult Update(const std::vector<Detection>& detections,
|
||||
std::optional<FrameShape> frame_shape = std::nullopt);
|
||||
TrackResult LastResult() const; // 线程安全拷贝
|
||||
void Reset(); // 清全部状态且 next_id 归零
|
||||
int ActiveCount() const;
|
||||
std::unordered_map<int, Tracklet> ActiveTracks() const; // 只读快照
|
||||
};
|
||||
```
|
||||
|
||||
数据流：调用方每帧将检测列表（像素坐标 `[x1,y1,x2,y2,conf,cls_id]`）传入
|
||||
`Update()`，得到 `TrackResult`（九字段：tracked / primary_id / center / box /
|
||||
confidence / is_predicted / lost_frames / n_active / raw）。丢失时
|
||||
`tracked=false`、定位字段空、`lost_frames=0`（技术路线 §2.3）。
|
||||
|
||||
## 三、关键实现点
|
||||
|
||||
### 3.1 卡尔曼滤波（对照技术路线 §4）
|
||||
|
||||
- 状态 `x[8]=[cx,cy,w,h,vx,vy,vw,vh]`，观测 `z[4]=[cx,cy,w,h]`，dt=1 帧。
|
||||
- 常量矩阵：`F`（8×8，I4 + 右上 I4）、`H`（4×8，[I4|0]）为 `constexpr`；
|
||||
矩阵乘法/转置用 `std::array` 定长手写，全程 `double`（§9.3）。
|
||||
- 常量：`kStdPos=1/50`、`kStdVel=1/200`、`kIniPos=2.0`、`kIniVel=100.0`、
|
||||
`kRScale=2.0`（§4.2/§8.2）。
|
||||
- **动态噪声**：`Predict()` 时 Q=diag(σ_pos²,σ_vel²)，σ 直接用状态中的 w/h
|
||||
缩放；`Update()` 时 R 用 `abs(w)/abs(h)` 缩放并乘 2——两处差异与规格
|
||||
§4.2 注记严格保持一致。
|
||||
- `Initiate()`：速度置零，P=diag(2,2,2,2,100,100,100,100)（§4.3）。
|
||||
- `Update()` 中 S=HPHᵀ+R 为 4×4，用**高斯-约当消元 + 部分主元**求逆，
|
||||
消元前对角加 1e-9 正则化防奇异（§9.3）；随后 K=PHᵀS⁻¹、
|
||||
x←x+K(z−Hx)、P←(I−KH)P（§4.5）。
|
||||
|
||||
### 3.2 贪婪关联（对照 §5）
|
||||
|
||||
- `cost = dist_weight·dist/max(Dmax,1e-3) + iou_weight·(1−IoU)`，
|
||||
距离用 `std::hypot`；每条轨道 `best_cost` 初始化为 1.0，**仅 cost 严格
|
||||
小于才匹配**（cost=1.0 恰好不匹配，§5.1）。
|
||||
- 按轨道 confidence 降序（平局按 ID 升序保证确定性）逐一贪心匹配；
|
||||
排序时复制 ID 到 `std::vector` 后 `std::sort`，不直接排哈希表（§9.2）。
|
||||
|
||||
### 3.3 主目标选举三级回退（对照 §6.3）
|
||||
|
||||
1. **锁定延续优先**：`locked_primary_id` 存活则直接输出（Coast 中照样输出
|
||||
`is_predicted=true`），不被高置信度目标抢占；
|
||||
2. **重新选举**（无锁定/锁定已死亡）：measured（lost==0）中 hits≥min_hits
|
||||
的已确认池取 confidence 最大；池空则退回全部 measured；
|
||||
3. 无 measured 时从 coasting 中选离画面中心最近者（欧氏距离平方，
|
||||
`frame_shape` 缺省按 640×480 中心 (320,240)），`is_predicted=true`；
|
||||
都没有则 `tracked=false`。
|
||||
|
||||
锁定仅在轨道死亡删除处或 `Reset()` 时解除。
|
||||
|
||||
### 3.4 主流程六步（对照 §7）
|
||||
|
||||
`Update()` 严格按：①全体轨道 predict ②检测中心表 + 贪婪关联 ③匹配轨道
|
||||
update（raw_box 由检测重建）④未匹配轨道 mark_miss、死亡即删、若为锁定
|
||||
目标同时清锁 ⑤未匹配检测新建轨道（创建当帧赋 last_box/last_raw_box，
|
||||
保证当帧可参与输出；next_id 从 0 递增永不复用）⑥主目标选举组装输出并
|
||||
缓存 last_result。
|
||||
|
||||
### 3.5 线程模型（对照 §9.5）
|
||||
|
||||
单线程写 `Update()`（推理主循环）；库内仅对 `last_result` 缓存加一把
|
||||
`std::mutex`，`LastResult()` 返回受保护拷贝。内部轨道状态不加锁；
|
||||
若未来改为多线程调用 `Update()` 需整体加锁。
|
||||
|
||||
### 3.6 符号导出说明
|
||||
|
||||
当前不加 `-fvisibility=hidden`、不使用导出宏，共享库默认导出全部公共符号。
|
||||
日后若启用隐藏可见性（如 `CXX_VISIBILITY_PRESET hidden`），需补
|
||||
`TARGET_TRACKER_API` 宏（导出/导入双态）并标注所有公共类与函数，
|
||||
否则下游链接将缺失符号。
|
||||
|
||||
## 四、日志行为
|
||||
|
||||
本库为**纯算法层，不打任何日志**（不依赖 spdlog）。若未来接入主工程，
|
||||
接线层（订阅/发布适配部件）按主工程日志纪律负责关键路径日志与节流，
|
||||
本库内部不新增日志。
|
||||
|
||||
## 五、测试方式
|
||||
|
||||
构建环境：WSL2 Ubuntu（AGENTS.md 规定的开发机构建环境）。
|
||||
|
||||
```bash
|
||||
cd /mnt/d/ProgramData/drone_test/drone_test
|
||||
cmake -S target_tracker -B target_tracker/build
|
||||
cmake --build target_tracker/build -j$(nproc)
|
||||
ctest --test-dir target_tracker/build --output-on-failure
|
||||
```
|
||||
|
||||
测试共 27 条（kalman_box_filter_test 5 条 / tracklet_test 12 条 /
|
||||
target_tracker_test 10 条），其中 `target_tracker_test.cpp` 逐条覆盖
|
||||
技术路线 §9.4 六条验收用例：
|
||||
|
||||
1. `EmptyFramesCoastThenLost`：空检测帧 Coast 持续 max_lost_frames 帧
|
||||
（is_predicted=true），第 max_lost_frames+1 帧 tracked=false；
|
||||
2. `IdStrictlyIncreasingNoReuse`：ID 严格递增不复用，Reset 后归零；
|
||||
3. `LockNotStolenAndSwitchAfterDeath`：Coast 期间高置信度新目标不抢占锁定，
|
||||
锁定目标死亡后切换；
|
||||
4. `MinHitsFallbackAllowsNewTrack`：无已确认实测轨道时新轨道（hits<min_hits）
|
||||
可当选主目标；
|
||||
5. `CostExactlyOneDoesNotMatch`：纯距离项构造 cost=1.0 恰好不匹配，
|
||||
并附 cost<1.0 对照帧；
|
||||
6. `HigherConfidenceTrackWinsDetection`：两轨道竞争同一检测，置信度高者获胜。
|
||||
|
||||
其余为补充行为测试（丢失输出字段约定、Coast 兜底选举、锁定延续、
|
||||
默认画面中心、IoU 边界、滤波器收敛性等）。
|
||||
|
||||
## 六、排查 / 修改要点
|
||||
|
||||
- **主目标"甩头"**：确认是否误改锁定解除逻辑——锁定只能在轨道死亡删除处
|
||||
或 `Reset()` 解除；选举分支不得覆盖存活锁定。
|
||||
- **Coast 时间不对**：死亡判定为 `lost_count > max_lost`（严格大于），
|
||||
默认允许 Coast 8 帧、第 9 帧判丢；改成 `>=` 属于行为变更。
|
||||
- **关联不上 / 乱匹配**：检查 `max_association_dist` 与目标帧间位移是否
|
||||
匹配；cost 判据为严格小于 1.0；dist/iou 权重之和建议保持 1.0。
|
||||
- **滤波发散或跳变**：检查 Q/R 动态噪声是否按状态 w/h 缩放（R 用 abs 且
|
||||
乘 2、Q 不用 abs，该差异是规格要求）；4×4 求逆已加 1e-9 正则化。
|
||||
- **测试链接失败（找不到 so）**：测试可执行文件通过 `CMAKE_BUILD_RPATH`
|
||||
定位库；若把测试挪到别处运行需设置 `LD_LIBRARY_PATH`。
|
||||
- 修改任何行为后必须同步更新本文档与 `docs/视觉追踪技术路线.md` 的对照注释。
|
||||
|
||||
## 七、未来接线指引（接入主工程视频链路）
|
||||
|
||||
本库以独立动态库交付，主工程接入建议采用：
|
||||
|
||||
1. **订阅解码帧主题作为每帧节拍**：接线部件订阅解码帧 Topic，以解码帧
|
||||
到达为帧节拍驱动追踪（保证与推理节奏一致）；
|
||||
2. **按 frame_sequence 聚合 DetectionResult**：YOLO 每目标发布一条
|
||||
DetectionResult（同帧共享 frame_sequence），接线部件按 frame_sequence
|
||||
聚合成 `std::vector<Detection>` 后调用 `Update()`；
|
||||
3. **主工程 `include/common/types.h` 新增 `TrackedTarget` 消息与
|
||||
`kTrackedTarget` 主题**：由接线部件把 `TrackResult` 转换为
|
||||
`TrackedTarget` 发布，供控制线程（20Hz PID）订阅消费；Coast
|
||||
（is_predicted=true）时调用方清 PID 积分、tracked=false 时零速刹车
|
||||
（技术路线 §1.2 上游行为约定）。
|
||||
|
||||
主工程链接该 so 的 CMake 片段示例：
|
||||
|
||||
```cmake
|
||||
# 主工程 CMakeLists.txt 中引入视觉追踪库（独立子工程）
|
||||
add_subdirectory(target_tracker)
|
||||
|
||||
target_link_libraries(drone_control PRIVATE target_tracker)
|
||||
# 部署时需将 libtarget_tracker.so.1 一并打包到可执行文件目录
|
||||
# （或设置 INSTALL_RPATH=$ORIGIN，与 docs/部署与打包.md 目录式部署约定一致）
|
||||
```
|
||||
@@ -0,0 +1,124 @@
|
||||
// =============================================================================
|
||||
// kalman_box_filter_test.cpp —— KalmanBoxFilter 单元测试
|
||||
//
|
||||
// 覆盖 docs/视觉追踪技术路线.md §4 行为：
|
||||
// - 初始化状态正确（§4.3）
|
||||
// - 静止目标多帧收敛到观测（§4.5）
|
||||
// - 匀速目标速度收敛且预测超前一个 dt（§4.4）
|
||||
// - 未初始化 predict 返回 nullopt（§4.4）
|
||||
// =============================================================================
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
|
||||
#include "target_tracker/kalman_box_filter.h"
|
||||
|
||||
using drone::tracker::BoxCXCYWH;
|
||||
using drone::tracker::KalmanBoxFilter;
|
||||
|
||||
namespace {
|
||||
|
||||
// 未初始化时 predict 返回 nullopt（§4.4）；update 直接返回不改变状态（§4.5）
|
||||
TEST(KalmanBoxFilterTest, UninitializedPredictReturnsNullopt) {
|
||||
KalmanBoxFilter kf;
|
||||
EXPECT_FALSE(kf.Initialized());
|
||||
EXPECT_FALSE(kf.Predict().has_value());
|
||||
|
||||
// 未初始化时 update 无副作用
|
||||
kf.Update({100.0, 100.0, 20.0, 20.0});
|
||||
EXPECT_FALSE(kf.Initialized());
|
||||
EXPECT_FALSE(kf.Predict().has_value());
|
||||
for (const double v : kf.State()) {
|
||||
EXPECT_DOUBLE_EQ(v, 0.0);
|
||||
}
|
||||
}
|
||||
|
||||
// 初始化状态正确（§4.3）：速度置零、位置/尺寸取首次观测
|
||||
TEST(KalmanBoxFilterTest, InitiateSetsStateCorrectly) {
|
||||
KalmanBoxFilter kf;
|
||||
kf.Initiate({320.0, 240.0, 40.0, 30.0});
|
||||
EXPECT_TRUE(kf.Initialized());
|
||||
|
||||
const std::array<double, 8> expected = {
|
||||
{320.0, 240.0, 40.0, 30.0, 0.0, 0.0, 0.0, 0.0}};
|
||||
for (size_t i = 0; i < expected.size(); ++i) {
|
||||
EXPECT_DOUBLE_EQ(kf.State()[i], expected[i]) << "index=" << i;
|
||||
}
|
||||
|
||||
// 初始化后 predict 返回状态前 4 维副本（速度为零 → 位置不变，§4.4）
|
||||
const auto pred = kf.Predict();
|
||||
ASSERT_TRUE(pred.has_value());
|
||||
EXPECT_NEAR(pred->cx, 320.0, 1e-9);
|
||||
EXPECT_NEAR(pred->cy, 240.0, 1e-9);
|
||||
EXPECT_NEAR(pred->w, 40.0, 1e-9);
|
||||
EXPECT_NEAR(pred->h, 30.0, 1e-9);
|
||||
}
|
||||
|
||||
// 静止目标：多帧重复观测同一框，滤波输出应收敛到观测值（§4.5）
|
||||
TEST(KalmanBoxFilterTest, StaticTargetConvergesToObservation) {
|
||||
KalmanBoxFilter kf;
|
||||
const BoxCXCYWH z{100.0, 150.0, 40.0, 60.0};
|
||||
kf.Initiate({104.0, 156.0, 42.0, 62.0}); // 带初始偏差，验证收敛能力
|
||||
|
||||
for (int i = 0; i < 50; ++i) {
|
||||
kf.Predict();
|
||||
kf.Update(z);
|
||||
}
|
||||
|
||||
EXPECT_NEAR(kf.State()[0], z.cx, 0.5); // cx
|
||||
EXPECT_NEAR(kf.State()[1], z.cy, 0.5); // cy
|
||||
EXPECT_NEAR(kf.State()[2], z.w, 0.5); // w
|
||||
EXPECT_NEAR(kf.State()[3], z.h, 0.5); // h
|
||||
// 静止目标速度应收敛到约 0
|
||||
EXPECT_NEAR(kf.State()[4], 0.0, 0.1);
|
||||
EXPECT_NEAR(kf.State()[5], 0.0, 0.1);
|
||||
}
|
||||
|
||||
// 匀速目标：速度估计收敛到真实速度，且 predict 超前一个 dt（§4.4）
|
||||
TEST(KalmanBoxFilterTest, ConstantVelocityConvergesAndPredictLeadsOneDt) {
|
||||
KalmanBoxFilter kf;
|
||||
const double vx = 3.0; // 像素/帧
|
||||
const double vy = 2.0; // 像素/帧
|
||||
|
||||
// 目标从 (100, 100) 出发匀速运动，每帧观测一次（predict → update）
|
||||
BoxCXCYWH z{100.0, 100.0, 30.0, 30.0};
|
||||
kf.Initiate(z);
|
||||
for (int i = 0; i < 60; ++i) {
|
||||
kf.Predict();
|
||||
z.cx += vx;
|
||||
z.cy += vy;
|
||||
kf.Update(z);
|
||||
}
|
||||
|
||||
// 速度收敛到真实速度（像素/帧）
|
||||
EXPECT_NEAR(kf.State()[4], vx, 0.3) << "vx 未收敛";
|
||||
EXPECT_NEAR(kf.State()[5], vy, 0.3) << "vy 未收敛";
|
||||
// 位置贴合当前观测
|
||||
EXPECT_NEAR(kf.State()[0], z.cx, 1.5);
|
||||
EXPECT_NEAR(kf.State()[1], z.cy, 1.5);
|
||||
|
||||
// 再预测一帧：应超前当前观测一个 dt（≈ 速度 × 1 帧）
|
||||
const auto pred = kf.Predict();
|
||||
ASSERT_TRUE(pred.has_value());
|
||||
EXPECT_NEAR(pred->cx, z.cx + vx, 1.0) << "预测未超前一个 dt";
|
||||
EXPECT_NEAR(pred->cy, z.cy + vy, 1.0) << "预测未超前一个 dt";
|
||||
}
|
||||
|
||||
// 动态噪声不退化：小尺寸目标多帧观测仍稳定收敛（§4.2 噪声与尺寸成比例）
|
||||
TEST(KalmanBoxFilterTest, SmallTargetNoiseScalingStable) {
|
||||
KalmanBoxFilter kf;
|
||||
const BoxCXCYWH z{50.0, 60.0, 4.0, 4.0}; // 小目标（4×4 像素）
|
||||
kf.Initiate(z);
|
||||
for (int i = 0; i < 30; ++i) {
|
||||
kf.Predict();
|
||||
kf.Update(z);
|
||||
}
|
||||
EXPECT_NEAR(kf.State()[0], z.cx, 0.5);
|
||||
EXPECT_NEAR(kf.State()[1], z.cy, 0.5);
|
||||
EXPECT_TRUE(std::isfinite(kf.State()[2]));
|
||||
EXPECT_TRUE(std::isfinite(kf.State()[3]));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,402 @@
|
||||
// =============================================================================
|
||||
// target_tracker_test.cpp —— TargetTracker 单元测试
|
||||
//
|
||||
// 逐条覆盖 docs/视觉追踪技术路线.md §9.4 行为一致性验收用例（硬验收标准）：
|
||||
// 1. 空检测帧序列：Coast 持续 max_lost_frames 帧，第 max_lost_frames+1 帧判丢
|
||||
// 2. ID 严格递增不复用（含 Reset 后归零）
|
||||
// 3. 锁定不被抢占；锁定目标死亡后切换到新目标
|
||||
// 4. min_hits 回退：无已确认实测轨道时新轨道也可当选主目标
|
||||
// 5. cost=1.0 恰好不构成匹配（严格小于）
|
||||
// 6. 置信度优先：两轨道竞争同一检测，置信度高者获胜
|
||||
// =============================================================================
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <optional>
|
||||
#include <vector>
|
||||
|
||||
#include "target_tracker/target_tracker.h"
|
||||
|
||||
using drone::tracker::Detection;
|
||||
using drone::tracker::FrameShape;
|
||||
using drone::tracker::TargetTracker;
|
||||
using drone::tracker::TrackResult;
|
||||
|
||||
namespace {
|
||||
|
||||
Detection MakeDet(double x1, double y1, double x2, double y2, double conf,
|
||||
int cls = 0) {
|
||||
return Detection{x1, y1, x2, y2, conf, cls};
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// 验收用例 1（§9.4-1）：空检测帧序列
|
||||
// 连续输入空检测，主目标输出 is_predicted=true 共 max_lost_frames 帧，
|
||||
// 第 max_lost_frames + 1 帧 tracked=false（死亡判定 lost_count > max_lost）。
|
||||
// ============================================================================
|
||||
TEST(TargetTrackerAcceptanceTest, EmptyFramesCoastThenLost) {
|
||||
TargetTracker tracker; // 默认配置 max_lost_frames = 8
|
||||
ASSERT_EQ(tracker.GetConfig().max_lost_frames, 8);
|
||||
|
||||
// 第 1 帧：给出一个检测，建立轨道并锁定为主目标
|
||||
TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 140.0, 140.0, 0.9)});
|
||||
ASSERT_TRUE(r.tracked);
|
||||
ASSERT_TRUE(r.primary_id.has_value());
|
||||
EXPECT_FALSE(r.is_predicted); // 当帧有实测
|
||||
EXPECT_EQ(r.lost_frames, 0);
|
||||
EXPECT_EQ(r.n_active, 1);
|
||||
ASSERT_TRUE(r.box.has_value());
|
||||
ASSERT_TRUE(r.raw.has_value()); // 当帧关联到原始检测框
|
||||
|
||||
// 后续空检测帧：应持续 Coast 共 max_lost_frames 帧（is_predicted=true）
|
||||
for (int i = 1; i <= 8; ++i) {
|
||||
r = tracker.Update({});
|
||||
ASSERT_TRUE(r.tracked) << "第 " << i << " 帧 Coast 期间不应判丢";
|
||||
EXPECT_TRUE(r.is_predicted) << "第 " << i << " 帧应为纯预测输出";
|
||||
EXPECT_EQ(r.lost_frames, i);
|
||||
EXPECT_TRUE(r.primary_id.has_value());
|
||||
EXPECT_TRUE(r.box.has_value()); // Coast 期间继续输出预测框
|
||||
EXPECT_FALSE(r.raw.has_value()); // Coast 时 raw 为空（§2.3）
|
||||
EXPECT_DOUBLE_EQ(r.confidence, 0.9); // 继承最近实测置信度（§2.3）
|
||||
}
|
||||
|
||||
// 第 max_lost_frames + 1 帧：轨道死亡回收 → tracked=false
|
||||
r = tracker.Update({});
|
||||
EXPECT_FALSE(r.tracked);
|
||||
EXPECT_FALSE(r.primary_id.has_value());
|
||||
EXPECT_FALSE(r.box.has_value());
|
||||
EXPECT_EQ(r.lost_frames, 0); // 丢失时 lost_frames=0（§2.3）
|
||||
EXPECT_EQ(r.n_active, 0);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// 验收用例 2（§9.4-2）：ID 单调性
|
||||
// 多次进出画面的目标获得严格递增、不复用的 ID；Reset 后计数器归零。
|
||||
// ============================================================================
|
||||
TEST(TargetTrackerAcceptanceTest, IdStrictlyIncreasingNoReuse) {
|
||||
TargetTracker::Config cfg;
|
||||
cfg.max_lost_frames = 1; // 缩短 Coast 窗口，加速轨道死亡
|
||||
TargetTracker tracker(cfg);
|
||||
|
||||
// 目标 A 出现 → id 0
|
||||
TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.9)});
|
||||
ASSERT_TRUE(r.primary_id.has_value());
|
||||
const int id_a = *r.primary_id;
|
||||
EXPECT_EQ(id_a, 0);
|
||||
|
||||
// 目标 A 消失至死亡（max_lost=1：第 2 帧 Coast、第 3 帧死亡回收）
|
||||
tracker.Update({});
|
||||
r = tracker.Update({});
|
||||
EXPECT_FALSE(r.tracked);
|
||||
EXPECT_EQ(tracker.ActiveCount(), 0);
|
||||
|
||||
// 目标 B 出现（同一位置）：ID 不复用，严格递增 → id 1
|
||||
r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.9)});
|
||||
ASSERT_TRUE(r.primary_id.has_value());
|
||||
const int id_b = *r.primary_id;
|
||||
EXPECT_EQ(id_b, 1);
|
||||
EXPECT_GT(id_b, id_a);
|
||||
|
||||
// 目标 C 同帧新建 → id 2（同帧多目标亦严格递增）
|
||||
r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.9),
|
||||
MakeDet(400.0, 300.0, 430.0, 330.0, 0.8)});
|
||||
EXPECT_EQ(tracker.ActiveCount(), 2);
|
||||
int max_id = 0;
|
||||
for (const auto& kv : tracker.ActiveTracks()) {
|
||||
max_id = std::max(max_id, kv.first);
|
||||
}
|
||||
EXPECT_EQ(max_id, 2);
|
||||
|
||||
// Reset 后计数器归零（§6.2）：新轨道重新从 id 0 开始
|
||||
tracker.Reset();
|
||||
EXPECT_EQ(tracker.ActiveCount(), 0);
|
||||
EXPECT_FALSE(tracker.LastResult().tracked);
|
||||
r = tracker.Update({MakeDet(50.0, 50.0, 70.0, 70.0, 0.9)});
|
||||
ASSERT_TRUE(r.primary_id.has_value());
|
||||
EXPECT_EQ(*r.primary_id, 0);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// 验收用例 3（§9.4-3）：锁定不被抢占
|
||||
// 锁定目标 Coast 期间出现更高置信度新目标，primary_id 不变；
|
||||
// 锁定目标死亡后切换到新目标。
|
||||
// ============================================================================
|
||||
TEST(TargetTrackerAcceptanceTest, LockNotStolenAndSwitchAfterDeath) {
|
||||
TargetTracker::Config cfg;
|
||||
cfg.max_lost_frames = 3;
|
||||
TargetTracker tracker(cfg);
|
||||
|
||||
// 低置信度目标 A 建立并锁定
|
||||
TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 140.0, 140.0, 0.4)});
|
||||
ASSERT_TRUE(r.primary_id.has_value());
|
||||
const int id_a = *r.primary_id;
|
||||
|
||||
// A 消失进入 Coast；期间出现更高置信度新目标 B（远离 A，不会被误关联）
|
||||
r = tracker.Update({MakeDet(500.0, 300.0, 540.0, 340.0, 0.95)});
|
||||
ASSERT_TRUE(r.primary_id.has_value());
|
||||
EXPECT_EQ(*r.primary_id, id_a) << "Coast 期间锁定不应被高置信度目标抢占";
|
||||
EXPECT_TRUE(r.is_predicted);
|
||||
EXPECT_EQ(tracker.ActiveCount(), 2); // A（Coast）与 B 并存
|
||||
|
||||
// 继续 Coast，锁定依旧延续
|
||||
r = tracker.Update({MakeDet(500.0, 300.0, 540.0, 340.0, 0.95)});
|
||||
EXPECT_EQ(*r.primary_id, id_a);
|
||||
EXPECT_TRUE(r.is_predicted);
|
||||
|
||||
// A 继续丢失直至死亡（max_lost=3：lost 达 4 帧时回收）
|
||||
for (int i = 0; i < 2; ++i) {
|
||||
r = tracker.Update({MakeDet(500.0, 300.0, 540.0, 340.0, 0.95)});
|
||||
}
|
||||
// A 已死亡删除 → 锁定解除，重新选举切换到 B（此时 B 已确认且实测）
|
||||
ASSERT_TRUE(r.tracked);
|
||||
EXPECT_NE(*r.primary_id, id_a) << "锁定目标死亡后应切换到新目标";
|
||||
EXPECT_FALSE(r.is_predicted);
|
||||
|
||||
// 后续 B 持续作为主目标
|
||||
r = tracker.Update({MakeDet(500.0, 300.0, 540.0, 340.0, 0.95)});
|
||||
ASSERT_TRUE(r.primary_id.has_value());
|
||||
EXPECT_NE(*r.primary_id, id_a);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// 验收用例 4（§9.4-4）：min_hits 回退
|
||||
// 新轨道（hits 未达 min_hits）在无其他已确认实测轨道时也可当选主目标。
|
||||
// ============================================================================
|
||||
TEST(TargetTrackerAcceptanceTest, MinHitsFallbackAllowsNewTrack) {
|
||||
TargetTracker::Config cfg;
|
||||
cfg.min_hits = 5; // 抬高确认门槛，保证测试期间轨道始终未确认
|
||||
TargetTracker tracker(cfg);
|
||||
|
||||
// 首帧新建轨道 hits=1 < min_hits：候选池 confirmed 为空 → 退回 measured
|
||||
TrackResult r = tracker.Update({MakeDet(200.0, 150.0, 240.0, 190.0, 0.6)});
|
||||
ASSERT_TRUE(r.tracked) << "无已确认轨道时新轨道应能当选主目标";
|
||||
ASSERT_TRUE(r.primary_id.has_value());
|
||||
EXPECT_EQ(*r.primary_id, 0);
|
||||
EXPECT_FALSE(r.is_predicted);
|
||||
|
||||
// 再跟几帧（hits 增长但仍未达 min_hits=5），依旧当选
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
r = tracker.Update({MakeDet(200.0, 150.0, 240.0, 190.0, 0.6)});
|
||||
ASSERT_TRUE(r.tracked);
|
||||
EXPECT_EQ(*r.primary_id, 0);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// 验收用例 5（§9.4-5）：cost 边界
|
||||
// 恰好 cost = 1.0 不构成匹配（仅严格小于才匹配）。
|
||||
// 构造：w_dist=1、w_iou=0（纯距离项），中心距离恰等于 Dmax → cost=1.0；
|
||||
// 同时给出略小于阈值的对照帧证明边界两侧行为正确。
|
||||
// ============================================================================
|
||||
TEST(TargetTrackerAcceptanceTest, CostExactlyOneDoesNotMatch) {
|
||||
TargetTracker::Config cfg;
|
||||
cfg.max_lost_frames = 1;
|
||||
cfg.max_association_dist = 200.0;
|
||||
cfg.dist_weight = 1.0; // 纯距离项，IoU 项权重为 0 且两框零交集
|
||||
cfg.iou_weight = 0.0;
|
||||
TargetTracker tracker(cfg);
|
||||
|
||||
// 建立轨道：中心 (100,100)
|
||||
TrackResult r = tracker.Update({MakeDet(90.0, 90.0, 110.0, 110.0, 0.9)});
|
||||
ASSERT_TRUE(r.primary_id.has_value());
|
||||
const int id_a = *r.primary_id;
|
||||
|
||||
// 下一帧检测中心 (100,300)：距离 = 200 = Dmax → cost = 1.0，恰好不匹配
|
||||
// （预测后轨道中心速度为 0，仍位于 (100,100)；两框零交集 IoU=0 无影响）
|
||||
r = tracker.Update({MakeDet(90.0, 290.0, 110.0, 310.0, 0.9)});
|
||||
// 未匹配 → 原轨道 Coast、检测新建轨道；两个目标并存
|
||||
EXPECT_EQ(tracker.ActiveCount(), 2) << "cost=1.0 不应匹配，应新建轨道";
|
||||
// 原轨道（锁定）进入 Coast：lost_frames=1
|
||||
EXPECT_TRUE(r.tracked);
|
||||
EXPECT_TRUE(r.is_predicted);
|
||||
EXPECT_EQ(*r.primary_id, id_a);
|
||||
EXPECT_EQ(r.lost_frames, 1);
|
||||
|
||||
// 对照：距离略小于阈值（199px → cost=0.995 < 1.0）应匹配成功
|
||||
TargetTracker tracker2(cfg);
|
||||
r = tracker2.Update({MakeDet(90.0, 90.0, 110.0, 110.0, 0.9)});
|
||||
const int id_b = *r.primary_id;
|
||||
r = tracker2.Update({MakeDet(90.0, 289.0, 110.0, 309.0, 0.9)});
|
||||
EXPECT_EQ(tracker2.ActiveCount(), 1) << "cost<1.0 应匹配成功，不应新建轨道";
|
||||
ASSERT_TRUE(r.primary_id.has_value());
|
||||
EXPECT_EQ(*r.primary_id, id_b);
|
||||
EXPECT_FALSE(r.is_predicted); // 实测命中
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// 验收用例 6（§9.4-6）：置信度优先
|
||||
// 两轨道竞争同一检测时，置信度高者获胜。
|
||||
// ============================================================================
|
||||
TEST(TargetTrackerAcceptanceTest, HigherConfidenceTrackWinsDetection) {
|
||||
TargetTracker tracker;
|
||||
|
||||
// 同帧建立两条轨道：A 置信度 0.5、B 置信度 0.9，相距 80px（不互相重叠）
|
||||
TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.5),
|
||||
MakeDet(180.0, 100.0, 200.0, 120.0, 0.9)});
|
||||
ASSERT_EQ(tracker.ActiveCount(), 2);
|
||||
|
||||
// 找出两条轨道 ID（按置信度区分）
|
||||
int id_low = -1;
|
||||
int id_high = -1;
|
||||
for (const auto& kv : tracker.ActiveTracks()) {
|
||||
if (kv.second.confidence > 0.8) {
|
||||
id_high = kv.first;
|
||||
} else {
|
||||
id_low = kv.first;
|
||||
}
|
||||
}
|
||||
ASSERT_NE(id_low, -1);
|
||||
ASSERT_NE(id_high, -1);
|
||||
|
||||
// 下一帧只给一个检测，放在两轨道预测中心之间（各距 40px，对称）：
|
||||
// 距离项对两者相同，贪婪按置信度降序 → 高置信度轨道 B 优先匹配
|
||||
r = tracker.Update({MakeDet(140.0, 100.0, 160.0, 120.0, 0.7)});
|
||||
|
||||
const auto tracks = tracker.ActiveTracks();
|
||||
ASSERT_EQ(tracks.size(), 2u);
|
||||
// 获胜者：命中（lost=0、hits=2、置信度更新）
|
||||
EXPECT_EQ(tracks.at(id_high).lost_count, 0) << "高置信度轨道应获胜";
|
||||
EXPECT_EQ(tracks.at(id_high).hits, 2);
|
||||
EXPECT_DOUBLE_EQ(tracks.at(id_high).confidence, 0.7);
|
||||
// 失败者：未匹配进入 Coast（lost=1、hits 不变）
|
||||
EXPECT_EQ(tracks.at(id_low).lost_count, 1) << "低置信度轨道应落选";
|
||||
EXPECT_EQ(tracks.at(id_low).hits, 1);
|
||||
EXPECT_DOUBLE_EQ(tracks.at(id_low).confidence, 0.5);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// NaN/Inf 防御（评审修复项）：坐标或置信度含非有限值的检测被丢弃，
|
||||
// 不崩溃、不新建轨道、不污染主目标输出
|
||||
// ============================================================================
|
||||
TEST(TargetTrackerAcceptanceTest, NonFiniteDetectionsAreDiscarded) {
|
||||
TargetTracker tracker;
|
||||
const double nan = std::numeric_limits<double>::quiet_NaN();
|
||||
const double inf = std::numeric_limits<double>::infinity();
|
||||
|
||||
// 第 1 帧：建立正常轨道
|
||||
TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 140.0, 140.0, 0.9)});
|
||||
ASSERT_TRUE(r.tracked);
|
||||
|
||||
// 第 2 帧：混入 NaN 坐标 / Inf 坐标 / NaN 置信度检测与一个正常检测：
|
||||
// 非有限检测全部被丢弃，仅正常检测参与关联，不新建轨道
|
||||
r = tracker.Update({MakeDet(nan, 100.0, 140.0, 140.0, 0.9),
|
||||
MakeDet(100.0, -inf, 140.0, 140.0, 0.9),
|
||||
MakeDet(100.0, 100.0, nan, 140.0, 0.9),
|
||||
MakeDet(102.0, 102.0, 142.0, 142.0, nan),
|
||||
MakeDet(101.0, 101.0, 141.0, 141.0, 0.9)});
|
||||
EXPECT_EQ(tracker.ActiveCount(), 1) << "非有限检测不应新建轨道";
|
||||
ASSERT_TRUE(r.tracked);
|
||||
EXPECT_FALSE(r.is_predicted); // 正常检测命中，实测输出
|
||||
// 主目标输出未被 NaN 污染
|
||||
EXPECT_TRUE(std::isfinite(r.center.first));
|
||||
EXPECT_TRUE(std::isfinite(r.center.second));
|
||||
ASSERT_TRUE(r.box.has_value());
|
||||
EXPECT_TRUE(std::isfinite(r.box->x1) && std::isfinite(r.box->y1));
|
||||
EXPECT_TRUE(std::isfinite(r.box->x2) && std::isfinite(r.box->y2));
|
||||
EXPECT_TRUE(std::isfinite(r.confidence));
|
||||
ASSERT_TRUE(r.raw.has_value()); // 实测帧 raw 有值
|
||||
|
||||
// 第 3 帧：仅含 NaN 检测 → 等价空帧，轨道 Coast 不崩溃
|
||||
r = tracker.Update({MakeDet(nan, nan, nan, nan, nan)});
|
||||
EXPECT_TRUE(r.tracked);
|
||||
EXPECT_TRUE(r.is_predicted);
|
||||
EXPECT_EQ(r.lost_frames, 1);
|
||||
EXPECT_FALSE(r.raw.has_value()); // Coast 时 raw 为空（§2.3）
|
||||
EXPECT_TRUE(std::isfinite(r.center.first));
|
||||
|
||||
// 持续 NaN 检测直至轨道死亡回收，全程不崩溃
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
r = tracker.Update({MakeDet(nan, 0.0, 10.0, 10.0, 0.9)});
|
||||
}
|
||||
EXPECT_FALSE(r.tracked);
|
||||
EXPECT_EQ(tracker.ActiveCount(), 0);
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// 补充行为测试（§2.3 / §6.3 / §9.5）
|
||||
// ============================================================================
|
||||
|
||||
// 丢失时返回值约定：tracked=false、定位字段空、lost_frames=0（§2.3）
|
||||
TEST(TargetTrackerBehaviorTest, LostResultFieldsAreEmpty) {
|
||||
TargetTracker tracker;
|
||||
TrackResult r = tracker.Update({}); // 空帧序列起点：无任何轨道
|
||||
EXPECT_FALSE(r.tracked);
|
||||
EXPECT_FALSE(r.primary_id.has_value());
|
||||
EXPECT_FALSE(r.box.has_value());
|
||||
EXPECT_FALSE(r.raw.has_value());
|
||||
EXPECT_EQ(r.lost_frames, 0);
|
||||
EXPECT_EQ(r.n_active, 0);
|
||||
EXPECT_DOUBLE_EQ(r.confidence, 0.0);
|
||||
|
||||
// LastResult 返回同一快照（§9.5 跨线程读取入口）
|
||||
const TrackResult cached = tracker.LastResult();
|
||||
EXPECT_FALSE(cached.tracked);
|
||||
}
|
||||
|
||||
// Coast 兜底选举：无实测轨道时选离画面中心最近的 Coast 轨道（§6.3）。
|
||||
// 注意锁定延续优先：需先让锁定轨道死亡解除锁定，才会进入重新选举分支，
|
||||
// 故采用错帧创建 + 缩短 max_lost 的方式构造场景。
|
||||
TEST(TargetTrackerBehaviorTest, CoastingFallbackChoosesNearestToFrameCenter) {
|
||||
TargetTracker::Config cfg;
|
||||
cfg.max_lost_frames = 2;
|
||||
TargetTracker tracker(cfg);
|
||||
|
||||
// F1：角落目标（高置信度）先建立并被锁定
|
||||
tracker.Update({MakeDet(10.0, 10.0, 30.0, 30.0, 0.95)});
|
||||
// F2：画面中心附近目标建立；角落目标丢失(1)
|
||||
tracker.Update({MakeDet(300.0, 220.0, 340.0, 260.0, 0.9)});
|
||||
// F3：远处目标建立；角落丢失(2)、中心目标丢失(1)
|
||||
tracker.Update({MakeDet(600.0, 450.0, 620.0, 470.0, 0.85)});
|
||||
// F4：全部丢失：角落丢失(3) > max_lost 死亡解除锁定；
|
||||
// 其余两条 Coast 存活 → 重新选举选离画面中心最近者
|
||||
TrackResult r = tracker.Update({}, FrameShape{480, 640});
|
||||
ASSERT_TRUE(r.tracked);
|
||||
EXPECT_TRUE(r.is_predicted);
|
||||
EXPECT_NEAR(r.center.first, 320.0, 25.0); // 靠近画面中心 (320,240) 者当选
|
||||
EXPECT_NEAR(r.center.second, 240.0, 25.0);
|
||||
EXPECT_EQ(r.n_active, 2); // 角落轨道已回收
|
||||
}
|
||||
|
||||
// 锁定延续优先于置信度选举：锁定轨道实测时即使置信度较低也保持主目标（§6.3 ①）
|
||||
TEST(TargetTrackerBehaviorTest, LockedPrimaryKeptWhileMeasured) {
|
||||
TargetTracker tracker;
|
||||
TrackResult r = tracker.Update({MakeDet(100.0, 100.0, 120.0, 120.0, 0.4)});
|
||||
ASSERT_TRUE(r.primary_id.has_value());
|
||||
const int locked_id = *r.primary_id;
|
||||
|
||||
// 高置信度新目标出现并持续，锁定目标也持续实测 → primary_id 不变
|
||||
for (int i = 0; i < 5; ++i) {
|
||||
r = tracker.Update({MakeDet(100.0 + i, 100.0, 120.0 + i, 120.0, 0.4),
|
||||
MakeDet(400.0, 300.0, 430.0, 330.0, 0.95)});
|
||||
ASSERT_TRUE(r.primary_id.has_value());
|
||||
EXPECT_EQ(*r.primary_id, locked_id);
|
||||
EXPECT_FALSE(r.is_predicted);
|
||||
}
|
||||
}
|
||||
|
||||
// frame_shape 缺省按 640×480 画面中心 (320,240) 计算（§8.2 / §2.2）：
|
||||
// 两条 Coast 轨道分别位于默认中心两侧，当选者应为更近的一侧
|
||||
TEST(TargetTrackerBehaviorTest, DefaultFrameCenterIs320x240) {
|
||||
TargetTracker::Config cfg;
|
||||
cfg.max_lost_frames = 2;
|
||||
TargetTracker tracker(cfg);
|
||||
|
||||
// F1：角落目标先建立并被锁定（高置信度）
|
||||
tracker.Update({MakeDet(0.0, 0.0, 20.0, 20.0, 0.95)});
|
||||
// F2：左侧目标（中心 (280,240)，距默认中心 40px）建立
|
||||
tracker.Update({MakeDet(270.0, 230.0, 290.0, 250.0, 0.9)});
|
||||
// F3：右侧目标（中心 (500,240)，距默认中心 180px）建立
|
||||
tracker.Update({MakeDet(490.0, 230.0, 510.0, 250.0, 0.85)});
|
||||
// F4：全部丢失：锁定轨道死亡解除锁定，两条 Coast 轨道兜底选举
|
||||
// （不传 frame_shape → 按默认画面中心 (320,240)；两候选间距足够大，
|
||||
// 速度估计漂移不影响最近者判定）
|
||||
TrackResult r = tracker.Update({});
|
||||
ASSERT_TRUE(r.tracked);
|
||||
EXPECT_TRUE(r.is_predicted);
|
||||
EXPECT_NEAR(r.center.first, 280.0, 30.0); // 左侧更近，当选
|
||||
EXPECT_NEAR(r.center.second, 240.0, 30.0);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,177 @@
|
||||
// =============================================================================
|
||||
// tracklet_test.cpp —— Tracklet 与几何工具单元测试
|
||||
//
|
||||
// 覆盖 docs/视觉追踪技术路线.md：
|
||||
// - IoU 边界（空框 / 零交集 / 完全重合=1，§5.2）
|
||||
// - IsDead / IsCoasting 判定（§3.2 派生状态，§6.1 严格大于）
|
||||
// - Predict / Update / MarkMiss 计数语义（§7 末尾伪代码）
|
||||
// - BoxXYXY ↔ BoxCXCYWH 互转（§7）
|
||||
// =============================================================================
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "target_tracker/tracklet.h"
|
||||
|
||||
using drone::tracker::BoxCXCYWH;
|
||||
using drone::tracker::BoxXYXY;
|
||||
using drone::tracker::ComputeIoU;
|
||||
using drone::tracker::ToCenterSize;
|
||||
using drone::tracker::ToXYXY;
|
||||
using drone::tracker::Tracklet;
|
||||
|
||||
namespace {
|
||||
|
||||
// ---- 框表示互转（§7） ----
|
||||
|
||||
TEST(BoxConversionTest, RoundTripPreservesValues) {
|
||||
const BoxXYXY box{10.0, 20.0, 50.0, 80.0};
|
||||
const BoxCXCYWH c = ToCenterSize(box);
|
||||
EXPECT_DOUBLE_EQ(c.cx, 30.0); // (10+50)/2
|
||||
EXPECT_DOUBLE_EQ(c.cy, 50.0); // (20+80)/2
|
||||
EXPECT_DOUBLE_EQ(c.w, 40.0); // 50−10
|
||||
EXPECT_DOUBLE_EQ(c.h, 60.0); // 80−20
|
||||
|
||||
const BoxXYXY back = ToXYXY(c);
|
||||
EXPECT_DOUBLE_EQ(back.x1, box.x1);
|
||||
EXPECT_DOUBLE_EQ(back.y1, box.y1);
|
||||
EXPECT_DOUBLE_EQ(back.x2, box.x2);
|
||||
EXPECT_DOUBLE_EQ(back.y2, box.y2);
|
||||
}
|
||||
|
||||
// ---- IoU 边界（§5.2） ----
|
||||
|
||||
TEST(IoUTest, IdenticalBoxesReturnOne) {
|
||||
const BoxXYXY a{10.0, 10.0, 30.0, 30.0};
|
||||
EXPECT_NEAR(ComputeIoU(a, a), 1.0, 1e-12); // 完全重合 = 1
|
||||
}
|
||||
|
||||
TEST(IoUTest, DisjointBoxesReturnZero) {
|
||||
const BoxXYXY a{0.0, 0.0, 10.0, 10.0};
|
||||
const BoxXYXY b{20.0, 20.0, 30.0, 30.0};
|
||||
EXPECT_DOUBLE_EQ(ComputeIoU(a, b), 0.0); // 零交集
|
||||
}
|
||||
|
||||
TEST(IoUTest, TouchingEdgesReturnZero) {
|
||||
// 仅边/角相接：交集宽高为 0 → 交集面积 ≤ 0 返回 0（§5.2）
|
||||
const BoxXYXY a{0.0, 0.0, 10.0, 10.0};
|
||||
const BoxXYXY b{10.0, 0.0, 20.0, 10.0}; // 右边缘相接
|
||||
EXPECT_DOUBLE_EQ(ComputeIoU(a, b), 0.0);
|
||||
const BoxXYXY c{10.0, 10.0, 20.0, 20.0}; // 角点相接
|
||||
EXPECT_DOUBLE_EQ(ComputeIoU(a, c), 0.0);
|
||||
}
|
||||
|
||||
TEST(IoUTest, EmptyBoxReturnsZero) {
|
||||
const BoxXYXY normal{0.0, 0.0, 10.0, 10.0};
|
||||
const BoxXYXY zero_area{5.0, 5.0, 5.0, 5.0}; // 零面积
|
||||
const BoxXYXY inverted{10.0, 10.0, 0.0, 0.0}; // 反向（负宽高）
|
||||
EXPECT_DOUBLE_EQ(ComputeIoU(normal, zero_area), 0.0);
|
||||
EXPECT_DOUBLE_EQ(ComputeIoU(zero_area, normal), 0.0);
|
||||
EXPECT_DOUBLE_EQ(ComputeIoU(normal, inverted), 0.0);
|
||||
EXPECT_DOUBLE_EQ(ComputeIoU(zero_area, zero_area), 0.0);
|
||||
}
|
||||
|
||||
TEST(IoUTest, PartialOverlapIsCorrect) {
|
||||
// A = [0,0,10,10]（面积 100），B = [5,5,15,15]（面积 100），交集 25
|
||||
const BoxXYXY a{0.0, 0.0, 10.0, 10.0};
|
||||
const BoxXYXY b{5.0, 5.0, 15.0, 15.0};
|
||||
EXPECT_NEAR(ComputeIoU(a, b), 25.0 / 175.0, 1e-12);
|
||||
EXPECT_NEAR(ComputeIoU(b, a), 25.0 / 175.0, 1e-12); // 对称性
|
||||
}
|
||||
|
||||
// ---- 构造与派生状态（§3.2 / §6.1） ----
|
||||
|
||||
TEST(TrackletTest, ConstructionInitialFields) {
|
||||
const BoxCXCYWH meas{100.0, 100.0, 20.0, 20.0};
|
||||
Tracklet t(7, meas, 0.9, 1, 8);
|
||||
|
||||
EXPECT_EQ(t.track_id, 7);
|
||||
EXPECT_EQ(t.lost_count, 0);
|
||||
EXPECT_EQ(t.max_lost, 8);
|
||||
EXPECT_DOUBLE_EQ(t.confidence, 0.9);
|
||||
EXPECT_EQ(t.cls_id, 1);
|
||||
EXPECT_EQ(t.age, 1); // 创建时为 1（§3.2）
|
||||
EXPECT_EQ(t.hits, 1); // 创建时为 1（§3.2）
|
||||
EXPECT_DOUBLE_EQ(t.last_center.first, 100.0);
|
||||
EXPECT_DOUBLE_EQ(t.last_center.second, 100.0);
|
||||
ASSERT_TRUE(t.last_box.has_value()); // 创建当帧即赋 last_box（§7 步骤 5）
|
||||
EXPECT_DOUBLE_EQ(t.last_box->x1, 90.0);
|
||||
EXPECT_DOUBLE_EQ(t.last_box->y1, 90.0);
|
||||
EXPECT_DOUBLE_EQ(t.last_box->x2, 110.0);
|
||||
EXPECT_DOUBLE_EQ(t.last_box->y2, 110.0);
|
||||
EXPECT_FALSE(t.IsDead());
|
||||
EXPECT_FALSE(t.IsCoasting());
|
||||
}
|
||||
|
||||
// IsDead 使用严格大于：lost_count == max_lost 仍存活（§6.1 注记 2）
|
||||
TEST(TrackletTest, IsDeadUsesStrictlyGreater) {
|
||||
Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8);
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
t.MarkMiss();
|
||||
EXPECT_FALSE(t.IsDead()) << "lost_count=" << t.lost_count
|
||||
<< " 未超过 max_lost 不应判死";
|
||||
}
|
||||
EXPECT_EQ(t.lost_count, 8);
|
||||
t.MarkMiss(); // lost_count = 9 > 8
|
||||
EXPECT_TRUE(t.IsDead());
|
||||
}
|
||||
|
||||
TEST(TrackletTest, IsCoastingWhenLostPositive) {
|
||||
Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8);
|
||||
EXPECT_FALSE(t.IsCoasting());
|
||||
t.MarkMiss();
|
||||
EXPECT_TRUE(t.IsCoasting()); // lost_count > 0 即 Coast（§3.2）
|
||||
}
|
||||
|
||||
// ---- 生命周期方法计数语义（§7） ----
|
||||
|
||||
TEST(TrackletTest, PredictAdvancesAgeAndUpdatesLastState) {
|
||||
Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8);
|
||||
const int age_before = t.age;
|
||||
t.Predict(); // 速度为零：位置不变，但 age+1、last_box 更新为预测框
|
||||
EXPECT_EQ(t.age, age_before + 1);
|
||||
ASSERT_TRUE(t.last_box.has_value());
|
||||
EXPECT_NEAR(t.last_center.first, 100.0, 1e-9);
|
||||
EXPECT_NEAR(t.last_center.second, 100.0, 1e-9);
|
||||
// hits / lost_count / confidence 不受预测影响
|
||||
EXPECT_EQ(t.hits, 1);
|
||||
EXPECT_EQ(t.lost_count, 0);
|
||||
EXPECT_DOUBLE_EQ(t.confidence, 0.9);
|
||||
}
|
||||
|
||||
TEST(TrackletTest, UpdateResetsLostAndIncrementsHits) {
|
||||
Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8);
|
||||
t.MarkMiss();
|
||||
t.MarkMiss();
|
||||
ASSERT_EQ(t.lost_count, 2);
|
||||
ASSERT_TRUE(t.IsCoasting());
|
||||
|
||||
const int age_before = t.age;
|
||||
const BoxXYXY box{150.0, 160.0, 170.0, 180.0};
|
||||
t.Update(box, 0.75, 2);
|
||||
|
||||
EXPECT_EQ(t.lost_count, 0); // 匹配成功清零（§7）
|
||||
EXPECT_EQ(t.hits, 2); // hits+1
|
||||
EXPECT_EQ(t.age, age_before + 1); // age+1
|
||||
EXPECT_DOUBLE_EQ(t.confidence, 0.75);
|
||||
EXPECT_EQ(t.cls_id, 2);
|
||||
EXPECT_DOUBLE_EQ(t.last_center.first, 160.0); // (150+170)/2
|
||||
EXPECT_DOUBLE_EQ(t.last_center.second, 170.0); // (160+180)/2
|
||||
ASSERT_TRUE(t.last_box.has_value());
|
||||
EXPECT_DOUBLE_EQ(t.last_box->x1, box.x1);
|
||||
ASSERT_TRUE(t.last_raw_box.has_value()); // 实测框同步记录
|
||||
EXPECT_DOUBLE_EQ(t.last_raw_box->x2, box.x2);
|
||||
EXPECT_FALSE(t.IsCoasting());
|
||||
}
|
||||
|
||||
TEST(TrackletTest, MarkMissIncrementsLostAndAge) {
|
||||
Tracklet t(0, {100.0, 100.0, 20.0, 20.0}, 0.9, 0, 8);
|
||||
const int age_before = t.age;
|
||||
t.MarkMiss();
|
||||
EXPECT_EQ(t.lost_count, 1);
|
||||
EXPECT_EQ(t.age, age_before + 1);
|
||||
// MarkMiss 不影响 hits / confidence
|
||||
EXPECT_EQ(t.hits, 1);
|
||||
EXPECT_DOUBLE_EQ(t.confidence, 0.9);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
@@ -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
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
|
||||
|
||||
# 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()
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
@@ -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 <chrono>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
#include <spdlog/spdlog.h>
|
||||
#include <target_tracker/target_tracker.h>
|
||||
|
||||
#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::milliseconds>(
|
||||
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<drone::tracker::Detection> 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<int>(1000.0 / cfg.control.frequency_hz);
|
||||
const int normal_ticks = kNormalPhaseSeconds * static_cast<int>(cfg.control.frequency_hz);
|
||||
const int coast_ticks = kCoastPhaseSeconds * static_cast<int>(cfg.control.frequency_hz);
|
||||
const int lost_ticks = kLostPhaseSeconds * static_cast<int>(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<double>(tick) / cfg.control.frequency_hz;
|
||||
// 模拟单调时钟：1 秒偏移保证取样时间戳有效
|
||||
const std::int64_t now_ms = 1000 + static_cast<std::int64_t>(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<drone::tracker::Detection> 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<std::uint32_t>(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;
|
||||
}
|
||||
@@ -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 <cmath>
|
||||
|
||||
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
|
||||
@@ -0,0 +1,31 @@
|
||||
#pragma once
|
||||
|
||||
// =============================================================================
|
||||
// i_command_transport.h —— 命令传输层抽象接口
|
||||
//
|
||||
// 设计目标（双重解耦）：
|
||||
// - 传输层只吃字节流：串口 / 电台 / 回环打印均可注入实现，
|
||||
// 后续重写传输只换实现，不动打包与控制律；
|
||||
// - 打包封装（MavlinkCommandGateway）与本接口组合，不直接依赖具体链路。
|
||||
//
|
||||
// 控制律不直接写飞控（物理追踪思路 §2.2 控制单出口约束的独立库映射）：
|
||||
// 网关打包后交由实现方决定如何送达。
|
||||
// =============================================================================
|
||||
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
|
||||
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
|
||||
@@ -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
|
||||
+65
@@ -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 <cstddef>
|
||||
#include <cstdint>
|
||||
|
||||
#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
|
||||
@@ -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
|
||||
@@ -0,0 +1,56 @@
|
||||
#pragma once
|
||||
|
||||
// =============================================================================
|
||||
// rate_limiter.h —— 限幅与变化率限制（纯算法构件，header-only）
|
||||
//
|
||||
// 对应规格：docs/物理追踪思路.md
|
||||
// §8 所有阈值集中配置：速度/加速度/方向变化率必须限幅（FR-033）
|
||||
// S6 首条视觉速度设定值必须经过限幅，避免位置→速度切换产生突变
|
||||
// （状态机设计.md S6）
|
||||
//
|
||||
// 特性（热路径零分配、无虚函数、无锁）：
|
||||
// - Clamp()：静态限幅；
|
||||
// - SlewRateLimiter：保存上次输出，按配置变化率限幅；初值置 0 时
|
||||
// 天然实现"首条指令突变抑制"（S6）。
|
||||
// =============================================================================
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
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
|
||||
@@ -0,0 +1,38 @@
|
||||
#pragma once
|
||||
|
||||
// =============================================================================
|
||||
// staleness_checker.h —— 输入超龄判定（单调毫秒时钟）
|
||||
//
|
||||
// 对应规格：docs/物理追踪思路.md
|
||||
// §7.1 超龄数据只能触发降级或告警，不能继续生成正常控制指令；
|
||||
// 超龄判定使用取样时间 + valid_for_ms（MessageHeader 语义）。
|
||||
//
|
||||
// 特性：无状态纯函数封装（零分配、无虚函数、无锁）。
|
||||
// =============================================================================
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
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
|
||||
+63
@@ -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 <cstdint>
|
||||
|
||||
#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
|
||||
@@ -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 <cstdint>
|
||||
#include <string>
|
||||
|
||||
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
|
||||
@@ -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 <cstdint>
|
||||
|
||||
#include <target_tracker/target_tracker.h>
|
||||
|
||||
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
|
||||
@@ -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 <cmath>
|
||||
#include <stdexcept>
|
||||
|
||||
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
|
||||
@@ -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 <cmath>
|
||||
|
||||
#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<mavlink_message_t*>(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<float>(output.vx_mps), static_cast<float>(output.vy_mps),
|
||||
static_cast<float>(output.vz_mps),
|
||||
0.0F, 0.0F, 0.0F, // 加速度字段：忽略
|
||||
static_cast<float>(output.yaw_deg * kDegToRad),
|
||||
static_cast<float>(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
|
||||
@@ -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 <cmath>
|
||||
|
||||
#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
|
||||
@@ -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 <algorithm>
|
||||
#include <cmath>
|
||||
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
#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<double>(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
|
||||
@@ -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 <fstream>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
#include <nlohmann/json.hpp>
|
||||
|
||||
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<double>();
|
||||
}
|
||||
|
||||
/// 取必需的整数字段
|
||||
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::int64_t>();
|
||||
}
|
||||
|
||||
/// 取必需的字符串字段
|
||||
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<std::string>();
|
||||
}
|
||||
|
||||
/// 校验字段为正数；非法抛异常并携带字段名
|
||||
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<int>(
|
||||
RequirePositive(RequireInteger(s, "camera", "image_width"), "camera.image_width"));
|
||||
c.image_height = static_cast<int>(
|
||||
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
|
||||
@@ -0,0 +1,56 @@
|
||||
// =============================================================================
|
||||
// frame_transform_test.cpp —— RotateBodyToNed 单元测试
|
||||
//
|
||||
// 覆盖（物理追踪思路 §2.3，A4）：
|
||||
// 零航向恒等、朝东时机体前向 → 东向、垂直分量不变、模长守恒。
|
||||
// =============================================================================
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#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
|
||||
@@ -0,0 +1,69 @@
|
||||
// =============================================================================
|
||||
// line_of_sight_test.cpp —— PixelToAngle 单元测试
|
||||
//
|
||||
// 覆盖（物理追踪思路 §4.1 / §5.1，C2）：
|
||||
// 零偏移、画面边缘偏移 = 半视场角、奇对称、单调性、
|
||||
// 与线性近似的偏离（全程针孔模型）、构造期非法参数抛异常。
|
||||
// =============================================================================
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#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
|
||||
@@ -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 <cstdint>
|
||||
#include <cstring>
|
||||
#include <vector>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#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<std::uint8_t>& last() const { return last_; }
|
||||
|
||||
private:
|
||||
bool succeed_ = true;
|
||||
int send_count_ = 0;
|
||||
std::vector<std::uint8_t> last_;
|
||||
};
|
||||
|
||||
float ReadFloat(const std::vector<std::uint8_t>& 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<std::uint8_t>& buf, std::size_t payload_off) {
|
||||
return static_cast<std::uint16_t>(buf[kHeaderLen + payload_off]) |
|
||||
(static_cast<std::uint16_t>(buf[kHeaderLen + payload_off + 1]) << 8);
|
||||
}
|
||||
|
||||
void ExpectId84Header(const std::vector<std::uint8_t>& 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<float>(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<float>(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
|
||||
@@ -0,0 +1,123 @@
|
||||
// =============================================================================
|
||||
// pid_controller_test.cpp —— PidController 单元测试
|
||||
//
|
||||
// 覆盖（物理追踪思路 §6.2 / §6.4）：
|
||||
// 位置式 P/I/D 三项、积分钳位、输出限幅、异常节拍保护、
|
||||
// anti-windup 条件积分（C1 修复回归：饱和冻结 + 误差反号回卷）、
|
||||
// ResetIntegral / Reset 行为。
|
||||
// =============================================================================
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#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
|
||||
@@ -0,0 +1,76 @@
|
||||
// =============================================================================
|
||||
// rate_limiter_test.cpp —— Clamp / SlewRateLimiter 单元测试
|
||||
//
|
||||
// 覆盖（物理追踪思路 §8、状态机设计 S6）：
|
||||
// 静态限幅、变化率限幅步进、首条指令从零渐变（S6）、
|
||||
// 异常节拍保护、Reset 归零。
|
||||
// =============================================================================
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#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
|
||||
@@ -0,0 +1,41 @@
|
||||
// =============================================================================
|
||||
// staleness_checker_test.cpp —— StalenessChecker 单元测试
|
||||
//
|
||||
// 覆盖（物理追踪思路 §7.1）：
|
||||
// 从未取样判超龄、有效期内新鲜、超出有效期超龄、
|
||||
// 边界恰好在有效期上（不算超龄）、时间戳不一致判超龄（C4）。
|
||||
// =============================================================================
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#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
|
||||
@@ -0,0 +1,349 @@
|
||||
// =============================================================================
|
||||
// tracking_control_law_test.cpp —— TrackingControlLaw 主控律单元测试
|
||||
//
|
||||
// 覆盖（物理追踪思路 §2.3 / §4 / §5 / §6 / §7 / §9）：
|
||||
// 超龄降级（姿态缺失/过期、视觉超龄、降级优先级）、判丢零速刹车、
|
||||
// Coast 清积分与标志、速率式/位置式航向通道（含绝对航向合成与回绕，
|
||||
// A2）、垂直统一速度支路（A1）、距离通道三种无距离降级动作、
|
||||
// 首拍变化率限幅（S6）、机体系→NED 输出旋转（A4）。
|
||||
// =============================================================================
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#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
|
||||
@@ -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 <cstdio>
|
||||
#include <fstream>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#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<bool>(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
|
||||
@@ -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<const TrackingConfig*>`，后台解析新配置成功后原子换指针，
|
||||
控制线程每拍读取当前指针快照使用（当前未实现，不引入额外复杂度）。
|
||||
|
||||
### 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 后需复核构建与测试。
|
||||
@@ -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）在测试文件头注释中标注对应章节。
|
||||
Binary file not shown.
@@ -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
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
"""pytest 根配置：保证无论从哪个工作目录启动 pytest，均可导入 utils/ 下的两个包。"""
|
||||
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
_UTILS_DIR = Path(__file__).resolve().parent
|
||||
if str(_UTILS_DIR) not in sys.path:
|
||||
sys.path.insert(0, str(_UTILS_DIR))
|
||||
@@ -0,0 +1,207 @@
|
||||
"""demo_track_control.py —— 视觉跟踪控制链路集成演示（胶水层）。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/visual_track_control/examples/track_control_demo.cpp。
|
||||
|
||||
串联三个组件（target_tracker 与 visual_track_control 互不 import，
|
||||
本脚本是唯一的转换点）：
|
||||
模拟检测序列 → TargetTracker（8 维恒速卡尔曼多目标追踪）
|
||||
→ TrackResult 转 TrackState（解耦边界）
|
||||
→ TrackingControlLaw（三通道控制律 + 限幅 + 状态机）
|
||||
→ CommandSender（mock 记录 / MAVSDK offboard 实发）
|
||||
|
||||
场景时间线（20 Hz，--speedup 可加速）：
|
||||
0~ 6s 正常跟踪：目标匀速横移 + 雷达距离 30→10 m 前向逼近
|
||||
6~10s 目标消失：追踪器 Coast 预测（60 帧 = 3 s 后轨道判丢，
|
||||
控制律自动切换零速刹车）
|
||||
10~13s 持续判丢：维持零速刹车
|
||||
|
||||
用法：
|
||||
python utils/demo_track_control.py # mock 传输
|
||||
python utils/demo_track_control.py --transport mavsdk # MAVSDK（需飞控/SITL）
|
||||
python utils/demo_track_control.py --speedup 4 # 4 倍速
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import asyncio
|
||||
import logging
|
||||
from pathlib import Path
|
||||
|
||||
from target_tracker.target_tracker import Detection, TargetTracker, TrackerConfig
|
||||
from visual_track_control.config import load_from_json
|
||||
from visual_track_control.mavsdk_gateway import MavsdkCommandGateway
|
||||
from visual_track_control.tracking_control_law import TrackingControlLaw
|
||||
from visual_track_control.types import (ControlMode, ControlSnapshot,
|
||||
ControlOutput, TrackState)
|
||||
|
||||
logger = logging.getLogger("demo")
|
||||
|
||||
_DEMO_DIR = Path(__file__).resolve().parent
|
||||
_DEFAULT_CONFIG = _DEMO_DIR / "config" / "track_control.json"
|
||||
|
||||
# 模拟场景参数
|
||||
_NORMAL_SECS = 6.0 # 正常跟踪阶段
|
||||
_COAST_SECS = 4.0 # 目标消失 → Coast（含轨道判亡）
|
||||
_LOST_SECS = 3.0 # 判丢后持续刹车阶段
|
||||
_DEMO_MAX_LOST_FRAMES = 60 # 追踪器 Coast 窗口（20 Hz 下 = 3 s）
|
||||
_STATUS_EVERY_TICKS = 10 # 每 0.5 s 打印一行状态
|
||||
|
||||
|
||||
def track_result_to_state(result) -> 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()
|
||||
@@ -0,0 +1,3 @@
|
||||
[pytest]
|
||||
testpaths = tests
|
||||
python_files = *_test.py test_*.py
|
||||
@@ -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<T>` → 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 条移植测试锁定。
|
||||
@@ -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",
|
||||
]
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
@@ -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])
|
||||
@@ -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)
|
||||
@@ -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
|
||||
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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++ 一致，由移植测试锁定。
|
||||
@@ -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",
|
||||
]
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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 模式（结束时调用）。"""
|
||||
...
|
||||
@@ -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
|
||||
@@ -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
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user