新增双目yolo实现python代码
This commit is contained in:
keita
Binary file not shown.
@@ -0,0 +1,170 @@
|
||||
# 双目 YOLO 目标识别模块需求说明（检测前端）
|
||||
|
||||
> **定位**：双目链路中的"检测前端"，服务于已实现的 `utils/stereo_ranger`。本模块负责在左右目图像上独立运行 YOLO 类检测器，并输出**符合 stereo_ranger 输入契约**的检测框列表；**不承担**测距、左右目关联与立体校正。
|
||||
>
|
||||
> **版本**：0.2（2026-09-11；按 `utils/stereo_ranger` v1.0 实现接口对齐，文档引用随《双目测距方案》更名为《双目测距技术路线》同步更新）。
|
||||
>
|
||||
> **语义权威**：`docs/双目测距技术路线.md`（下称《测距路线》）——几何、取点、匹配、有效性、退化语义一律以其为准；**字段级接口以 `utils/stereo_ranger/types.py` 为准**（本文只做需求描述，不重复定义）。
|
||||
|
||||
---
|
||||
|
||||
## 1. 目的与定位
|
||||
|
||||
单目检测给出像素框，stereo_ranger 需要"同一逻辑帧、左右目各自"的检测结果来完成视差测距。本模块把"图像 → 原生坐标系检测框"的整段处理收敛为对测距端透明的单一输入源：
|
||||
|
||||
```
|
||||
采集/同步 → 预处理（校正+letterbox）→ 左右目并行 YOLO 推理 → NMS
|
||||
→ 框映射回原生分辨率 → 输出 DetectionFrame（左右各一）→ stereo_ranger.update(...)
|
||||
```
|
||||
|
||||
### 1.1 明确不做的事
|
||||
|
||||
- 不做视差计算、左右目关联、距离/方位估计（均属 stereo_ranger）；
|
||||
- 不做立体校正本身（校正映射属于采集预处理，契约见《测距路线》§2.1"校正契约"与本文 §2.2 第 4 条）；
|
||||
- 不做跨帧跟踪（`track_id` 可由外部追踪器注入；无追踪器时留空，见 §2.1）。
|
||||
|
||||
---
|
||||
|
||||
## 2. 与 stereo_ranger 的接口契约（硬性要求）
|
||||
|
||||
接口对齐 `utils/stereo_ranger/types.py` 与 `utils/stereo_ranger/stereo_ranger.py` 的实现，任何偏离都会破坏测距正确性。
|
||||
|
||||
### 2.1 输出数据定义
|
||||
|
||||
输出即 stereo_ranger 的输入：**每目一个 `DetectionFrame`**。字段名与语义以
|
||||
`utils/stereo_ranger/types.py` 为准（下例仅为语义说明，不重复定义）：
|
||||
|
||||
```python
|
||||
@dataclass
|
||||
class Detection:
|
||||
x1: float # 框左上角 x（已校正图像坐标，原生分辨率 px）
|
||||
y1: float # 框左上角 y
|
||||
x2: float # 框右下角 x
|
||||
y2: float # 框右下角 y
|
||||
confidence: float = 1.0 # 检测置信度 [0,1]
|
||||
class_id: int = 0 # 类别 id（左右目同一目标必须一致，§2.2 第 5 条）
|
||||
track_id: int | None = None # 检测侧轨道关联；无追踪器时留空（0/负数一律视为"无轨道"）
|
||||
|
||||
@dataclass
|
||||
class DetectionFrame:
|
||||
frame_id: int # 帧标识；左右两帧必须一致（同帧契约，§2.3）
|
||||
detections: Sequence[Detection] = () # 该目本帧检测框
|
||||
```
|
||||
|
||||
- **推荐直接复用** `utils/stereo_ranger/types.py` 的 `Detection` / `DetectionFrame`
|
||||
（纯值语义、无第三方依赖；只共享数据约定、不引入解算逻辑，不违反前端/测距解耦）；
|
||||
若因部署边界另建类型，字段名与语义必须逐字一致——两处定义漂移会静默破坏视差取点；
|
||||
- `track_id` **仅供测距端做目标隔离键**（左右一致且为正时优先），不参与匹配与解算（§2.5）；
|
||||
- 每目一帧内多目标的排列顺序无要求，按检测器输出自然顺序即可。
|
||||
|
||||
### 2.2 坐标与几何硬约束
|
||||
|
||||
1. **原生分辨率坐标系**：输出框必须位于校正后图像的原生分辨率（如 1920×1080）下；视差在原生坐标计算，才能保留传感器全部像素精度（《测距路线》§2.1"坐标基准一致"）。检测器输入尺寸（如 640×640）上的框必须经 letter-box 逆变换精确映射回原生坐标。
|
||||
2. **左右目预处理几何位级一致**：同一缩放、同一填充。禁止左右图各自自适应缩放/填充——那会破坏校正后的极线对齐性质。
|
||||
3. **框边语义**：`(x1, y1)` 为左上角、`x1` 即框"左边缘"（测距端主方案直接取 `x1_L − x1_R` 作视差，《测距路线》§6.1）。实现方不得把框定义改为中心点+宽高而不转换。
|
||||
4. **立体校正前提**：本模块输出的是"校正后图像"上的框。若输入为原始图，采集端须先做与测距端内参一致的重映射（校准映射、内参由标定给出）。
|
||||
5. **同一物理目标在左右目必然同时可见时**，其两框 `y` 坐标应接近（垂直残差验收见 §6）；`x1` 有系统偏移（`≈ F·B/Z` 视差）属正常，不得在识别端"对齐"掉。**同一目标的类别在两目必须一致**——类别一致是测距端匹配的硬约束（《测距路线》§6.2），不一致会退化为两条"仅方位"记录。
|
||||
6. **输出前自检**：所有框坐标必须为有限值且满足 `x2 > x1`、`y2 > y1`。非有限值框会被测距端丢弃（不参与匹配、不发布记录），畸变框会污染框高约束与匹配代价——自检应在识别端出口完成，而不是依赖下游兜底。
|
||||
|
||||
### 2.3 帧配对要求
|
||||
|
||||
- 每帧必须携带**单调递增的帧标识**（`DetectionFrame.frame_id`，int），左右两个容器共用同一值；
|
||||
- `frame_id` 不一致时测距端**拒收整帧、返回空列表**（《测距路线》§2.1 同帧契约、§5 第 ① 步）——下游兜底不等于可以松懈，正确配对是采集端的责任；
|
||||
- 无硬件同步的双独立流形态下，采集端按接收时间窗配对并输出帧标识，残余时序抖动应记录供误差评估（两种同步形态的取舍见《双目深度估计思路》§3.2，该文档为链路外部参考）；
|
||||
- 同帧拼接（side-by-side）单流形态：解码一次、裁切为左右图，天然同曝光，推荐优先支持；
|
||||
- 帧标识由采集端按真实曝光顺序分配，**禁止为凑成"同帧"而重编号**（§2.5 第 5 条）。
|
||||
|
||||
### 2.4 与测距端的对接方式（示例）
|
||||
|
||||
```python
|
||||
from utils.stereo_ranger import StereoRanger
|
||||
from utils.stereo_ranger.types import DetectionFrame # 纯值类型，可安全复用
|
||||
|
||||
frontend = StereoYoloFrontend(cfg) # 本模块主类
|
||||
ranger = StereoRanger(stereo_cfg) # 配置见 §4，实测值见 utils/config/stereo_ranger.json
|
||||
|
||||
left_frame, right_frame = frontend.detect(left_img, right_img, frame_id=n) # 各一个 DetectionFrame
|
||||
records = ranger.update(left_frame, right_frame, now_ms=monotonic_ms())
|
||||
```
|
||||
|
||||
- **第三个参数是 `now_ms`（平台单调时钟毫秒，仅用于输出打戳，默认 0），不是帧序号**——按位置把 `n` 传进去会被静默当作时间戳，污染 `timestamp_ms`；
|
||||
- 返回 `list[RangeRecord]`，顺序：先按左目框顺序（配对→完整记录 / 未配对→仅方位记录），再补未配对的右目框的"仅方位"记录；左目非空时记录数与左目框数一致；
|
||||
- **没有 `matched` 字段**。单目检出的记录为 `degradation = BEARING_ONLY`、`valid = False`、`forward_distance_m = None`，方位取该目框中心；
|
||||
- 配对成功但域外/一致性异常时仍发布 `degradation = COMPLETE` 的记录（`disparity_px`、`center_mid_px` 可用），但 `valid = False`、`forward_distance_m = None`——**域外不撒谎，也不丢方位**（《测距路线》§7.3）；
|
||||
- **测距端不消费检测置信度**：完整记录的 `confidence` 是匹配质量 `clamp(1 − 匹配代价)`（连续跳变时乘降级系数，判 invalid 时置 0）；仅方位记录的 `confidence` 才直接取检测置信度。故 conf 阈值/NMS 必须由识别端自行完成后再输出；
|
||||
- 两目检测均为空 → 返回空列表（正常空帧，不告警）。
|
||||
|
||||
### 2.5 输入纯净性硬约束（v0.2 新增，飞行安全相关）
|
||||
|
||||
1. **只送本帧原生实测框**：`update()` 的输入必须是**本帧、在原生校正图上检测得到的框**。跨帧外推框（追踪器预测/Coast 补帧）、平滑后的框、按上一帧位移推算的"虚拟框"**一律不得送入**；
|
||||
2. **外推框会伪装成合法测量**：框的 `x1` 偏差直接进入视差 `D = x1_L − x1_R`，`Z = F·B/D` 没有任何机制识别"这不是本帧观测"，误差随 `Z²` 放大且不触发 `valid` 门（《测距路线》§7.1）；测距端的时序一致性监控最多把它降级，不能纠偏（《测距路线》§6.7）；
|
||||
3. **目标丢失就该没有距离**：本帧未检出 → 不发布该目标检测框；测距端对其记失效、连续 `filter_reset_frames` 帧后清空滤波状态（《测距路线》§6.5）。距离中断（coast）属于**消费侧**策略，且必须与 `valid` 语义分开标注；
|
||||
4. **`track_id` 仅作隔离键**：由外部追踪器注入时只用于目标状态隔离（《测距路线》§4）；**不得用追踪框充当检测框**；左右目同一目标的 `track_id` 必须一致，否则隔离退化为最近邻兜底（§2.1）；
|
||||
5. **两目同源**：左右框必须来自同一次曝光（同帧契约）；左右取自不同帧、仅凭"时间接近"拼成一对，即使 `frame_id` 相同也属违约——`frame_id` 由采集端保证真实（§2.3）；
|
||||
6. **前端不做左右匹配、不做对齐**：左右匹配、视差取点、方位解算全部在 stereo_ranger；识别端只需保证"每目各自把该目可见目标检测出来"，不得为便于后端匹配而调整框位置（《测距路线》§1.4）。
|
||||
|
||||
---
|
||||
|
||||
## 3. 模块内部建议构成（实现参考）
|
||||
|
||||
| 子模块 | 职责 | 说明 |
|
||||
|---|---|---|
|
||||
| 采集/同步（可选） | 双流按帧标识+时间窗配对，或 sbs 裁切 | 无硬同步时量化残余抖动；帧标识不得重编号（§2.3） |
|
||||
| 几何预处理 | 校正 remap → 左右完全一致的 letterbox | letterbox 参数（缩放 s、填充）必须左右共用一份配置 |
|
||||
| 推理调度 | 左右目各一次检测 | **必须并发**：单设备双流或双实例双线程；串行使时延翻倍（《测距路线》§10.1：检测单核 20~30ms，NPU 预算成立） |
|
||||
| YOLO 推理 + NMS | 输出 640×640（或配置尺寸）坐标系下的框 | 类别表、conf/NMS 阈值外置 |
|
||||
| 坐标回映 | 逆 letterbox 映射到原生分辨率 | 与预处理使用同一套变换参数，禁止各写一份 |
|
||||
| 输出适配 | 出口自检（有限值、`x2>x1`、`y2>y1`）→ 每目封装 `DetectionFrame(frame_id, detections)` | 字段严格按 §2.1；`frame_id` 左右共用 |
|
||||
|
||||
---
|
||||
|
||||
## 4. 配置项需求（全部外置，禁止硬编码）
|
||||
|
||||
| 配置 | 说明 |
|
||||
|---|---|
|
||||
| 模型权重 / 类别表 | 检测器选择（YOLOv5/v8/…）不设限 |
|
||||
| 输入尺寸 | 如 640×640；左右共用 |
|
||||
| conf / NMS IoU 阈值 | 识别端自身过滤口径（测距端不消费检测置信度，§2.4） |
|
||||
| 输出类别过滤 | 可选白名单；同一目标左右目类别必须一致（§2.2 第 5 条） |
|
||||
| 原分辨率与内参 | 逆映射/校验用；`fx_px`/`fy_px`/`cx_px`/`cy_px` 必须与测距端配置**同源同值**（当前占位值见 `utils/config/stereo_ranger.json`，标定回填后同步） |
|
||||
| 并发策略 | 单实例多线程 or 双实例 |
|
||||
| 帧同步方式 | sbs / 双流 + 时间窗 |
|
||||
|
||||
配置一致性要求（§5"一致性"项的落地）：letterbox 缩放与填充参数、内参、原分辨率
|
||||
均由**同一配置源**驱动左右两目与两个模块；任何一处单独调整都会造成视差系统性偏差
|
||||
（《测距路线》§9 全部参数外置、启动期校验的语义同样适用于本模块）。
|
||||
|
||||
---
|
||||
|
||||
## 5. 非功能需求（大致）
|
||||
|
||||
| 项目 | 要求 |
|
||||
|---|---|
|
||||
| 帧率 | 满足测距流水线帧周期（如 30 fps → ≤33 ms/帧） |
|
||||
| 左右推理 | 必须并发执行（《测距路线》§10.1：检测单核 20~30ms，串行会吃掉全部预算） |
|
||||
| 端到端时延 | 实测并留出余量：识别之外，匹配+测距在少量目标时 <1 ms（测距端为纯计算单元、无 IO 无阻塞，《测距路线》§11.3） |
|
||||
| 一致性 | 左右目预处理/回映参数由同一配置源驱动，杜绝两路漂移（§4 配置一致性要求） |
|
||||
| 失败处理 | 单目推理失败等异常不阻塞整体，该目输出空帧列表（测距端退化为另一目的仅方位记录）；错误日志节流（《测距路线》§5：第 1 次 + 每满 100 次，带累计计数） |
|
||||
|
||||
---
|
||||
|
||||
## 6. 验证与验收（大致）
|
||||
|
||||
| 项目 | 方法 | 通过标准（示意） |
|
||||
|---|---|---|
|
||||
| 坐标回映 | 已知缩放/填充的合成图，比对回映框与真值 | 误差 <0.5 px（像素取整前） |
|
||||
| 垂直对齐 | 静态场景，左右匹配框 \|Δy\| 直方图 | P95 ≤1 px（《测距路线》§12.1 校正质量；不达标触发自标定提前） |
|
||||
| 类别一致性 | 静态场景统计同一目标两目类别 | 不一致率 ≈0（不一致会被测距端匹配硬约束拦截，退化为两条仅方位记录） |
|
||||
| 输出合法性 | 出口自检单测（含非有限值、退化框构造用例） | 无 `x2 ≤ x1` / `y2 ≤ y1` / 非有限值框输出（§2.2 第 6 条） |
|
||||
| 输入纯净性 | 代码审查 + 单测：断言送入 `update()` 的框集合恰为本帧检测输出 | 无预测/Coast/平滑框进入（§2.5） |
|
||||
| 下游对接 | 用本模块输出喂 `StereoRanger.update()` | 与 `utils/demo_stereo_ranger.py` 真值投影法结果同量级；误差随距离趋势符合 `δZ ∝ Z²`（《测距路线》§7.1） |
|
||||
| 帧周期 | 双流满负荷计时 | 满足所选帧率预算 |
|
||||
|
||||
---
|
||||
|
||||
## 7. 依赖与边界
|
||||
|
||||
- 依赖：YOLO 推理栈（如 onnxruntime/torch）、opencv（解码/预处理/校正 remap，若选用 numpy 实现 letterbox 亦可）；**不 import `stereo_ranger` 的测距逻辑**（保持前端/测距解耦），仅可复用 `stereo_ranger/types.py` 的纯值类型（可选，见 §2.1）与同一份配置数值（§4）；
|
||||
- 本模块不实现：跨帧跟踪（无追踪器时 `track_id` 留 `None` 可接受）、距离 coast/外推（属消费侧，§2.5 第 3 条）、自标定与立体校正本身、主测距传感器融合；
|
||||
- 若部署平台算力不足（如边缘设备），允许降低输入分辨率或帧率，但左右一致性与原生回映两条约束不可妥协（§2.2 第 1、2 条）；
|
||||
- **红线同源**：本模块输出同样受《测距路线》§12.3 约束——标定完成前内参等数值均为占位值，仅限台架/回放验证，不得接入实飞控制回路。
|
||||
Binary file not shown.
+38
-10
@@ -1,11 +1,16 @@
|
||||
# utils —— reference-c++ 的 Python 移植 + 双目测距新实现
|
||||
# utils —— reference-c++ 的 Python 移植 + 双目链路新实现
|
||||
|
||||
将 `reference-c++/` 下的 `target_tracker`（多目标追踪过滤器）与
|
||||
`visual_track_control`（视觉跟踪控制律）移植为 Python，**两包互不依赖**
|
||||
（唯一的数据转换点在胶水层 demo）。
|
||||
|
||||
另有 `stereo_ranger`（双目稀疏目标测距）：**按规格新实现**（非 C++ 移植），
|
||||
规格 `docs/双目测距技术路线.md`；同样与既有两包互不依赖。
|
||||
另有双目链路新实现（非 C++ 移植、按规格实现）：
|
||||
- `stereo_ranger`（双目稀疏目标测距），规格 `docs/双目测距技术路线.md`；
|
||||
- `stereo_yolo`（双目 YOLO 检测前端），规格 `docs/双目YOLO目标识别需求说明.md`，
|
||||
输出直接作为 stereo_ranger 的输入（仅复用其 `types.py` 纯值类型）。
|
||||
|
||||
Python 侧定位为 **C++ 实现的先验验证**：构件划分与语义按规格冻结，便于逐构件
|
||||
移植对照；`stereo_yolo` 的 C++ 移植注记见其包 README。
|
||||
|
||||
## 目录结构
|
||||
|
||||
@@ -17,22 +22,30 @@ utils/
|
||||
│ └── README.md # 接口清单 + C++ 差异
|
||||
├── stereo_ranger/ # 包 3：双目稀疏目标测距（纯标准库，无第三方依赖）
|
||||
│ └── README.md # 接口清单 + 与规格的偏差
|
||||
├── stereo_yolo/ # 包 4：双目 YOLO 检测前端（真模型需 ultralytics）
|
||||
│ └── README.md # 接口清单 + C++ 移植注记
|
||||
├── demo_track_control.py # 胶水层集成演示（模拟检测 → 追踪 → 控制律 → 发送）
|
||||
├── demo_stereo_ranger.py # 双目测距演示（合成框对，六场景，无硬件）
|
||||
├── demo_stereo_yolo.py # 检测前端演示（真模型 + 合成素材 → 测距联调）
|
||||
├── assets/stereo_yolo/ # demo 占位素材（AI 合成图，可整体替换为真实素材）
|
||||
├── config/track_control.json # 控制律配置（拷贝自 reference-c++）
|
||||
├── config/stereo_ranger.json # 双目测距配置（占位值，待标定回填）
|
||||
├── config/stereo_yolo.json # 检测前端配置（不存内参，几何参数唯一来源见上）
|
||||
├── tests/ # pytest 测试
|
||||
│ ├── target_tracker/ # 28 条（对应 C++ 测试文件）
|
||||
│ ├── visual_track_control/ # 81 条（对应 C++ 测试文件）
|
||||
│ └── stereo_ranger/ # 111 条（规格 §11.4 构件级 + 全流程）
|
||||
├── conftest.py # 保证可从任意工作目录导入三包
|
||||
└── pytest.ini # 收集 *_test.py 命名（与 C++ 测试文件名对齐）
|
||||
│ ├── stereo_ranger/ # 111 条（规格 §11.4 构件级 + 全流程）
|
||||
│ └── stereo_yolo/ # 36 条（假检测器驱动，无模型/素材/硬件）
|
||||
├── conftest.py # 保证可从任意工作目录导入各包
|
||||
└── pytest.ini # 收集 *_test.py 命名（与 C++ 测试文件名对齐）；
|
||||
# importlib 导入模式避免跨包同名测试文件冲突
|
||||
```
|
||||
|
||||
## 环境
|
||||
|
||||
conda env `pytorch`（`D:\program file\conda\envs\pytorch`），Python 3.12，
|
||||
依赖：numpy、mavsdk、pytest（stereo_ranger 仅标准库）。
|
||||
依赖：numpy、mavsdk、pytest；stereo_ranger 仅标准库；stereo_yolo 只有真模型
|
||||
推理路径需要 ultralytics（在检测器内部延迟导入，测试与假检测器路径不受影响）。
|
||||
|
||||
## 运行测试
|
||||
|
||||
@@ -41,9 +54,10 @@ cd utils
|
||||
"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q
|
||||
```
|
||||
|
||||
220 条测试全部通过：109 条 C++ 移植用例（含 §9.4 六条硬验收、A1/A2/A4
|
||||
256 条测试全部通过：109 条 C++ 移植用例（含 §9.4 六条硬验收、A1/A2/A4
|
||||
回归、PID anti-windup C1、超龄 C4、首拍限幅 S6）+ 111 条 stereo_ranger
|
||||
（合成框对驱动，无需真实图像/硬件）。
|
||||
（合成框对驱动）+ 36 条 stereo_yolo（假检测器驱动）。
|
||||
stereo_ranger / stereo_yolo 测试均不需要真实图像、模型权重或硬件。
|
||||
|
||||
## 运行演示
|
||||
|
||||
@@ -56,14 +70,24 @@ python utils/demo_track_control.py --transport mavsdk
|
||||
|
||||
# 双目测距（合成框对，离线，无相机/飞控）
|
||||
python utils/demo_stereo_ranger.py
|
||||
|
||||
# 双目检测前端（真模型推理；需 ultralytics + models/yolo26n-drone.pt）
|
||||
python utils/demo_stereo_yolo.py
|
||||
```
|
||||
|
||||
控制链路时间线 13 s（20 Hz）：正常跟踪 6 s（vx 从零渐变 → 逼近限速 3 m/s）→
|
||||
目标消失 4 s（Coast 预测，60 帧后轨道判亡）→ 判零速刹车 3 s。
|
||||
|
||||
双目演示六场景：逼近（三道有效性门 + 滤波）→ 远场域外（方位仍发布）→
|
||||
双目测距演示六场景：逼近（三道有效性门 + 滤波）→ 远场域外（方位仍发布）→
|
||||
单目退化 → 非同帧拒收 → 一致性降级/判无效 → 多目标贪心配对。
|
||||
|
||||
检测前端演示五场景：单图冒烟 → 合成双目对联调（真检测框喂 stereo_ranger，
|
||||
与解析式 D=F·B/Z 对照）→ 空场景 → 左右形状不一致拒收 → 帧周期计时。
|
||||
合成对用 AI 占位素材 `assets/stereo_yolo/drone_sky.png`（可整体替换为真实
|
||||
素材），本机实测 |ΔD|≈0.3 px、|Δy|≈0.2 px；帧周期 25~34 ms（640 输入，
|
||||
双实例并发，随机器负载波动），与 30 fps 预算 33.3 ms 同量级，结论不可外推
|
||||
到机载平台（§5 的算力预算另见平台工程文档）。
|
||||
|
||||
## 移植约定
|
||||
|
||||
- MAVLink 部分改为 **MAVSDK 语义映射**（offboard API，非字节透传），三分支
|
||||
@@ -74,3 +98,7 @@ python utils/demo_stereo_ranger.py
|
||||
- `stereo_ranger` 不是移植：按规格五步流程新实现，与规格的偏差（越界=有效性门、
|
||||
视差域权威、补充 consistency_jump_px/isolation_gate_px 等）见其包 README；
|
||||
⚠ 全部数值为占位值，标定完成前不得作为飞行控制决策依据。
|
||||
- `stereo_yolo` 不是移植：推理口径为 ultralytics 直跑 `.pt`，letterbox、解码、
|
||||
NMS、坐标回映均在库内完成；C++ 侧复现要点与接口契约硬约束（原生分辨率坐标、
|
||||
左右预处理位级一致、`x1` 即框左边缘、无跨帧状态）见其包 README 的移植注记表。
|
||||
⚠ 占位素材 `assets/stereo_yolo/` 为 AI 合成图，真实双目素材到位后需重跑联调。
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
After Width: | Height: | Size: 821 KiB |
@@ -0,0 +1,19 @@
|
||||
{
|
||||
"fx_px": 960.0,
|
||||
"fy_px": 960.0,
|
||||
"cx_px": 960.0,
|
||||
"cy_px": 540.0,
|
||||
"baseline_m": 0.06,
|
||||
"tau_y_px": 2.0,
|
||||
"tau_h_ratio": 0.25,
|
||||
"disparity_min_px": 3.0,
|
||||
"disparity_max_px": 57.6,
|
||||
"max_delta_z_m": 2.0,
|
||||
"consistency_N_frames": 3,
|
||||
"consistency_M_frames": 5,
|
||||
"consistency_jump_px": 3.0,
|
||||
"smooth_alpha": 0.3,
|
||||
"jump_reset_m": 5.0,
|
||||
"filter_reset_frames": 5,
|
||||
"isolation_gate_px": 50.0
|
||||
}
|
||||
@@ -0,0 +1,9 @@
|
||||
{
|
||||
"model_path": "models/yolo26n-drone.pt",
|
||||
"imgsz": 640,
|
||||
"conf_threshold": 0.25,
|
||||
"iou_threshold": 0.7,
|
||||
"class_whitelist": null,
|
||||
"device": null,
|
||||
"concurrency": "parallel"
|
||||
}
|
||||
@@ -0,0 +1,269 @@
|
||||
"""demo_stereo_ranger.py —— 双目稀疏目标测距演示（规格 docs/双目测距技术路线.md）。
|
||||
|
||||
全部场景用**解析构造**的左右目检测框驱动：不需要相机、不需要图像、不需要
|
||||
飞控，几何是闭式可验证的（规格 §11.4）。覆盖：
|
||||
|
||||
① 逼近：三条有效性门依次生效（域外 → 预算门 → 有效）+ 时间滤波平滑
|
||||
② 远场域外：valid = false 但 c_mid（虚拟双目中心方位）照常发布
|
||||
③ 单目退化：仅一目检出 → 仅方位记录
|
||||
④ 非同帧拒收：左右帧标识不一致 → 整帧拒收 + 告警节流
|
||||
⑤ 一致性监控：持续视差跳变 → N 帧降置信度 → M 帧判无效
|
||||
⑥ 多目标：贪心一一配对，各自独立状态；未匹配右目退化为仅方位
|
||||
|
||||
用法：
|
||||
python utils/demo_stereo_ranger.py # 默认配置
|
||||
python utils/demo_stereo_ranger.py --config <path> # 指定配置
|
||||
|
||||
⚠ 所有参数均为占位值（焦距/标定待厂商数据）：标定与地面真值验证完成前，
|
||||
本模块输出仅限静态标靶/回放/台架验证，不得作为飞行控制决策依据
|
||||
（规格 §12.3 红线）。
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import logging
|
||||
from pathlib import Path
|
||||
from typing import List, Optional, Tuple
|
||||
|
||||
from stereo_ranger import (
|
||||
Detection,
|
||||
DetectionFrame,
|
||||
RangeRecord,
|
||||
StereoConfig,
|
||||
StereoRanger,
|
||||
load_from_json,
|
||||
)
|
||||
|
||||
logger = logging.getLogger("demo_stereo_ranger")
|
||||
|
||||
_DEMO_DIR = Path(__file__).resolve().parent
|
||||
_DEFAULT_CONFIG = _DEMO_DIR / "config" / "stereo_ranger.json"
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 合成框对
|
||||
|
||||
|
||||
def boxes_for(
|
||||
cfg: StereoConfig,
|
||||
x_center_m: float,
|
||||
y_m: float,
|
||||
z_m: float,
|
||||
*,
|
||||
w_m: float = 0.5,
|
||||
h_m: float = 0.4,
|
||||
class_id: int = 0,
|
||||
track_id: Optional[int] = None,
|
||||
confidence: float = 0.95,
|
||||
) -> Tuple[Detection, Detection]:
|
||||
"""模组中心系 (X_C, Y, Z) 的理想目标 → (左目框, 右目框)。
|
||||
|
||||
两目框同宽，因此边缘视差 = 中心视差 = F·B/Z（§3.2/§6.6 的精确投影）。
|
||||
"""
|
||||
x_l = cfg.cx_px + cfg.fx_px * (x_center_m + cfg.baseline_m / 2.0) / z_m
|
||||
x_r = cfg.cx_px + cfg.fx_px * (x_center_m - cfg.baseline_m / 2.0) / z_m
|
||||
y = cfg.cy_px + cfg.fy_px * y_m / z_m
|
||||
w_px = cfg.fx_px * w_m / z_m
|
||||
h_px = cfg.fy_px * h_m / z_m
|
||||
|
||||
def _box(cx_px: float) -> Detection:
|
||||
return Detection(
|
||||
cx_px - w_px / 2.0,
|
||||
y - h_px / 2.0,
|
||||
cx_px + w_px / 2.0,
|
||||
y + h_px / 2.0,
|
||||
confidence=confidence,
|
||||
class_id=class_id,
|
||||
track_id=track_id,
|
||||
)
|
||||
|
||||
return _box(x_l), _box(x_r)
|
||||
|
||||
|
||||
def pair_with_disparity(d_px: float, *, y_center: float = 500.0) -> Tuple[Detection, Detection]:
|
||||
"""构造视差恰为 d_px 的等宽框对（d 取 0.5 的整数倍时浮点精确）。"""
|
||||
width, height = 60.0, 40.0
|
||||
left = Detection(1000.0 - width / 2, y_center - height / 2, 1000.0 + width / 2, y_center + height / 2)
|
||||
right = Detection(
|
||||
1000.0 - d_px - width / 2,
|
||||
y_center - height / 2,
|
||||
1000.0 - d_px + width / 2,
|
||||
y_center + height / 2,
|
||||
)
|
||||
return left, right
|
||||
|
||||
|
||||
def frame(frame_id: int, detections: List[Detection]) -> DetectionFrame:
|
||||
return DetectionFrame(frame_id=frame_id, detections=detections)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 打印
|
||||
|
||||
|
||||
def describe(record: RangeRecord) -> str:
|
||||
z_text = f"{record.forward_distance_m:7.3f}" if record.forward_distance_m is not None else " — "
|
||||
r_text = f"{record.slant_range_m:7.3f}" if record.slant_range_m is not None else " — "
|
||||
d_text = f"{record.disparity_px:7.2f}" if record.disparity_px is not None else " — "
|
||||
return (
|
||||
f"[F{record.frame_id:02d}] {record.degradation.name:<12} valid={int(record.valid)} "
|
||||
f"Z={z_text} m R={r_text} m D={d_text} px "
|
||||
f"c_mid=({record.center_mid_px[0]:7.1f},{record.center_mid_px[1]:6.1f}) "
|
||||
f"conf={record.confidence:.2f}"
|
||||
)
|
||||
|
||||
|
||||
def log_scene(title: str) -> None:
|
||||
logger.info("")
|
||||
logger.info("── %s", title)
|
||||
|
||||
|
||||
def log_config_summary(cfg: StereoConfig) -> None:
|
||||
logger.info("配置：B=%.3f m F=%.1f px (F·B=%.2f px·m)", cfg.baseline_m, cfg.fx_px, cfg.focal_baseline_px_m)
|
||||
logger.info(
|
||||
" 视差域 [%.1f, %.1f] px ⇔ 距离域 [%.2f, %.2f] m（视差域权威）",
|
||||
cfg.disparity_min_px,
|
||||
cfg.disparity_max_px,
|
||||
cfg.z_min_m,
|
||||
cfg.z_max_m,
|
||||
)
|
||||
logger.info(
|
||||
" 误差预算 δZ(δD=1px) ≤ %.1f m ⇒ 有效远界 %.2f m；一致性 N=%d / M=%d（跳变门限 %.1f px）",
|
||||
cfg.max_delta_z_m,
|
||||
cfg.z_error_budget_max_m,
|
||||
cfg.consistency_N_frames,
|
||||
cfg.consistency_M_frames,
|
||||
cfg.consistency_jump_px,
|
||||
)
|
||||
logger.info(
|
||||
" 滤波 α=%.2f（≈%.0f 帧群延迟）跳变重置 %.1f m；目标隔离门限 %.0f px",
|
||||
cfg.smooth_alpha,
|
||||
1.0 / cfg.smooth_alpha,
|
||||
cfg.jump_reset_m,
|
||||
cfg.isolation_gate_px,
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 场景
|
||||
|
||||
|
||||
def scene_approach(ranger: StereoRanger, cfg: StereoConfig) -> None:
|
||||
"""① 逼近：Z 20 → 6 m，三条有效性门依次生效，有效帧起低通平滑。"""
|
||||
ranger.reset() # 场景独立：清空跨场景的滤波/一致性/隔离状态
|
||||
log_scene("① 逼近场景：目标沿光轴 20 → 6 m（1 m/帧），观察三道门与滤波")
|
||||
logger.info(" %-7s %-9s %-6s %-7s %s", "真值Z", "D(px)", "valid", "输出Z", "说明")
|
||||
user_ms = 1000
|
||||
for index, z_true in enumerate([20.0, 18.0, 16.0, 14.0, 12.0, 11.0, 10.0, 9.0, 8.0, 7.0, 6.0], start=1):
|
||||
left, right = boxes_for(cfg, 0.0, 0.0, z_true)
|
||||
record = ranger.update(frame(index, [left]), frame(index, [right]), now_ms=user_ms)[0]
|
||||
user_ms += 50
|
||||
if record.disparity_px is not None and record.disparity_px < cfg.disparity_min_px:
|
||||
note = "门① 视差域外 → invalid"
|
||||
elif not record.valid and record.disparity_px is not None:
|
||||
note = "门② 误差预算不满足 → invalid"
|
||||
elif record.forward_distance_m is not None and abs(record.forward_distance_m - z_true) > 1e-9:
|
||||
note = f"有效（滤波后，真值 {z_true:.2f}）"
|
||||
else:
|
||||
note = "有效（滤波器初始化）"
|
||||
logger.info(
|
||||
" %-7.2f %-9.2f %-6d %-7s %s",
|
||||
z_true,
|
||||
record.disparity_px if record.disparity_px is not None else float("nan"),
|
||||
int(record.valid),
|
||||
f"{record.forward_distance_m:.3f}" if record.forward_distance_m is not None else "—",
|
||||
note,
|
||||
)
|
||||
logger.info(
|
||||
" → 远场：域外或 δZ 超预算时诚实输出 invalid（不强行估计）；"
|
||||
"有效帧之间才做一阶低通，输出滞后于真值属预期（消费侧 PID 与限幅吸收）"
|
||||
)
|
||||
|
||||
|
||||
def scene_far_field_bearing(ranger: StereoRanger, cfg: StereoConfig) -> None:
|
||||
"""② 远场域外：无距离但方位照常发布（远场定速接近策略的输入基础）。"""
|
||||
ranger.reset()
|
||||
log_scene("② 远场域外：Z=25 m（D < 下限），valid=false 但 c_mid 仍发布")
|
||||
x_center_m = 0.6
|
||||
left, right = boxes_for(cfg, x_center_m, 0.0, 25.0)
|
||||
record = ranger.update(frame(20, [left]), frame(20, [right]), now_ms=2000)[0]
|
||||
logger.info(" %s", describe(record))
|
||||
logger.info(
|
||||
" c_mid.x=%.1f = cx + F·X_C/Z = %.1f（虚拟双目中心，无近场视差偏差）",
|
||||
record.center_mid_px[0],
|
||||
cfg.cx_px + cfg.fx_px * x_center_m / 25.0,
|
||||
)
|
||||
logger.info(" → 消费侧据此走\"方位闭环 + 定速接近\"（D4），而不是使用不可靠的远场估计")
|
||||
|
||||
|
||||
def scene_monocular_degradation(ranger: StereoRanger, cfg: StereoConfig) -> None:
|
||||
"""③ 单目退化：仅一目检出 → 仅方位记录（v1.0 接受近场视差偏差）。"""
|
||||
ranger.reset()
|
||||
log_scene("③ 单目退化：仅左目检出")
|
||||
left, _ = boxes_for(cfg, 0.3, 0.0, 4.0, confidence=0.88)
|
||||
records = ranger.update(frame(21, [left]), frame(21, []), now_ms=3000)
|
||||
logger.info(" %s", describe(records[0]))
|
||||
logger.info(" → 退化 = 仅方位：报该目框中心作方位，valid=false 无距离（§2.2 记录两态）")
|
||||
|
||||
|
||||
def scene_frame_mismatch(ranger: StereoRanger) -> None:
|
||||
"""④ 非同帧拒收：左右帧标识不一致 → 整帧拒收（WARN 节流）。"""
|
||||
ranger.reset()
|
||||
log_scene("④ 同帧契约：左右帧标识不一致（F30 vs F31）")
|
||||
left_pair = pair_with_disparity(9.6)
|
||||
for _ in range(2):
|
||||
records = ranger.update(frame(30, [left_pair[0]]), frame(31, [left_pair[1]]))
|
||||
logger.info(" 两次违约共输出记录 %d 条（应为 0）；告警按节流规范只在第 1 次打印", len(records))
|
||||
logger.info(" → 不同曝光时刻的框配对解算没有物理意义，宁可拒收不可给值（§2.1）")
|
||||
|
||||
|
||||
def scene_consistency(ranger: StereoRanger) -> None:
|
||||
"""⑤ 一致性监控：持续视差跳变 → N 帧降置信度 → M 帧判无效。"""
|
||||
ranger.reset()
|
||||
log_scene("⑤ 一致性监控：视差逐帧跳变 4 px（门限 3 px）")
|
||||
for index, d_px in enumerate([9.5, 13.5, 17.5, 21.5, 25.5, 29.5], start=40):
|
||||
left, right = pair_with_disparity(d_px)
|
||||
record = ranger.update(frame(index, [left]), frame(index, [right]), now_ms=4000)[0]
|
||||
logger.info(" %s", describe(record))
|
||||
logger.info(" → 只能降级、不能纠偏：连续异常帧数达 N 降置信度、达 M 判无效（§6.7）")
|
||||
|
||||
|
||||
def scene_multi_target(ranger: StereoRanger, cfg: StereoConfig) -> None:
|
||||
"""⑥ 多目标：贪心一一配对；未配对的右目框退化为仅方位记录。"""
|
||||
ranger.reset()
|
||||
log_scene("⑥ 多目标：两个同类别目标 + 一个孤立右目框")
|
||||
left_a, right_a = boxes_for(cfg, -1.0, 0.0, 8.0)
|
||||
left_b, right_b = boxes_for(cfg, 1.2, 0.0, 9.0)
|
||||
extra_right = Detection(360.0, 480.0, 420.0, 540.0)
|
||||
records = ranger.update(frame(50, [left_a, left_b]), frame(50, [right_a, right_b, extra_right]), now_ms=5000)
|
||||
for record in records:
|
||||
logger.info(" %s", describe(record))
|
||||
logger.info(" → 目标数极少（1~2）时 O(n²) 贪心与全局最优几乎必然同解；匹配器接口可替换（§10.3）")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 入口
|
||||
|
||||
|
||||
def run_demo(config_path: str) -> None:
|
||||
cfg = load_from_json(config_path)
|
||||
logger.info("配置来源：%s", config_path)
|
||||
log_config_summary(cfg)
|
||||
logger.warning("⚠ 占位参数未标定：仅限静态标靶/回放/台架验证，不得作为飞行控制决策依据（§12.3）")
|
||||
|
||||
ranger = StereoRanger(cfg) # 演示共用一个实例，状态跨场景延续（逼近 → 后续场景）
|
||||
scene_approach(ranger, cfg)
|
||||
scene_far_field_bearing(ranger, cfg)
|
||||
scene_monocular_degradation(ranger, cfg)
|
||||
scene_frame_mismatch(ranger)
|
||||
scene_consistency(ranger)
|
||||
scene_multi_target(ranger, cfg)
|
||||
logger.info("")
|
||||
logger.info("演示结束：以上全部为合成框对的解析结果，无真实图像与硬件参与")
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description="双目稀疏目标测距演示（合成框对，离线）")
|
||||
parser.add_argument("--config", default=str(_DEFAULT_CONFIG), help="配置 JSON 路径")
|
||||
args = parser.parse_args()
|
||||
logging.basicConfig(level=logging.INFO, format="%(levelname)s %(message)s")
|
||||
run_demo(args.config)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,286 @@
|
||||
"""demo_stereo_yolo.py —— 双目 YOLO 检测前端演示（规格 docs/双目YOLO目标识别需求说明.md）。
|
||||
|
||||
用真实模型（models/yolo26n-drone.pt）与一张占位素材（utils/assets/stereo_yolo/
|
||||
drone_sky.png，AI 合成图、非真实飞行数据）驱动，覆盖：
|
||||
|
||||
① 单图检测冒烟：真实推理 → 原生坐标框 + 出口自检（§2.2）
|
||||
② 合成双目对（已知视差）：同一无人机 patch 按已知视差贴到左右图 →
|
||||
真检测框喂 StereoRanger.update() → 与解析真值 Z=F·B/D 对比（§6 下游对接）
|
||||
③ 空场景：无目标 → 两目空帧 → 测距端返回空列表（正常空帧，不告警）
|
||||
④ 形状不一致：左右图分辨率不同 → 整帧拒收输出空（§2.2 第 2 条）
|
||||
⑤ 帧周期：双流满负荷计时（concurrency=parallel，§5 并发要求）
|
||||
|
||||
用法（从仓库根运行）：
|
||||
python utils/demo_stereo_yolo.py
|
||||
python utils/demo_stereo_yolo.py --frames 20
|
||||
|
||||
⚠ 占位参数 + 合成素材：仅限台架/回放验证，不得接入实飞控制回路（§7 红线）。
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import logging
|
||||
import time
|
||||
from dataclasses import replace
|
||||
from pathlib import Path
|
||||
from typing import List, Optional, Tuple
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
from stereo_ranger import (
|
||||
Detection,
|
||||
DetectionFrame,
|
||||
RangeRecord,
|
||||
StereoConfig,
|
||||
StereoRanger,
|
||||
load_from_json as load_ranger_config,
|
||||
)
|
||||
from stereo_yolo import StereoYoloConfig, StereoYoloFrontend, load_from_json
|
||||
|
||||
logger = logging.getLogger("demo_stereo_yolo")
|
||||
|
||||
_DEMO_DIR = Path(__file__).resolve().parent # utils/
|
||||
_REPO_ROOT = _DEMO_DIR.parent
|
||||
_DEFAULT_CONFIG = _DEMO_DIR / "config" / "stereo_yolo.json"
|
||||
_RANGER_CONFIG = _DEMO_DIR / "config" / "stereo_ranger.json"
|
||||
_ASSET = _DEMO_DIR / "assets" / "stereo_yolo" / "drone_sky.png"
|
||||
|
||||
# 合成双目对参数：粘贴同一 patch 的整数水平偏移 = 精确视差
|
||||
_DISPARITY_PX = 12 # ⇔ Z ≈ 4.8 m（F·B=57.6 px·m，域内）
|
||||
_LEFT_CENTER_X = 1200
|
||||
_PATCH_Y_CENTER = 560
|
||||
_CANVAS = (1920, 1080)
|
||||
_FEATHER_PX = 32 # 羽化混合宽度：硬边接缝会被检测器误当目标边缘（实测数 px 框偏移）
|
||||
_RANGER_NOW_MS = 1000
|
||||
|
||||
|
||||
def log_scene(title: str) -> None:
|
||||
logger.info("")
|
||||
logger.info("── %s", title)
|
||||
|
||||
|
||||
def describe(record: RangeRecord) -> str:
|
||||
z_text = f"{record.forward_distance_m:7.3f}" if record.forward_distance_m is not None else " — "
|
||||
d_text = f"{record.disparity_px:7.2f}" if record.disparity_px is not None else " — "
|
||||
return (
|
||||
f"[F{record.frame_id:02d}] {record.degradation.name:<12} valid={int(record.valid)} "
|
||||
f"Z={z_text} m D={d_text} px "
|
||||
f"c_mid=({record.center_mid_px[0]:7.1f},{record.center_mid_px[1]:6.1f}) "
|
||||
f"conf={record.confidence:.2f}"
|
||||
)
|
||||
|
||||
|
||||
def log_frame(eye: str, frame: DetectionFrame) -> None:
|
||||
if not frame.detections:
|
||||
logger.info(" %s：无检出", eye)
|
||||
return
|
||||
for detection in frame.detections:
|
||||
logger.info(
|
||||
" %s：框=(%7.1f,%7.1f,%7.1f,%7.1f) conf=%.3f class=%d",
|
||||
eye,
|
||||
detection.x1,
|
||||
detection.y1,
|
||||
detection.x2,
|
||||
detection.y2,
|
||||
detection.confidence,
|
||||
detection.class_id,
|
||||
)
|
||||
|
||||
|
||||
def _best_box(frame: DetectionFrame) -> Optional[Detection]:
|
||||
if not frame.detections:
|
||||
return None
|
||||
return max(frame.detections, key=lambda detection: detection.confidence)
|
||||
|
||||
|
||||
def _crop_patch(asset: np.ndarray, box: Detection, *, margin_x: float = 0.3, margin_y: float = 0.5) -> np.ndarray:
|
||||
width, height = box.width, box.height
|
||||
x1 = max(0, int(box.x1 - margin_x * width))
|
||||
x2 = min(asset.shape[1], int(box.x2 + margin_x * width))
|
||||
y1 = max(0, int(box.y1 - margin_y * height))
|
||||
y2 = min(asset.shape[0], int(box.y2 + margin_y * height))
|
||||
return asset[y1:y2, x1:x2].copy()
|
||||
|
||||
|
||||
def _sky_canvas(asset: np.ndarray) -> np.ndarray:
|
||||
"""用素材顶部天空条拉伸成画布，避免 patch 与背景出现色差硬边。"""
|
||||
strip = asset[0 : max(8, asset.shape[0] // 12), :, :]
|
||||
return cv2.resize(strip, (_CANVAS[0], _CANVAS[1]))
|
||||
|
||||
|
||||
def _paste_blended(canvas: np.ndarray, patch: np.ndarray, center_x: int, center_y: int) -> None:
|
||||
"""羽化混合粘贴：硬边接缝会被检测器当作目标边缘，造成数 px 的框偏移。"""
|
||||
height, width = patch.shape[:2]
|
||||
alpha = np.zeros((height, width), dtype=np.float32)
|
||||
alpha[_FEATHER_PX : height - _FEATHER_PX, _FEATHER_PX : width - _FEATHER_PX] = 1.0
|
||||
alpha = cv2.GaussianBlur(alpha, (0, 0), _FEATHER_PX * 0.5)[..., None]
|
||||
x0, y0 = int(center_x - width / 2), int(center_y - height / 2)
|
||||
region = canvas[y0 : y0 + height, x0 : x0 + width]
|
||||
canvas[y0 : y0 + height, x0 : x0 + width] = (patch * alpha + region * (1.0 - alpha)).astype(np.uint8)
|
||||
|
||||
|
||||
def build_synthetic_pair(frontend: StereoYoloFrontend, asset: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
|
||||
"""素材上检出无人机 → 取 patch → 同一 patch 按已知整数视差贴到左右天空画布。"""
|
||||
frame, _ = frontend.detect(asset, asset, frame_id=0)
|
||||
box = _best_box(frame)
|
||||
if box is None:
|
||||
raise RuntimeError("占位素材未检出无人机，请更换 utils/assets/stereo_yolo/drone_sky.png")
|
||||
patch = _crop_patch(asset, box)
|
||||
left_img, right_img = _sky_canvas(asset), _sky_canvas(asset)
|
||||
_paste_blended(left_img, patch, _LEFT_CENTER_X, _PATCH_Y_CENTER)
|
||||
_paste_blended(right_img, patch, _LEFT_CENTER_X - _DISPARITY_PX, _PATCH_Y_CENTER)
|
||||
return left_img, right_img
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 场景
|
||||
|
||||
|
||||
def scene_single_image(frontend: StereoYoloFrontend, asset: np.ndarray) -> None:
|
||||
log_scene("① 单图检测冒烟：真实模型 + 占位素材（同一图喂左右目，仅看输出契约）")
|
||||
left_frame, right_frame = frontend.detect(asset, asset, frame_id=1)
|
||||
log_frame("左目", left_frame)
|
||||
log_frame("右目", right_frame)
|
||||
logger.info(
|
||||
" → 框位于输入图原生坐标系（%d×%d）；出口自检/白名单已在识别端完成（§2.2 第 6 条）",
|
||||
asset.shape[1],
|
||||
asset.shape[0],
|
||||
)
|
||||
|
||||
|
||||
def scene_synthetic_stereo(
|
||||
frontend: StereoYoloFrontend,
|
||||
ranger: StereoRanger,
|
||||
ranger_cfg: StereoConfig,
|
||||
asset: np.ndarray,
|
||||
) -> Tuple[np.ndarray, np.ndarray]:
|
||||
log_scene(f"② 合成双目对（粘贴真值视差 {_DISPARITY_PX} px）→ StereoRanger.update()")
|
||||
left_img, right_img = build_synthetic_pair(frontend, asset)
|
||||
left_frame, right_frame = frontend.detect(left_img, right_img, frame_id=2)
|
||||
log_frame("左目", left_frame)
|
||||
log_frame("右目", right_frame)
|
||||
|
||||
left_box, right_box = _best_box(left_frame), _best_box(right_frame)
|
||||
if left_box is not None and right_box is not None:
|
||||
disparity = left_box.x1 - right_box.x1
|
||||
vertical_residual = abs(left_box.y1 - right_box.y1)
|
||||
logger.info(
|
||||
" 实测边缘视差 D=x1_L−x1_R=%.2f px（真值 %d px）；|Δy|=%.2f px",
|
||||
disparity,
|
||||
_DISPARITY_PX,
|
||||
vertical_residual,
|
||||
)
|
||||
logger.info(
|
||||
" 解析对照 Z=F·B/D=%.3f m（F·B=%.2f px·m）",
|
||||
ranger_cfg.focal_baseline_px_m / disparity,
|
||||
ranger_cfg.focal_baseline_px_m,
|
||||
)
|
||||
|
||||
records = ranger.update(left_frame, right_frame, now_ms=_RANGER_NOW_MS)
|
||||
for record in records:
|
||||
logger.info(" %s", describe(record))
|
||||
if left_box is not None and right_box is not None:
|
||||
logger.info(
|
||||
" → 误差来源：检测框边缘定位抖动（本合成对 |ΔD|=%.2f px），小视差下被 Z=F·B/D 放大（§7.1）",
|
||||
abs(disparity - _DISPARITY_PX),
|
||||
)
|
||||
logger.info(" → 与真值投影法（demo_stereo_ranger）同量级；真值验证待真实双目素材回填（§6）")
|
||||
return left_img, right_img
|
||||
|
||||
|
||||
def scene_empty(frontend: StereoYoloFrontend, ranger: StereoRanger, sky: np.ndarray) -> None:
|
||||
log_scene("③ 空场景：无目标 → 两目空帧 → 测距端空列表（正常空帧，不告警）")
|
||||
left_frame, right_frame = frontend.detect(sky, sky, frame_id=3)
|
||||
records = ranger.update(left_frame, right_frame, now_ms=2000)
|
||||
logger.info(
|
||||
" 两目检出数：%d / %d；测距记录数：%d（应为 0）",
|
||||
len(left_frame.detections),
|
||||
len(right_frame.detections),
|
||||
len(records),
|
||||
)
|
||||
logger.info(" → 无检测即无输出；\"目标丢失就该没有距离\"（§2.5 第 3 条）")
|
||||
|
||||
|
||||
def scene_shape_mismatch(frontend: StereoYoloFrontend, sky: np.ndarray) -> None:
|
||||
log_scene("④ 形状不一致拒收：左 1920×1080 vs 右 1280×720")
|
||||
resized = cv2.resize(sky, (1280, 720))
|
||||
left_frame, right_frame = frontend.detect(sky, resized, frame_id=4)
|
||||
logger.info(" 左右检出数：%d / %d（应为 0 / 0，WARN 节流打印）", len(left_frame.detections), len(right_frame.detections))
|
||||
logger.info(" → 禁止左右各自缩放/填充，否则破坏校正后的极线对齐（§2.2 第 2 条）")
|
||||
|
||||
|
||||
def scene_timing(frontend: StereoYoloFrontend, left_img: np.ndarray, right_img: np.ndarray, frames: int) -> None:
|
||||
log_scene(f"⑤ 帧周期：双流满负荷 {frames} 帧（concurrency=parallel）")
|
||||
for index in range(2): # 预热
|
||||
frontend.detect(left_img, right_img, frame_id=100 + index)
|
||||
started = time.perf_counter()
|
||||
for index in range(frames):
|
||||
frontend.detect(left_img, right_img, frame_id=200 + index)
|
||||
per_frame_ms = (time.perf_counter() - started) / frames * 1000.0
|
||||
logger.info(" 平均 %.1f ms/帧（%.1f fps）；30 fps 预算 33.3 ms/帧（§5）", per_frame_ms, 1000.0 / per_frame_ms)
|
||||
logger.info(" → 本值为台架开发机实测（device 配置 None = 自动选择）；平台算力预算另见平台工程文档")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 入口
|
||||
|
||||
|
||||
def _rebase_model_path(cfg: StereoYoloConfig) -> StereoYoloConfig:
|
||||
"""demo 便利：相对 model_path 按仓库根解析（库内规则是启动工作目录）。"""
|
||||
path = Path(cfg.model_path).expanduser()
|
||||
if not path.is_absolute() and not path.is_file():
|
||||
candidate = _REPO_ROOT / path
|
||||
if candidate.is_file():
|
||||
return replace(cfg, model_path=str(candidate))
|
||||
return cfg
|
||||
|
||||
|
||||
def run_demo(config_path: str, frames: int) -> None:
|
||||
cfg = _rebase_model_path(load_from_json(config_path))
|
||||
ranger_cfg = load_ranger_config(str(_RANGER_CONFIG))
|
||||
logger.info("配置来源：%s", config_path)
|
||||
logger.info(
|
||||
"检测前端：model=%s imgsz=%d conf=%.2f iou=%.2f whitelist=%s device=%s concurrency=%s",
|
||||
Path(cfg.model_path).name,
|
||||
cfg.imgsz,
|
||||
cfg.conf_threshold,
|
||||
cfg.iou_threshold,
|
||||
cfg.class_whitelist,
|
||||
cfg.device,
|
||||
cfg.concurrency,
|
||||
)
|
||||
logger.info(
|
||||
"测距几何（唯一来源 utils/config/stereo_ranger.json）：F=%.1f px B=%.3f m F·B=%.2f px·m",
|
||||
ranger_cfg.fx_px,
|
||||
ranger_cfg.baseline_m,
|
||||
ranger_cfg.focal_baseline_px_m,
|
||||
)
|
||||
logger.warning("⚠ 占位参数与合成素材：仅限台架/回放验证，不得接入实飞控制回路（§7 红线同源）")
|
||||
|
||||
asset = cv2.imread(str(_ASSET))
|
||||
if asset is None:
|
||||
raise FileNotFoundError(f"占位素材缺失: {_ASSET}")
|
||||
|
||||
frontend = StereoYoloFrontend(cfg)
|
||||
ranger = StereoRanger(ranger_cfg)
|
||||
try:
|
||||
scene_single_image(frontend, asset)
|
||||
left_img, right_img = scene_synthetic_stereo(frontend, ranger, ranger_cfg, asset)
|
||||
scene_empty(frontend, ranger, _sky_canvas(asset))
|
||||
scene_shape_mismatch(frontend, _sky_canvas(asset))
|
||||
scene_timing(frontend, left_img, right_img, frames)
|
||||
finally:
|
||||
frontend.close()
|
||||
logger.info("")
|
||||
logger.info("演示结束：素材为 AI 生成合成图（替换 utils/assets/stereo_yolo/drone_sky.png 可换真实素材）")
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description="双目 YOLO 检测前端演示（真实模型 + 占位素材）")
|
||||
parser.add_argument("--config", default=str(_DEFAULT_CONFIG), help="检测前端配置 JSON 路径")
|
||||
parser.add_argument("--frames", type=int, default=10, help="帧周期场景的测量帧数")
|
||||
args = parser.parse_args()
|
||||
logging.basicConfig(level=logging.INFO, format="%(levelname)s %(message)s")
|
||||
run_demo(args.config, args.frames)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,3 +1,6 @@
|
||||
[pytest]
|
||||
testpaths = tests
|
||||
python_files = *_test.py test_*.py
|
||||
; 各包测试文件名与 C++ 测试文件对齐，跨包存在同名（如 config_test.py）：
|
||||
; importlib 模式使 pytest 不再按裸模块名导入，避免 basename 冲突。
|
||||
addopts = --import-mode=importlib
|
||||
|
||||
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,109 @@
|
||||
# stereo_yolo（Python 版）
|
||||
|
||||
双目 YOLO 检测前端：在左右目图像上**独立运行** YOLO 检测器，输出符合
|
||||
`stereo_ranger` 输入契约的检测框（每目一个 `DetectionFrame`、共用 frame_id）。
|
||||
不做测距、左右目关联与立体校正。
|
||||
|
||||
规格：`docs/双目YOLO目标识别需求说明.md`（接口契约以其为准；几何语义权威
|
||||
`docs/双目测距技术路线.md`）。
|
||||
|
||||
⚠ 全部参数为占位值、demo 素材为 AI 合成图：标定与地面真值验证完成前，
|
||||
仅限台架/回放验证，不得接入实飞控制回路（规格 §7 红线同源）。
|
||||
|
||||
## 接口清单
|
||||
|
||||
| 名称 | 说明 |
|
||||
| --- | --- |
|
||||
| `StereoYoloFrontend(config, detector=None)` | 主入口；`detector` 可注入（接口 `detect(img) -> list[Detection]`，单测用假检测器） |
|
||||
| `.detect(left_img, right_img, frame_id) -> (DetectionFrame, DetectionFrame)` | 单帧检测；BGR ndarray（cv2 约定），输出框位于输入图原生坐标系 |
|
||||
| `.close()` | 释放并发线程池 |
|
||||
| `.config` | 只读配置访问 |
|
||||
| `StereoYoloConfig` / `load_from_json(path)` | 7 项配置；缺项/未知字段/类型错误/非法取值一律 `ValueError`（fail-fast） |
|
||||
| `UltralyticsDetector` | ultralytics YOLO `.pt` 后端（双实例池；`ultralytics` 延迟导入） |
|
||||
| `split_sbs(image) -> (left, right)` | side-by-side 单流裁切（宽须为偶数） |
|
||||
| `Detection` / `DetectionFrame` | 直接复用 `stereo_ranger.types`（纯值语义，§2.1 推荐） |
|
||||
| `WarnThrottle` | 告警节流（第 1 次 + 每满 100 次，带累计计数） |
|
||||
|
||||
`logger` 名称为 `stereo_yolo`，走标准 `logging`。
|
||||
|
||||
## 单帧流程（`detect` 四步）
|
||||
|
||||
1. **形状一致性**：左右图形状不一致 → 整帧拒收输出空 + WARN 节流
|
||||
（§2.2 第 2 条：禁止左右各自缩放/填充）；
|
||||
2. **左右推理**：`parallel`（默认，ThreadPoolExecutor(2)，§5 硬性要求）或
|
||||
`sequential`（基准对照）；单目异常被隔离，该目输出空帧列表（§5 失败处理）；
|
||||
3. **出口自检 + 白名单**：丢弃非有限值 / `x2<=x1` / `y2<=y1` 的框
|
||||
（§2.2 第 6 条）；`class_whitelist` 非空时过滤类别；
|
||||
4. **封装**：每目一个 `DetectionFrame(frame_id, detections)`，`frame_id`
|
||||
左右共用（§2.3 同帧契约）。
|
||||
|
||||
## 实现决策（v1.0 冻结）
|
||||
|
||||
1. **ultralytics 直跑 `.pt`**（项目拍板口径）：letterbox、输出解码、NMS、
|
||||
框映射回原生坐标系全部由库内完成；识别端只负责阈值外置、出口自检与
|
||||
契约封装。
|
||||
2. **双实例池**：ultralytics 单实例非线程安全，左右并发各持一份模型
|
||||
（§4 并发策略的"双实例"形态）。
|
||||
3. **无跨帧状态**：前端不持有任何历史（无追踪、无平滑、无 Coast）——
|
||||
结构性满足"只送本帧原生实测框"（§2.5 第 1 条）。
|
||||
4. **形状不一致不 raise**：按"拒收 + 空帧 + 节流 WARN"处理，与 stereo_ranger
|
||||
非同帧拒收同哲学（不阻塞整体）。
|
||||
5. **几何参数单一来源**：`stereo_yolo.json` 不存内参/原分辨率（输出框天然在
|
||||
输入图坐标系），F/B/cx/cy 唯一来源是 `utils/config/stereo_ranger.json`，
|
||||
避免两处漂移（§4 配置一致性）。
|
||||
6. **`track_id` 留 `None`**：外部追踪器注入属链路后段（§2.1；v1.1 可选）。
|
||||
|
||||
## 移植注记（C++ 侧需自行复现的库内行为）
|
||||
|
||||
本次为 ultralytics 直跑口径，以下行为在库内完成、C++ 实现必须逐项复现
|
||||
（不一致会在视差取点上引入系统性偏差）：
|
||||
|
||||
| 库内行为 | C++ 需复现要点 |
|
||||
| --- | --- |
|
||||
| letterbox | 等比缩放到 `imgsz`、灰边填充（ultralytics 预测默认填充值 114）；左右同一套参数（形状一致时天然满足） |
|
||||
| 输出解码 | YOLO26 检测头输出布局与解码（建议导出一份 ONNX 作为参照物；本仓环境：ultralytics 8.4 / torch 2.11 / onnx 1.18） |
|
||||
| NMS | `conf` 先过滤、NMS 用配置 IoU 门限；`max_det` 默认 300 |
|
||||
| 坐标回映 | letterbox 逆变换（scale/pad）到原生分辨率；输出 `x1,y1,x2,y2` 浮点 |
|
||||
| 并发 | 双实例双线程（单实例并发调用不安全） |
|
||||
|
||||
## 配置项（全部外置，占位值）
|
||||
|
||||
| 键 | 占位值 | 说明 |
|
||||
| --- | --- | --- |
|
||||
| `model_path` | `models/yolo26n-drone.pt` | 相对路径按启动工作目录解析（demo 内按仓库根兜底） |
|
||||
| `imgsz` | 640 | 须为 32 的整数倍（YOLO 步长约束） |
|
||||
| `conf_threshold` | 0.25 | 识别端自身过滤口径（测距端不消费检测置信度，§2.4） |
|
||||
| `iou_threshold` | 0.7 | NMS IoU 门限 |
|
||||
| `class_whitelist` | `null` | 可选白名单；同一目标左右目类别必须一致（§2.2 第 5 条） |
|
||||
| `device` | `null` | null = 自动选择（`cpu` / `cuda:0` / …） |
|
||||
| `concurrency` | `parallel` | `parallel` / `sequential` |
|
||||
|
||||
## 测试与演示
|
||||
|
||||
```bash
|
||||
cd utils
|
||||
"D:/program file/conda/envs/pytorch/python.exe" -m pytest tests/stereo_yolo -q # 36 条
|
||||
python utils/demo_stereo_yolo.py # 真模型 + 占位素材
|
||||
```
|
||||
|
||||
单测全部用注入的假检测器驱动（无模型、无素材、无硬件）：frame_id 左右共用、
|
||||
出口自检丢弃退化框、白名单过滤、单目失败隔离、形状不一致拒收、双人屏障
|
||||
断言真并发（串行实现会屏障超时）。
|
||||
|
||||
demo（真模型 + AI 合成素材 `utils/assets/stereo_yolo/drone_sky.png`）：
|
||||
① 单图冒烟 → ② 合成双目对（同一 patch 羽化粘贴、已知视差 12 px，实测
|
||||
|ΔD|≈0.3 px、|Δy|≈0.2 px）→ 真检测框喂 `StereoRanger.update()`，记录值与
|
||||
解析值 `Z=F·B/D` 同量级 → ③ 空场景（正常空帧不告警）→ ④ 形状不一致拒收
|
||||
→ ⑤ 帧周期实测（台架开发机 ≈34 ms/帧，占位）。替换素材文件即可换真实图片。
|
||||
|
||||
## 验收对照（规格 §6）
|
||||
|
||||
| 项 | 状态 |
|
||||
| --- | --- |
|
||||
| 坐标回映误差 <0.5 px | 由库内完成，无法构件级单测；以合成对整体校验（|ΔD|≈0.3 px @1080p，含检测边缘噪声）；C++ 移植后需实测 |
|
||||
| 垂直对齐 P95 ≤1 px | 合成对实测 |Δy|≈0.2 px（合成口径，非真实校正质量）；待真实素材 |
|
||||
| 类别一致性 | 模型单类 `Drone`，合成对天然一致；待真实素材 |
|
||||
| 输出合法性 | ✓ 单测（非有限值/退化框构造用例） |
|
||||
| 输入纯净性 | ✓ 结构性保证（前端无跨帧状态） |
|
||||
| 下游对接 | ✓ demo 场景②；δZ ∝ Z² 趋势由测距端单测覆盖 |
|
||||
| 帧周期 | △ 台架开发机实测 ≈34 ms/帧（30 fps 预算 33.3 ms）；待平台实测（P7） |
|
||||
@@ -0,0 +1,20 @@
|
||||
"""stereo_yolo —— 双目 YOLO 检测前端（规格：docs/双目YOLO目标识别需求说明.md）。
|
||||
|
||||
在左右目图像上独立运行 YOLO 检测器，输出**符合 stereo_ranger 输入契约**的
|
||||
检测框列表（每目一个 DetectionFrame）；不做测距、左右目关联与立体校正。
|
||||
⚠ 全部参数为占位值：标定与地面验证完成前仅限台架/回放验证，不得接入实飞
|
||||
控制回路（需求说明 §7 红线同源）。
|
||||
"""
|
||||
|
||||
from .config import StereoYoloConfig, load_from_json
|
||||
from .detector import UltralyticsDetector
|
||||
from .frontend import StereoYoloFrontend
|
||||
from .sbs import split_sbs
|
||||
|
||||
__all__ = [
|
||||
"StereoYoloConfig",
|
||||
"StereoYoloFrontend",
|
||||
"UltralyticsDetector",
|
||||
"load_from_json",
|
||||
"split_sbs",
|
||||
]
|
||||
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,86 @@
|
||||
"""StereoYoloConfig 与 JSON 配置加载（需求说明 §4 配置项需求）。
|
||||
|
||||
全部配置外置、构造期一次加载并校验：缺项、未知字段、类型错误、非法取值
|
||||
一律拒绝进入运行期（fail-fast）。几何参数（F/B/cx/cy/原分辨率）不在此
|
||||
重复存储——输出框位于**输入图像自身的原生坐标系**，几何参数的唯一来源
|
||||
是 utils/config/stereo_ranger.json（§4 配置一致性要求：同一配置源驱动，
|
||||
杜绝两处漂移）。
|
||||
"""
|
||||
|
||||
import json
|
||||
from dataclasses import dataclass, fields
|
||||
from typing import Any, Dict, Optional, Tuple
|
||||
|
||||
_CONCURRENCY_MODES = ("parallel", "sequential")
|
||||
_INT_FIELDS = frozenset({"imgsz"})
|
||||
_STR_FIELDS = frozenset({"model_path", "concurrency"})
|
||||
|
||||
|
||||
@dataclass
|
||||
class StereoYoloConfig:
|
||||
model_path: str = "models/yolo26n-drone.pt"
|
||||
imgsz: int = 640 # 左右共用（§2.2 第 2 条：同一缩放、同一填充）
|
||||
conf_threshold: float = 0.25 # ⚠占位：识别端自身过滤口径（§2.4）
|
||||
iou_threshold: float = 0.7 # ⚠占位：NMS IoU 门限
|
||||
class_whitelist: Optional[Tuple[int, ...]] = None # None = 不过滤（§4 可选白名单）
|
||||
device: Optional[str] = None # None = 自动选择（cpu / cuda:0 / …）
|
||||
concurrency: str = "parallel" # parallel = 左右并发（§5 硬性要求，串行会吃掉全部预算）
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
_require(bool(self.model_path.strip()), "model_path", "不得为空")
|
||||
_require(
|
||||
self.imgsz >= 32 and self.imgsz % 32 == 0,
|
||||
"imgsz",
|
||||
"须为 32 的整数倍且 ≥ 32（YOLO 步长约束）",
|
||||
)
|
||||
_require(0.0 <= self.conf_threshold <= 1.0, "conf_threshold", "须落在 [0, 1]")
|
||||
_require(0.0 < self.iou_threshold <= 1.0, "iou_threshold", "须落在 (0, 1]")
|
||||
_require(self.concurrency in _CONCURRENCY_MODES, "concurrency", f"须为 {_CONCURRENCY_MODES} 之一")
|
||||
_require(self.device is None or bool(self.device.strip()), "device", "须为非空字符串或 null")
|
||||
if self.class_whitelist is not None:
|
||||
ids = tuple(self.class_whitelist)
|
||||
_require(
|
||||
len(ids) > 0 and all(isinstance(item, int) and not isinstance(item, bool) and item >= 0 for item in ids),
|
||||
"class_whitelist",
|
||||
"须为非空非负整数列表或 null",
|
||||
)
|
||||
self.class_whitelist = ids
|
||||
|
||||
|
||||
def load_from_json(path: str) -> StereoYoloConfig:
|
||||
"""从 JSON 文件构建配置；缺项/未知字段/类型错误/非法取值一律抛 ValueError。"""
|
||||
with open(path, encoding="utf-8") as handle:
|
||||
data: Dict[str, Any] = json.load(handle)
|
||||
if not isinstance(data, dict):
|
||||
raise ValueError("配置非法: 顶层须为 JSON 对象")
|
||||
known = {item.name for item in fields(StereoYoloConfig)}
|
||||
unknown = sorted(set(data) - known)
|
||||
if unknown:
|
||||
raise ValueError(f"未知配置字段: {unknown}")
|
||||
missing = sorted(known - set(data))
|
||||
if missing:
|
||||
raise ValueError(f"缺少配置字段: {missing}")
|
||||
for name, value in data.items():
|
||||
if name == "class_whitelist":
|
||||
if value is not None and not (
|
||||
isinstance(value, list)
|
||||
and all(isinstance(item, int) and not isinstance(item, bool) for item in value)
|
||||
):
|
||||
raise ValueError("配置类型非法: class_whitelist 须为整数数组或 null")
|
||||
elif name == "device":
|
||||
if value is not None and not isinstance(value, str):
|
||||
raise ValueError("配置类型非法: device 须为字符串或 null")
|
||||
elif name in _STR_FIELDS:
|
||||
if not isinstance(value, str):
|
||||
raise ValueError(f"配置类型非法: {name} 须为字符串")
|
||||
elif name in _INT_FIELDS:
|
||||
if not isinstance(value, int) or isinstance(value, bool):
|
||||
raise ValueError(f"配置类型非法: {name} 须为整数")
|
||||
elif isinstance(value, bool) or not isinstance(value, (int, float)):
|
||||
raise ValueError(f"配置类型非法: {name} 须为数值")
|
||||
return StereoYoloConfig(**data)
|
||||
|
||||
|
||||
def _require(condition: bool, field_name: str, message: str) -> None:
|
||||
if not condition:
|
||||
raise ValueError(f"配置非法: {field_name} {message}")
|
||||
@@ -0,0 +1,78 @@
|
||||
"""UltralyticsDetector —— ultralytics YOLO .pt 直载的检测后端。
|
||||
|
||||
⚠ 移植注记（C++ 侧需自行复现，详见 README）：letterbox（缩放 + 填充到
|
||||
imgsz）、输出张量解码、NMS、框映射回输入图原生坐标系，本例由 ultralytics
|
||||
库内完成；移植时必须逐项复现同一几何（步长对齐、填充方式、IoU 门限、
|
||||
原生坐标回映），否则视差取点会引入系统性偏差。
|
||||
"""
|
||||
|
||||
import queue
|
||||
from pathlib import Path
|
||||
from typing import List
|
||||
|
||||
from stereo_ranger.types import Detection
|
||||
|
||||
from .config import StereoYoloConfig
|
||||
|
||||
|
||||
class UltralyticsDetector:
|
||||
"""双实例池：ultralytics 单实例非线程安全，左右并发各持一份（§3/§4）。
|
||||
|
||||
接口约定（可注入替换）：``detect(image) -> list[Detection]``，
|
||||
image 为 BGR ndarray（cv2 约定），返回框位于该图原生坐标系。
|
||||
"""
|
||||
|
||||
def __init__(self, config: StereoYoloConfig) -> None:
|
||||
self._cfg = config
|
||||
self._model_path = _resolve_model_path(config.model_path)
|
||||
self._pool: "queue.SimpleQueue" = queue.SimpleQueue()
|
||||
from ultralytics import YOLO # 延迟导入：仅真实推理付加载成本，单测注入假检测器（§6）
|
||||
|
||||
for _ in range(2):
|
||||
self._pool.put(YOLO(str(self._model_path)))
|
||||
|
||||
def detect(self, image) -> List[Detection]:
|
||||
model = self._pool.get()
|
||||
try:
|
||||
results = model.predict(
|
||||
source=image,
|
||||
imgsz=self._cfg.imgsz,
|
||||
conf=self._cfg.conf_threshold,
|
||||
iou=self._cfg.iou_threshold,
|
||||
device=self._cfg.device,
|
||||
verbose=False,
|
||||
)
|
||||
finally:
|
||||
self._pool.put(model)
|
||||
if not results:
|
||||
return []
|
||||
return _result_to_detections(results[0])
|
||||
|
||||
|
||||
def _result_to_detections(result) -> List[Detection]:
|
||||
boxes = getattr(result, "boxes", None)
|
||||
if boxes is None or len(boxes) == 0:
|
||||
return []
|
||||
xyxy = boxes.xyxy.cpu().numpy()
|
||||
conf = boxes.conf.cpu().numpy()
|
||||
cls = boxes.cls.cpu().numpy()
|
||||
return [
|
||||
Detection(
|
||||
float(box[0]),
|
||||
float(box[1]),
|
||||
float(box[2]),
|
||||
float(box[3]),
|
||||
confidence=float(score),
|
||||
class_id=int(class_id),
|
||||
)
|
||||
for box, score, class_id in zip(xyxy, conf, cls)
|
||||
]
|
||||
|
||||
|
||||
def _resolve_model_path(raw: str) -> Path:
|
||||
path = Path(raw).expanduser()
|
||||
if not path.is_absolute():
|
||||
path = Path.cwd() / path
|
||||
if not path.is_file():
|
||||
raise FileNotFoundError(f"模型权重不存在: {path}（model_path 相对路径按启动工作目录解析）")
|
||||
return path.resolve()
|
||||
@@ -0,0 +1,113 @@
|
||||
"""StereoYoloFrontend —— 双目检测前端主类（需求说明 §2/§3）。
|
||||
|
||||
单帧流程：左右图（已校正、原生分辨率）→ 左右并发检测 → 出口自检 + 类别
|
||||
白名单 → 每目封装 DetectionFrame（共用 frame_id，§2.3 同帧契约）。
|
||||
|
||||
前端**无跨帧状态**：没有历史就没有外推，结构性满足"只送本帧原生实测框"
|
||||
的输入纯净性硬约束（§2.5 第 1 条）。
|
||||
"""
|
||||
|
||||
import logging
|
||||
import math
|
||||
from concurrent.futures import ThreadPoolExecutor
|
||||
from typing import List, Optional, Sequence, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
from stereo_ranger.types import Detection, DetectionFrame
|
||||
|
||||
from .config import StereoYoloConfig
|
||||
from .detector import UltralyticsDetector
|
||||
from .warn_throttle import WarnThrottle
|
||||
|
||||
logger = logging.getLogger("stereo_yolo")
|
||||
|
||||
_EYE_LABELS = ("左目", "右目")
|
||||
|
||||
|
||||
class StereoYoloFrontend:
|
||||
"""图像 → 符合 stereo_ranger 输入契约的检测框（§2 接口契约）。
|
||||
|
||||
``detector`` 可注入替换：接口为 ``detect(img) -> list[Detection]``
|
||||
（单测用假检测器驱动，§6 输出合法性/失败隔离用例无需真实模型）。
|
||||
"""
|
||||
|
||||
def __init__(self, config: StereoYoloConfig, detector: Optional[object] = None) -> None:
|
||||
self._cfg = config
|
||||
self._detector = detector if detector is not None else UltralyticsDetector(config)
|
||||
self._throttle = WarnThrottle()
|
||||
self._executor = (
|
||||
ThreadPoolExecutor(max_workers=2, thread_name_prefix="stereo_yolo")
|
||||
if config.concurrency == "parallel"
|
||||
else None
|
||||
)
|
||||
|
||||
@property
|
||||
def config(self) -> StereoYoloConfig:
|
||||
return self._cfg
|
||||
|
||||
def detect(
|
||||
self,
|
||||
left_img: np.ndarray,
|
||||
right_img: np.ndarray,
|
||||
frame_id: int,
|
||||
) -> Tuple[DetectionFrame, DetectionFrame]:
|
||||
"""左右各输出一个 DetectionFrame；两帧共用同一 frame_id（§2.3）。"""
|
||||
if left_img.shape != right_img.shape:
|
||||
self._warn(
|
||||
"shape_mismatch",
|
||||
f"左右图尺寸不一致（{left_img.shape} vs {right_img.shape}），整帧拒收输出空（§2.2 第 2 条）",
|
||||
)
|
||||
return DetectionFrame(frame_id, ()), DetectionFrame(frame_id, ())
|
||||
|
||||
if self._executor is not None:
|
||||
left_future = self._executor.submit(self._detect_eye, left_img, _EYE_LABELS[0])
|
||||
right_future = self._executor.submit(self._detect_eye, right_img, _EYE_LABELS[1])
|
||||
left, right = left_future.result(), right_future.result()
|
||||
else:
|
||||
left = self._detect_eye(left_img, _EYE_LABELS[0])
|
||||
right = self._detect_eye(right_img, _EYE_LABELS[1])
|
||||
|
||||
return DetectionFrame(frame_id, tuple(left)), DetectionFrame(frame_id, tuple(right))
|
||||
|
||||
def close(self) -> None:
|
||||
"""释放并发线程池（前端不再使用时调用）。"""
|
||||
if self._executor is not None:
|
||||
self._executor.shutdown(wait=True)
|
||||
|
||||
# ---------------- 内部 ----------------
|
||||
|
||||
def _detect_eye(self, image: np.ndarray, eye_label: str) -> List[Detection]:
|
||||
try:
|
||||
raw = self._detector.detect(image)
|
||||
except Exception as exc: # 单目失败隔离：不阻塞整体，该目输出空帧列表（§5 失败处理）
|
||||
self._warn(f"infer_failed:{eye_label}", f"{eye_label}推理失败，该目输出空帧列表：{exc!r}")
|
||||
return []
|
||||
return self._sanitize(raw)
|
||||
|
||||
def _sanitize(self, detections: Sequence[Detection]) -> List[Detection]:
|
||||
"""出口自检（§2.2 第 6 条）：丢弃非有限值 / x2<=x1 / y2<=y1 的框；白名单过滤。"""
|
||||
kept: List[Detection] = []
|
||||
dropped = 0
|
||||
whitelist = self._cfg.class_whitelist
|
||||
for detection in detections:
|
||||
if not _is_valid_box(detection):
|
||||
dropped += 1
|
||||
continue
|
||||
if whitelist is not None and detection.class_id not in whitelist:
|
||||
continue
|
||||
kept.append(detection)
|
||||
if dropped:
|
||||
self._warn("degenerate_box", f"丢弃退化检测框 {dropped} 个（非有限值或 x2<=x1/y2<=y1）")
|
||||
return kept
|
||||
|
||||
def _warn(self, category: str, message: str) -> None:
|
||||
if self._throttle.should_emit(category):
|
||||
logger.warning("%s（累计 %d 次）", message, self._throttle.count(category))
|
||||
|
||||
|
||||
def _is_valid_box(detection: Detection) -> bool:
|
||||
values = (detection.x1, detection.y1, detection.x2, detection.y2)
|
||||
if not all(math.isfinite(value) for value in values):
|
||||
return False
|
||||
return detection.x2 > detection.x1 and detection.y2 > detection.y1
|
||||
@@ -0,0 +1,16 @@
|
||||
"""side-by-side 单流裁切（需求说明 §2.3：同帧拼接形态推荐优先支持）。
|
||||
|
||||
解码一次、裁切为左右图，天然同曝光、天然同帧；宽须为偶数。
|
||||
"""
|
||||
|
||||
from typing import Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def split_sbs(image: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
|
||||
"""左右各半裁切，返回 (左图, 右图)。"""
|
||||
if image.ndim < 2 or image.shape[1] % 2 != 0:
|
||||
raise ValueError(f"side-by-side 图像宽度须为偶数，实际 shape={image.shape}")
|
||||
half = int(image.shape[1] // 2)
|
||||
return image[:, :half].copy(), image[:, half:].copy()
|
||||
@@ -0,0 +1,26 @@
|
||||
"""告警节流器（需求说明 §5 失败处理）：每类异常"第 1 次 + 每满 100 次"打印。
|
||||
|
||||
语义与 stereo_ranger 的节流器一致，但按前端/测距解耦要求（§7）不跨包复用。
|
||||
"""
|
||||
|
||||
from typing import Dict
|
||||
|
||||
|
||||
class WarnThrottle:
|
||||
"""按异常类独立计数与限频。"""
|
||||
|
||||
def __init__(self, every: int = 100) -> None:
|
||||
if every < 1:
|
||||
raise ValueError("every 必须 ≥ 1")
|
||||
self._every = every
|
||||
self._counts: Dict[str, int] = {}
|
||||
|
||||
def should_emit(self, category: str) -> bool:
|
||||
"""推进该类异常计数并返回本次是否应打印（第 1 次 + 每满 every 次）。"""
|
||||
count = self._counts.get(category, 0) + 1
|
||||
self._counts[category] = count
|
||||
return count == 1 or count % self._every == 0
|
||||
|
||||
def count(self, category: str) -> int:
|
||||
"""该类异常的累计次数。"""
|
||||
return self._counts.get(category, 0)
|
||||
Binary file not shown.
Binary file not shown.
BIN
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,94 @@
|
||||
"""StereoConfig 校验与 JSON 加载测试（规格 §9：配置外置、fail-fast）。"""
|
||||
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import StereoConfig, load_from_json
|
||||
|
||||
CONFIG_PATH = Path(__file__).resolve().parents[2] / "config" / "stereo_ranger.json"
|
||||
|
||||
|
||||
def _config_dict() -> dict:
|
||||
return json.loads(CONFIG_PATH.read_text(encoding="utf-8"))
|
||||
|
||||
|
||||
def _write(tmp_path: Path, data: dict) -> str:
|
||||
path = tmp_path / "stereo.json"
|
||||
path.write_text(json.dumps(data, ensure_ascii=False), encoding="utf-8")
|
||||
return str(path)
|
||||
|
||||
|
||||
def test_defaults_are_valid_and_domains_are_consistent():
|
||||
cfg = StereoConfig()
|
||||
assert cfg.focal_baseline_px_m == pytest.approx(cfg.fx_px * cfg.baseline_m)
|
||||
assert cfg.z_min_m == pytest.approx(cfg.focal_baseline_px_m / cfg.disparity_max_px)
|
||||
assert cfg.z_max_m == pytest.approx(cfg.focal_baseline_px_m / cfg.disparity_min_px)
|
||||
assert cfg.z_error_budget_max_m == pytest.approx((cfg.max_delta_z_m * cfg.focal_baseline_px_m) ** 0.5)
|
||||
|
||||
|
||||
def test_load_from_json_matches_defaults():
|
||||
assert load_from_json(str(CONFIG_PATH)) == StereoConfig()
|
||||
|
||||
|
||||
def test_missing_field_rejected(tmp_path):
|
||||
data = _config_dict()
|
||||
data.pop("baseline_m")
|
||||
with pytest.raises(ValueError, match="缺少配置字段"):
|
||||
load_from_json(_write(tmp_path, data))
|
||||
|
||||
|
||||
def test_unknown_field_rejected(tmp_path):
|
||||
data = _config_dict()
|
||||
data["radar_fallback"] = True
|
||||
with pytest.raises(ValueError, match="未知配置字段"):
|
||||
load_from_json(_write(tmp_path, data))
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"field,value",
|
||||
[
|
||||
("fx_px", "960"),
|
||||
("consistency_N_frames", 3.5),
|
||||
("smooth_alpha", True),
|
||||
],
|
||||
)
|
||||
def test_type_error_rejected(tmp_path, field, value):
|
||||
data = _config_dict()
|
||||
data[field] = value
|
||||
with pytest.raises(ValueError, match="配置类型非法"):
|
||||
load_from_json(_write(tmp_path, data))
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"field,value",
|
||||
[
|
||||
("fx_px", 0.0),
|
||||
("fy_px", -1.0),
|
||||
("cx_px", -0.5),
|
||||
("cy_px", -0.5),
|
||||
("baseline_m", 0.0),
|
||||
("tau_y_px", 0.0),
|
||||
("tau_h_ratio", 0.0),
|
||||
("disparity_min_px", 0.0),
|
||||
("disparity_min_px", 60.0),
|
||||
("max_delta_z_m", 0.0),
|
||||
("consistency_N_frames", 0),
|
||||
("consistency_M_frames", 2),
|
||||
("consistency_jump_px", 0.0),
|
||||
("smooth_alpha", 0.0),
|
||||
("smooth_alpha", 1.5),
|
||||
("jump_reset_m", 0.0),
|
||||
("filter_reset_frames", 0),
|
||||
("isolation_gate_px", 0.0),
|
||||
],
|
||||
)
|
||||
def test_invalid_values_rejected(field, value):
|
||||
with pytest.raises(ValueError, match="配置非法"):
|
||||
StereoConfig(**{field: value})
|
||||
|
||||
|
||||
def test_boundary_values_accepted():
|
||||
cfg = StereoConfig(disparity_min_px=3.0, disparity_max_px=3.0 + 1e-9, smooth_alpha=1.0, consistency_M_frames=3)
|
||||
assert cfg.z_max_m > cfg.z_min_m
|
||||
@@ -0,0 +1,80 @@
|
||||
"""一致性监控测试（规格 §6.7/§11.4：N 帧降级、M 帧判 invalid、正常帧清零、每目标隔离）。"""
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import ConsistencyMonitor
|
||||
|
||||
|
||||
def _monitor() -> ConsistencyMonitor:
|
||||
return ConsistencyMonitor(jump_px=3.0, degrade_frames=3, invalid_frames=5)
|
||||
|
||||
|
||||
def test_first_frame_has_no_reference_jump():
|
||||
status = _monitor().advance("t", 10.0)
|
||||
assert status.jump_px is None
|
||||
assert status.abnormal is False
|
||||
assert status.streak == 0
|
||||
|
||||
|
||||
def test_normal_jump_resets_streak():
|
||||
monitor = _monitor()
|
||||
monitor.advance("t", 10.0)
|
||||
abnormal = monitor.advance("t", 16.0)
|
||||
assert abnormal.abnormal and abnormal.streak == 1
|
||||
normal = monitor.advance("t", 17.0)
|
||||
assert not normal.abnormal and normal.streak == 0
|
||||
|
||||
|
||||
def test_jump_at_threshold_is_normal():
|
||||
monitor = _monitor()
|
||||
monitor.advance("t", 10.0)
|
||||
assert not monitor.advance("t", 13.0).abnormal # |ΔD| = 3.0 不超门限
|
||||
|
||||
|
||||
def test_degrades_at_n_frames_and_invalidates_at_m_frames():
|
||||
monitor = _monitor()
|
||||
monitor.advance("t", 10.0)
|
||||
disparities = [14.0, 18.0, 22.0, 26.0, 30.0] # 每帧跳变 4 px > 3 px
|
||||
statuses = [monitor.advance("t", d) for d in disparities]
|
||||
assert [s.streak for s in statuses] == [1, 2, 3, 4, 5]
|
||||
assert [s.confidence_degraded for s in statuses] == [False, False, True, True, True]
|
||||
assert [s.invalid for s in statuses] == [False, False, False, False, True]
|
||||
|
||||
|
||||
def test_targets_are_isolated():
|
||||
monitor = _monitor()
|
||||
monitor.advance("a", 10.0)
|
||||
monitor.advance("b", 20.0)
|
||||
monitor.advance("a", 15.0) # a 跳变异常
|
||||
assert monitor.streak("a") == 1
|
||||
assert monitor.streak("b") == 0
|
||||
|
||||
|
||||
def test_drop_clears_only_target():
|
||||
monitor = _monitor()
|
||||
monitor.advance("a", 10.0)
|
||||
monitor.advance("a", 20.0)
|
||||
monitor.drop("a")
|
||||
assert monitor.streak("a") == 0
|
||||
assert monitor.advance("a", 40.0).jump_px is None # 状态已清空，重新起始
|
||||
|
||||
|
||||
def test_clear_resets_all():
|
||||
monitor = _monitor()
|
||||
monitor.advance("a", 10.0)
|
||||
monitor.advance("a", 20.0)
|
||||
monitor.clear()
|
||||
assert monitor.streak("a") == 0
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"kwargs",
|
||||
[
|
||||
{"jump_px": 0.0, "degrade_frames": 3, "invalid_frames": 5},
|
||||
{"jump_px": 3.0, "degrade_frames": 0, "invalid_frames": 5},
|
||||
{"jump_px": 3.0, "degrade_frames": 5, "invalid_frames": 3},
|
||||
],
|
||||
)
|
||||
def test_invalid_ctor_args(kwargs):
|
||||
with pytest.raises(ValueError):
|
||||
ConsistencyMonitor(**kwargs)
|
||||
@@ -0,0 +1,94 @@
|
||||
"""时间滤波器测试（规格 §6.5/§11.4：低通收敛、跳变重置不拖尾、失效清理、每目标隔离）。"""
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import DistanceFilter
|
||||
|
||||
|
||||
def _filter() -> DistanceFilter:
|
||||
return DistanceFilter(alpha=0.3, jump_reset_m=5.0, reset_frames=5)
|
||||
|
||||
|
||||
def test_first_value_initializes():
|
||||
assert _filter().advance("t", 10.0) == pytest.approx(10.0)
|
||||
|
||||
|
||||
def test_low_pass_step_response():
|
||||
distance_filter = _filter()
|
||||
distance_filter.advance("t", 10.0)
|
||||
assert distance_filter.advance("t", 12.0) == pytest.approx(10.6)
|
||||
|
||||
|
||||
def test_low_pass_converges_to_measurement():
|
||||
distance_filter = _filter()
|
||||
distance_filter.advance("t", 10.0)
|
||||
value = 10.0
|
||||
for _ in range(40):
|
||||
value = distance_filter.advance("t", 12.0)
|
||||
assert value == pytest.approx(12.0, abs=1e-3)
|
||||
|
||||
|
||||
def test_alpha_one_is_identity():
|
||||
distance_filter = DistanceFilter(alpha=1.0, jump_reset_m=5.0, reset_frames=5)
|
||||
distance_filter.advance("t", 10.0)
|
||||
assert distance_filter.advance("t", 12.0) == pytest.approx(12.0)
|
||||
|
||||
|
||||
def test_jump_reset_without_tail():
|
||||
distance_filter = _filter()
|
||||
distance_filter.advance("t", 10.0)
|
||||
assert distance_filter.advance("t", 20.0) == pytest.approx(20.0) # |Δ| > 5 → 直接重置
|
||||
assert distance_filter.value("t") == pytest.approx(20.0)
|
||||
assert distance_filter.advance("t", 20.2) == pytest.approx(20.06) # 之后恢复低通
|
||||
|
||||
|
||||
def test_invalidate_keeps_state_before_threshold_and_clears_after():
|
||||
distance_filter = _filter()
|
||||
distance_filter.advance("t", 10.0)
|
||||
for _ in range(4):
|
||||
distance_filter.invalidate("t")
|
||||
assert distance_filter.value("t") == pytest.approx(10.0)
|
||||
assert distance_filter.invalid_streak("t") == 4
|
||||
distance_filter.invalidate("t")
|
||||
assert distance_filter.value("t") is None
|
||||
assert distance_filter.advance("t", 30.0) == pytest.approx(30.0) # 重新初始化而非平滑
|
||||
|
||||
|
||||
def test_valid_frame_resets_invalid_streak():
|
||||
distance_filter = _filter()
|
||||
distance_filter.advance("t", 10.0)
|
||||
distance_filter.invalidate("t")
|
||||
distance_filter.invalidate("t")
|
||||
distance_filter.advance("t", 10.5)
|
||||
assert distance_filter.invalid_streak("t") == 0
|
||||
|
||||
|
||||
def test_invalidate_without_state_is_noop():
|
||||
distance_filter = _filter()
|
||||
distance_filter.invalidate("t")
|
||||
assert distance_filter.value("t") is None
|
||||
|
||||
|
||||
def test_targets_are_isolated():
|
||||
distance_filter = _filter()
|
||||
distance_filter.advance("a", 10.0)
|
||||
distance_filter.advance("b", 100.0)
|
||||
assert distance_filter.advance("a", 12.0) == pytest.approx(10.6)
|
||||
assert distance_filter.value("b") == pytest.approx(100.0)
|
||||
distance_filter.drop("a")
|
||||
assert distance_filter.value("a") is None
|
||||
assert distance_filter.value("b") == pytest.approx(100.0)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"kwargs",
|
||||
[
|
||||
{"alpha": 0.0, "jump_reset_m": 5.0, "reset_frames": 5},
|
||||
{"alpha": 1.5, "jump_reset_m": 5.0, "reset_frames": 5},
|
||||
{"alpha": 0.3, "jump_reset_m": 0.0, "reset_frames": 5},
|
||||
{"alpha": 0.3, "jump_reset_m": 5.0, "reset_frames": 0},
|
||||
],
|
||||
)
|
||||
def test_invalid_ctor_args(kwargs):
|
||||
with pytest.raises(ValueError):
|
||||
DistanceFilter(**kwargs)
|
||||
@@ -0,0 +1,103 @@
|
||||
"""测距计算与有效性判定测试（规格 §3.2/§6.4/§6.6/§7.1/§11.4）。"""
|
||||
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import (
|
||||
Detection,
|
||||
StereoConfig,
|
||||
confidence_from_cost,
|
||||
evaluate_validity,
|
||||
forward_distance_m,
|
||||
range_error_abs_m,
|
||||
range_error_rel,
|
||||
solve_range,
|
||||
virtual_center_px,
|
||||
)
|
||||
|
||||
CFG = StereoConfig()
|
||||
|
||||
|
||||
def _boxes_for_point(x_m: float, y_m: float, z_m: float, width_px: float = 50.0, height_px: float = 40.0):
|
||||
"""由左目系 (X, Y, Z) 反推左右目框（同宽，边缘视差 = 中心视差 = F·B/Z）。"""
|
||||
d_px = CFG.focal_baseline_px_m / z_m
|
||||
x_l = CFG.cx_px + CFG.fx_px * x_m / z_m
|
||||
y_l = CFG.cy_px + CFG.fy_px * y_m / z_m
|
||||
left = Detection(x_l - width_px / 2, y_l - height_px / 2, x_l + width_px / 2, y_l + height_px / 2)
|
||||
right = Detection(
|
||||
x_l - width_px / 2 - d_px, y_l - height_px / 2, x_l + width_px / 2 - d_px, y_l + height_px / 2
|
||||
)
|
||||
return left, right, d_px
|
||||
|
||||
|
||||
def test_solve_range_recovers_ground_truth():
|
||||
# §11.4：已知 Z 反推 D 构造框对，断言 Z = F·B/D 回收
|
||||
left, right, d_px = _boxes_for_point(0.6, 0.3, 6.0)
|
||||
solution = solve_range(left, right, d_px, CFG)
|
||||
assert solution.z_m == pytest.approx(6.0)
|
||||
assert solution.x_m == pytest.approx(0.6)
|
||||
assert solution.y_m == pytest.approx(0.3)
|
||||
assert solution.r_m == pytest.approx(math.sqrt(0.6**2 + 0.3**2 + 6.0**2))
|
||||
|
||||
|
||||
def test_forward_distance_rejects_non_positive_disparity():
|
||||
with pytest.raises(ValueError):
|
||||
forward_distance_m(0.0, CFG)
|
||||
with pytest.raises(ValueError):
|
||||
forward_distance_m(-2.0, CFG)
|
||||
|
||||
|
||||
def test_virtual_center_equals_midline_camera_projection():
|
||||
# §6.6 恒等式：xL = cx + F(X_C + B/2)/Z，xR = cx + F(X_C − B/2)/Z ⇒ c_mid = cx + F·X_C/Z
|
||||
z_m = 5.0
|
||||
x_center_m = 0.57
|
||||
x_l = CFG.cx_px + CFG.fx_px * (x_center_m + CFG.baseline_m / 2.0) / z_m
|
||||
x_r = CFG.cx_px + CFG.fx_px * (x_center_m - CFG.baseline_m / 2.0) / z_m
|
||||
left = Detection(x_l - 30.0, 480.0, x_l + 30.0, 520.0)
|
||||
right = Detection(x_r - 30.0, 480.0, x_r + 30.0, 520.0)
|
||||
center_mid = virtual_center_px(left, right)
|
||||
assert center_mid[0] == pytest.approx(CFG.cx_px + CFG.fx_px * x_center_m / z_m)
|
||||
assert center_mid[1] == pytest.approx(500.0)
|
||||
|
||||
|
||||
def test_error_model_is_quadratic_in_z():
|
||||
assert range_error_abs_m(10.0, CFG) == pytest.approx(10.0**2 / CFG.focal_baseline_px_m)
|
||||
assert range_error_rel(10.0, CFG) == pytest.approx(10.0 / CFG.focal_baseline_px_m)
|
||||
assert range_error_abs_m(20.0, CFG) == pytest.approx(4.0 * range_error_abs_m(10.0, CFG))
|
||||
|
||||
|
||||
def test_validity_gate_1_out_of_domain():
|
||||
decision = evaluate_validity(CFG.disparity_min_px - 0.1, 30.0, CFG)
|
||||
assert not decision.in_domain and not decision.valid
|
||||
|
||||
|
||||
def test_validity_gate_2_error_budget_blocks_far_field():
|
||||
# 域内（D = 4.8 px → Z = 12 m）但 δZ = 2.5 m > 2 m，预算门拦截
|
||||
d_px = 4.8
|
||||
z_m = CFG.focal_baseline_px_m / d_px
|
||||
decision = evaluate_validity(d_px, z_m, CFG)
|
||||
assert decision.in_domain and not decision.budget_ok and not decision.valid
|
||||
|
||||
|
||||
def test_validity_gate_2_boundary_sides():
|
||||
z_limit = CFG.z_error_budget_max_m
|
||||
assert evaluate_validity(CFG.focal_baseline_px_m / z_limit, z_limit * 0.999, CFG).budget_ok
|
||||
assert not evaluate_validity(CFG.focal_baseline_px_m / z_limit, z_limit * 1.001, CFG).budget_ok
|
||||
|
||||
|
||||
def test_validity_requires_finite_z():
|
||||
assert not evaluate_validity(5.0, None, CFG).valid
|
||||
assert not evaluate_validity(5.0, float("inf"), CFG).valid
|
||||
|
||||
|
||||
def test_domain_boundaries_are_inclusive():
|
||||
assert evaluate_validity(CFG.disparity_min_px, 19.2, CFG).in_domain
|
||||
assert evaluate_validity(CFG.disparity_max_px, 1.0, CFG).in_domain
|
||||
|
||||
|
||||
def test_confidence_from_cost_linear_clamped():
|
||||
assert confidence_from_cost(0.0) == pytest.approx(1.0)
|
||||
assert confidence_from_cost(0.4) == pytest.approx(0.6)
|
||||
assert confidence_from_cost(1.0) == pytest.approx(0.0)
|
||||
assert confidence_from_cost(1.5) == pytest.approx(0.0)
|
||||
@@ -0,0 +1,87 @@
|
||||
"""匹配器测试（规格 §6.2/§11.4：硬约束各一例、贪心排序、一一配对、cost ≥ 1 拒绝）。"""
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import Detection, GreedyMatcher, SamplePoint, StereoConfig, sample_point
|
||||
|
||||
CFG = StereoConfig()
|
||||
MATCHER = GreedyMatcher()
|
||||
|
||||
|
||||
def _point(cx: float, cy: float, *, w: float = 60.0, h: float = 40.0, class_id: int = 0) -> SamplePoint:
|
||||
return sample_point(
|
||||
Detection(cx - w / 2.0, cy - h / 2.0, cx + w / 2.0, cy + h / 2.0, class_id=class_id)
|
||||
)
|
||||
|
||||
|
||||
def test_class_mismatch_rejected():
|
||||
assert MATCHER.match([_point(1000.0, 500.0)], [_point(1000.0, 500.0, class_id=1)], CFG) == []
|
||||
|
||||
|
||||
def test_dy_beyond_tau_rejected():
|
||||
assert MATCHER.match([_point(1000.0, 500.0)], [_point(1000.0, 502.1)], CFG) == []
|
||||
|
||||
|
||||
def test_dy_within_tau_accepted_with_cost():
|
||||
pairs = MATCHER.match([_point(1000.0, 500.0)], [_point(1000.0, 501.9)], CFG)
|
||||
assert len(pairs) == 1
|
||||
assert pairs[0].cost == pytest.approx(1.9 / CFG.tau_y_px)
|
||||
|
||||
|
||||
def test_height_ratio_beyond_tau_rejected():
|
||||
assert MATCHER.match([_point(1000.0, 500.0, h=100.0)], [_point(1000.0, 500.0, h=140.0)], CFG) == []
|
||||
|
||||
|
||||
def test_height_ratio_near_tau_accepted():
|
||||
pairs = MATCHER.match([_point(1000.0, 500.0, h=120.0)], [_point(1000.0, 500.0, h=159.0)], CFG)
|
||||
assert len(pairs) == 1
|
||||
|
||||
|
||||
def test_cost_at_one_rejected():
|
||||
# dy = tau_y ⇒ cost = 1.0，严格 cost < 1 才接受（§6.2）
|
||||
assert MATCHER.match([_point(1000.0, 500.0)], [_point(1000.0, 502.0)], CFG) == []
|
||||
|
||||
|
||||
def test_combined_cost_at_one_rejected():
|
||||
# dy/tau = 0.5 与 Δh/(tau_h·h_max) = 0.5 相加恰为 1.0 → 拒绝
|
||||
assert MATCHER.match([_point(1000.0, 500.0, h=112.0)], [_point(1000.0, 501.0, h=128.0)], CFG) == []
|
||||
|
||||
|
||||
def test_greedy_pairs_cheapest_first_and_one_to_one():
|
||||
left = [_point(1000.0, 500.0), _point(1000.0, 501.0)]
|
||||
right = [_point(1000.0, 500.0), _point(1000.0, 501.0)]
|
||||
pairs = MATCHER.match(left, right, CFG)
|
||||
assert [(p.left_index, p.right_index) for p in pairs] == [(0, 0), (1, 1)]
|
||||
assert pairs[0].cost == pytest.approx(0.0)
|
||||
|
||||
|
||||
def test_one_left_claims_cheapest_right():
|
||||
pairs = MATCHER.match([_point(1000.0, 500.0)], [_point(1000.0, 500.0), _point(1000.0, 500.5)], CFG)
|
||||
assert [(p.left_index, p.right_index) for p in pairs] == [(0, 0)]
|
||||
|
||||
|
||||
def test_two_lefts_cannot_share_one_right():
|
||||
left = [_point(1000.0, 500.0), _point(1000.0, 500.2)]
|
||||
right = [_point(1000.0, 500.0)]
|
||||
pairs = MATCHER.match(left, right, CFG)
|
||||
assert len(pairs) == 1
|
||||
assert pairs[0].left_index == 0
|
||||
|
||||
|
||||
def test_unmatched_right_is_not_paired():
|
||||
pairs = MATCHER.match([_point(1000.0, 500.0)], [_point(1000.0, 500.0), _point(1400.0, 500.0)], CFG)
|
||||
assert len(pairs) == 1
|
||||
assert pairs[0].right_index == 0
|
||||
|
||||
|
||||
def test_non_finite_points_excluded():
|
||||
bad = sample_point(Detection(float("nan"), 500.0, 1060.0, 540.0))
|
||||
good = _point(1000.0, 500.0)
|
||||
assert MATCHER.match([bad], [good], CFG) == []
|
||||
assert MATCHER.match([good], [bad], CFG) == []
|
||||
|
||||
|
||||
def test_zero_height_box_rejected():
|
||||
left = sample_point(Detection(970.0, 500.0, 1030.0, 500.0))
|
||||
right = sample_point(Detection(970.0, 500.0, 1030.0, 500.0))
|
||||
assert MATCHER.match([left], [right], CFG) == []
|
||||
@@ -0,0 +1,79 @@
|
||||
"""视差取点器测试（规格 §6.1/§11.4：同侧边缘、框宽差隔离、越窗回退、边界含等号）。"""
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import Detection, DisparitySource, StereoConfig, sample_point, select_disparity
|
||||
|
||||
CFG = StereoConfig()
|
||||
|
||||
|
||||
def _box(x1: float, x2: float, *, y1: float = 500.0, y2: float = 540.0) -> Detection:
|
||||
return Detection(x1, y1, x2, y2)
|
||||
|
||||
|
||||
def _disparity(left: Detection, right: Detection):
|
||||
return select_disparity(sample_point(left), sample_point(right), CFG)
|
||||
|
||||
|
||||
def test_left_edge_used_as_primary_sampling():
|
||||
d = _disparity(_box(1000.0, 1060.0), _box(990.4, 1050.4))
|
||||
assert d.source is DisparitySource.LEFT_EDGE
|
||||
assert d.d_px == pytest.approx(9.6)
|
||||
assert d.in_domain and d.positive and not d.fallback_used
|
||||
|
||||
|
||||
def test_box_width_difference_does_not_enter_edge_disparity():
|
||||
# §11.4：构造两目不等宽框，断言同侧边缘视差相同（框宽差不进入视差）
|
||||
left = _box(1000.0, 1100.0) # 宽 100
|
||||
right_narrow = _box(994.0, 1024.0) # 宽 30，左边缘差同 6 px
|
||||
right_wide = _box(994.0, 1094.0) # 宽 100
|
||||
d_narrow = _disparity(left, right_narrow)
|
||||
d_wide = _disparity(left, right_wide)
|
||||
assert d_narrow.d_px == pytest.approx(6.0)
|
||||
assert d_wide.d_px == pytest.approx(6.0)
|
||||
|
||||
|
||||
def test_center_disparity_would_be_polluted_by_width_difference():
|
||||
# 对照：中心视差 41 px = 边缘视差 6 px + 框宽差的一半 35 px，证明主取点选择边缘的理由
|
||||
left = _box(1000.0, 1100.0) # 宽 100, center 1050
|
||||
right_narrow = _box(994.0, 1024.0) # 宽 30, center 1009
|
||||
assert (left.cx - right_narrow.cx) == pytest.approx(41.0)
|
||||
|
||||
|
||||
def test_center_fallback_when_edge_out_of_window():
|
||||
# 边缘视差 60 px 越窗（> 57.6），中心视差 40 px 在域内 → 回退中心并标记
|
||||
left = _box(1000.0, 1060.0) # center 1030
|
||||
right = _box(940.0, 1040.0) # center 990
|
||||
d = _disparity(left, right)
|
||||
assert d.fallback_used is True
|
||||
assert d.source is DisparitySource.CENTER_FALLBACK
|
||||
assert d.d_px == pytest.approx(40.0)
|
||||
assert d.in_domain and d.positive
|
||||
|
||||
|
||||
def test_both_out_of_window_keeps_edge_value_for_diagnostics():
|
||||
left = _box(1000.0, 1200.0) # center 1100
|
||||
right = _box(900.0, 1000.0) # center 950
|
||||
d = _disparity(left, right)
|
||||
assert d.d_px == pytest.approx(100.0) # 边缘值保留（诊断 + 违例告警）
|
||||
assert not d.in_domain and not d.fallback_used and d.positive
|
||||
|
||||
|
||||
def test_domain_boundaries_are_inclusive():
|
||||
assert _disparity(_box(1000.0, 1060.0), _box(997.0, 1057.0)).in_domain # D = 3.0 = 下限
|
||||
# 上限用二进制可精确表示的 57.5 构造：十进制字面量 57.6 无法精确表示，
|
||||
# 框坐标相减会落到 57.6 常量之上，边界断言被浮点舍入干扰
|
||||
exact_cfg = StereoConfig(disparity_max_px=57.5)
|
||||
left, right = _box(1000.0, 1060.0), _box(942.5, 1002.5)
|
||||
assert select_disparity(sample_point(left), sample_point(right), exact_cfg).in_domain # D = 57.5 = 上限
|
||||
|
||||
|
||||
def test_negative_disparity_flagged_as_violation():
|
||||
d = _disparity(_box(1000.0, 1060.0), _box(1010.0, 1070.0))
|
||||
assert d.d_px == pytest.approx(-10.0)
|
||||
assert not d.positive and not d.in_domain
|
||||
|
||||
|
||||
def test_non_finite_box_marked_unusable():
|
||||
assert sample_point(Detection(float("nan"), 0.0, 10.0, 10.0)).finite is False
|
||||
assert sample_point(Detection(0.0, 0.0, 10.0, 10.0)).finite is True
|
||||
@@ -0,0 +1,292 @@
|
||||
"""全流程测试（规格 §5/§11.4：单目标、多目标、单目退化、非同帧拒收、越界、跳变）。
|
||||
|
||||
合成框对驱动，无需真实图像：几何是解析可断言的（§11.4）。
|
||||
"""
|
||||
|
||||
import logging
|
||||
import math
|
||||
from typing import List, Optional, Tuple
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import (
|
||||
Degradation,
|
||||
Detection,
|
||||
DetectionFrame,
|
||||
Eye,
|
||||
MatchPair,
|
||||
StereoConfig,
|
||||
StereoRanger,
|
||||
)
|
||||
|
||||
CFG = StereoConfig()
|
||||
LOGGER_NAME = "stereo_ranger"
|
||||
|
||||
|
||||
def _boxes(
|
||||
x_center_m: float = 0.0,
|
||||
y_m: float = 0.0,
|
||||
z_m: float = 6.0,
|
||||
*,
|
||||
w_m: float = 0.5,
|
||||
h_m: float = 0.3,
|
||||
class_id: int = 0,
|
||||
track_id: Optional[int] = None,
|
||||
confidence: float = 0.95,
|
||||
) -> Tuple[Detection, Detection]:
|
||||
"""模组中心系 (X_C, Y, Z) 目标 → 左右目检测框（两目同宽，边缘视差 = 中心视差）。"""
|
||||
x_l = CFG.cx_px + CFG.fx_px * (x_center_m + CFG.baseline_m / 2.0) / z_m
|
||||
x_r = CFG.cx_px + CFG.fx_px * (x_center_m - CFG.baseline_m / 2.0) / z_m
|
||||
y = CFG.cy_px + CFG.fy_px * y_m / z_m
|
||||
w_px = CFG.fx_px * w_m / z_m
|
||||
h_px = CFG.fy_px * h_m / z_m
|
||||
|
||||
def _box(cx_px: float) -> Detection:
|
||||
return Detection(
|
||||
cx_px - w_px / 2.0,
|
||||
y - h_px / 2.0,
|
||||
cx_px + w_px / 2.0,
|
||||
y + h_px / 2.0,
|
||||
confidence=confidence,
|
||||
class_id=class_id,
|
||||
track_id=track_id,
|
||||
)
|
||||
|
||||
return _box(x_l), _box(x_r)
|
||||
|
||||
|
||||
def _pair_with_disparity(d_px: float, *, y_center: float = 500.0, class_id: int = 0) -> Tuple[Detection, Detection]:
|
||||
"""构造视差恰为 d_px 的等宽框对（d 取 0.5 的整数倍时浮点精确）。"""
|
||||
width, height = 60.0, 40.0
|
||||
left = Detection(1000.0 - width / 2, y_center - height / 2, 1000.0 + width / 2, y_center + height / 2, class_id=class_id)
|
||||
right = Detection(
|
||||
1000.0 - d_px - width / 2,
|
||||
y_center - height / 2,
|
||||
1000.0 - d_px + width / 2,
|
||||
y_center + height / 2,
|
||||
class_id=class_id,
|
||||
)
|
||||
return left, right
|
||||
|
||||
|
||||
def _frame(frame_id: int, detections: List[Detection]) -> DetectionFrame:
|
||||
return DetectionFrame(frame_id=frame_id, detections=detections)
|
||||
|
||||
|
||||
def test_single_target_complete_valid_record():
|
||||
ranger = StereoRanger()
|
||||
left, right = _boxes(x_center_m=0.0, z_m=6.0)
|
||||
records = ranger.update(_frame(1, [left]), _frame(1, [right]), now_ms=1000)
|
||||
|
||||
assert len(records) == 1
|
||||
record = records[0]
|
||||
assert record.degradation is Degradation.COMPLETE
|
||||
assert record.valid is True
|
||||
assert record.forward_distance_m == pytest.approx(6.0)
|
||||
assert record.slant_range_m == pytest.approx(math.hypot(CFG.baseline_m / 2.0, 6.0)) # R 含左目系 +B/2 横向偏移
|
||||
assert record.disparity_px == pytest.approx(CFG.focal_baseline_px_m / 6.0)
|
||||
assert record.center_mid_px == pytest.approx((CFG.cx_px, CFG.cy_px))
|
||||
assert record.reference_box == left.bbox
|
||||
assert record.reference_eye is Eye.LEFT
|
||||
assert record.confidence == pytest.approx(1.0)
|
||||
assert record.frame_id == 1 and record.timestamp_ms == 1000
|
||||
|
||||
|
||||
def test_filter_smooths_across_valid_frames():
|
||||
ranger = StereoRanger()
|
||||
left_1, right_1 = _boxes(z_m=6.0)
|
||||
left_2, right_2 = _boxes(z_m=6.5)
|
||||
ranger.update(_frame(1, [left_1]), _frame(1, [right_1]))
|
||||
records = ranger.update(_frame(2, [left_2]), _frame(2, [right_2]))
|
||||
assert records[0].valid is True
|
||||
assert records[0].forward_distance_m == pytest.approx(6.0 + 0.3 * (6.5 - 6.0))
|
||||
# 参考斜距与已发布 Z 同基准：按 z_out/z_raw 从本帧原始斜距精确缩放（R ≥ Z）
|
||||
raw_slant = math.hypot(CFG.baseline_m / 2.0, 6.5)
|
||||
assert records[0].slant_range_m == pytest.approx(raw_slant * 6.15 / 6.5)
|
||||
|
||||
|
||||
def test_jump_reset_skips_smoothing():
|
||||
ranger = StereoRanger()
|
||||
left_1, right_1 = _boxes(z_m=4.0)
|
||||
left_2, right_2 = _boxes(z_m=10.0) # 跳变 6 m > jump_reset_m = 5 m
|
||||
ranger.update(_frame(1, [left_1]), _frame(1, [right_1]))
|
||||
records = ranger.update(_frame(2, [left_2]), _frame(2, [right_2]))
|
||||
assert records[0].forward_distance_m == pytest.approx(10.0)
|
||||
|
||||
|
||||
def test_far_field_out_of_domain_still_matched():
|
||||
# 越界 = 有效性门（决策）：配对仍成立，c_mid（无偏方位）与 D 照常发布，无 Z
|
||||
ranger = StereoRanger()
|
||||
left, right = _boxes(x_center_m=0.6, z_m=25.0) # D ≈ 2.3 px < 3 px
|
||||
records = ranger.update(_frame(1, [left]), _frame(1, [right]))
|
||||
|
||||
record = records[0]
|
||||
assert record.degradation is Degradation.COMPLETE
|
||||
assert record.valid is False
|
||||
assert record.forward_distance_m is None
|
||||
assert record.disparity_px == pytest.approx(2.304)
|
||||
assert record.center_mid_px == pytest.approx((CFG.cx_px + CFG.fx_px * 0.6 / 25.0, CFG.cy_px))
|
||||
assert record.slant_range_m == pytest.approx(math.hypot(0.6 + CFG.baseline_m / 2.0, 25.0))
|
||||
|
||||
|
||||
def test_error_budget_gate_blocks_far_but_in_domain():
|
||||
ranger = StereoRanger()
|
||||
left, right = _boxes(z_m=12.0) # D = 4.8 px 域内，δZ = 2.5 m > 2 m
|
||||
record = ranger.update(_frame(1, [left]), _frame(1, [right]))[0]
|
||||
assert record.disparity_px == pytest.approx(4.8)
|
||||
assert record.valid is False
|
||||
assert record.forward_distance_m is None
|
||||
assert record.slant_range_m == pytest.approx(math.hypot(CFG.baseline_m / 2.0, 12.0))
|
||||
|
||||
|
||||
def test_error_budget_gate_passes_just_inside():
|
||||
ranger = StereoRanger()
|
||||
left, right = _pair_with_disparity(5.8) # Z ≈ 9.93 m，δZ ≈ 1.71 m ≤ 2 m
|
||||
record = ranger.update(_frame(1, [left]), _frame(1, [right]))[0]
|
||||
assert record.valid is True
|
||||
assert record.forward_distance_m == pytest.approx(CFG.focal_baseline_px_m / 5.8)
|
||||
|
||||
|
||||
def test_negative_disparity_violation(caplog):
|
||||
ranger = StereoRanger()
|
||||
left = Detection(1000.0, 500.0, 1060.0, 540.0)
|
||||
right = Detection(1010.0, 500.0, 1070.0, 540.0)
|
||||
with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
|
||||
record = ranger.update(_frame(1, [left]), _frame(1, [right]))[0]
|
||||
assert record.degradation is Degradation.COMPLETE
|
||||
assert record.valid is False
|
||||
assert record.forward_distance_m is None
|
||||
assert record.slant_range_m is None
|
||||
assert record.disparity_px == pytest.approx(-10.0)
|
||||
assert any("disparity_violation" in message for message in caplog.messages)
|
||||
|
||||
|
||||
def test_center_fallback_keeps_target_valid(caplog):
|
||||
ranger = StereoRanger()
|
||||
left = Detection(1000.0, 500.0, 1060.0, 540.0) # 宽 60, center 1030
|
||||
right = Detection(940.0, 500.0, 1040.0, 540.0) # 宽 100, center 990
|
||||
with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
|
||||
record = ranger.update(_frame(1, [left]), _frame(1, [right]))[0]
|
||||
assert record.disparity_px == pytest.approx(40.0) # 边缘 60 px 越窗 → 中心 40 px
|
||||
assert record.valid is True
|
||||
assert record.forward_distance_m == pytest.approx(CFG.focal_baseline_px_m / 40.0)
|
||||
assert any("center_fallback" in message for message in caplog.messages)
|
||||
|
||||
|
||||
def test_single_eye_degradation_left_and_right():
|
||||
ranger = StereoRanger()
|
||||
left, right = _boxes(x_center_m=0.3, z_m=4.0)
|
||||
|
||||
left_only = ranger.update(_frame(1, [left]), _frame(1, []))
|
||||
assert len(left_only) == 1
|
||||
record = left_only[0]
|
||||
assert record.degradation is Degradation.BEARING_ONLY
|
||||
assert record.valid is False
|
||||
assert record.center_mid_px == pytest.approx((left.cx, left.cy))
|
||||
assert record.forward_distance_m is None and record.disparity_px is None
|
||||
assert record.reference_eye is Eye.LEFT
|
||||
assert record.confidence == pytest.approx(left.confidence)
|
||||
|
||||
right_only = ranger.update(_frame(2, []), _frame(2, [right]))
|
||||
assert len(right_only) == 1
|
||||
assert right_only[0].reference_eye is Eye.RIGHT
|
||||
assert right_only[0].center_mid_px == pytest.approx((right.cx, right.cy))
|
||||
|
||||
|
||||
def test_records_order_left_first_then_unmatched_right():
|
||||
ranger = StereoRanger()
|
||||
left, right_matched = _boxes(z_m=6.0, class_id=0)
|
||||
_, right_extra = _boxes(z_m=6.0, class_id=0, y_m=0.0)
|
||||
right_extra = Detection(400.0, 500.0, 460.0, 540.0) # 远处无对应左框
|
||||
records = ranger.update(_frame(1, [left]), _frame(1, [right_matched, right_extra]))
|
||||
assert len(records) == 2
|
||||
assert records[0].degradation is Degradation.COMPLETE
|
||||
assert records[0].valid is True
|
||||
assert records[1].degradation is Degradation.BEARING_ONLY
|
||||
assert records[1].reference_eye is Eye.RIGHT
|
||||
|
||||
|
||||
def test_frame_id_mismatch_rejects_frame_and_throttles_warning(caplog):
|
||||
ranger = StereoRanger()
|
||||
left, right = _boxes()
|
||||
with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
|
||||
assert ranger.update(_frame(1, [left]), _frame(2, [right])) == []
|
||||
assert ranger.update(_frame(3, [left]), _frame(4, [right])) == []
|
||||
warnings = [message for message in caplog.messages if "frame_mismatch" in message]
|
||||
assert len(warnings) == 1 # 第 1 次打印，第 2 次被节流
|
||||
assert "累计 1 次" in warnings[0]
|
||||
|
||||
|
||||
def test_empty_input_returns_empty_without_warning(caplog):
|
||||
ranger = StereoRanger()
|
||||
with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
|
||||
assert ranger.update(_frame(1, []), _frame(1, [])) == []
|
||||
assert caplog.messages == []
|
||||
|
||||
|
||||
def test_non_finite_box_dropped_with_warning(caplog):
|
||||
ranger = StereoRanger()
|
||||
bad = Detection(float("nan"), 500.0, 1060.0, 540.0)
|
||||
good_left, good_right = _boxes(z_m=6.0)
|
||||
with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
|
||||
records = ranger.update(_frame(1, [bad, good_left]), _frame(1, [good_right]))
|
||||
assert len(records) == 1
|
||||
assert records[0].valid is True
|
||||
assert any("invalid_box" in message for message in caplog.messages)
|
||||
|
||||
|
||||
def test_consistency_degrades_then_invalidates(caplog):
|
||||
ranger = StereoRanger()
|
||||
disparities = [9.5, 13.5, 17.5, 21.5, 25.5, 29.5] # 每帧跳变 4 px > 3 px
|
||||
records = []
|
||||
with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
|
||||
for index, d_px in enumerate(disparities, start=1):
|
||||
left, right = _pair_with_disparity(d_px)
|
||||
records.append(ranger.update(_frame(index, [left]), _frame(index, [right]))[0])
|
||||
|
||||
assert [record.valid for record in records] == [True, True, True, True, True, False]
|
||||
assert records[3].confidence == pytest.approx(0.5) # 第 3 帧起置信度降级（N = 3）
|
||||
assert records[5].forward_distance_m is None
|
||||
assert records[5].confidence == pytest.approx(0.0)
|
||||
assert any("consistency_invalid" in message for message in caplog.messages)
|
||||
|
||||
|
||||
def test_track_id_keeps_state_across_large_pixel_move():
|
||||
ranger = StereoRanger()
|
||||
left_1, right_1 = _boxes(x_center_m=0.6, z_m=4.0, track_id=7)
|
||||
left_2, right_2 = _boxes(x_center_m=0.0, z_m=4.5, track_id=7) # c_mid 移动 144 px > 门限
|
||||
ranger.update(_frame(1, [left_1]), _frame(1, [right_1]))
|
||||
records = ranger.update(_frame(2, [left_2]), _frame(2, [right_2]))
|
||||
assert records[0].track_id == 7
|
||||
assert records[0].forward_distance_m == pytest.approx(4.0 + 0.3 * (4.5 - 4.0))
|
||||
|
||||
|
||||
def test_without_track_id_large_move_starts_new_state():
|
||||
ranger = StereoRanger()
|
||||
left_1, right_1 = _boxes(x_center_m=0.6, z_m=4.0)
|
||||
left_2, right_2 = _boxes(x_center_m=0.0, z_m=4.5) # 最近邻越门限 → 新目标状态
|
||||
ranger.update(_frame(1, [left_1]), _frame(1, [right_1]))
|
||||
records = ranger.update(_frame(2, [left_2]), _frame(2, [right_2]))
|
||||
assert records[0].forward_distance_m == pytest.approx(4.5) # 未平滑
|
||||
|
||||
|
||||
def test_custom_matcher_injection():
|
||||
class _NoMatchMatcher:
|
||||
def match(self, left, right, config) -> List[MatchPair]:
|
||||
return []
|
||||
|
||||
ranger = StereoRanger(matcher=_NoMatchMatcher())
|
||||
left, right = _boxes(z_m=6.0)
|
||||
records = ranger.update(_frame(1, [left]), _frame(1, [right]))
|
||||
assert len(records) == 2
|
||||
assert all(record.degradation is Degradation.BEARING_ONLY for record in records)
|
||||
|
||||
|
||||
def test_reset_clears_state():
|
||||
ranger = StereoRanger()
|
||||
left_1, right_1 = _boxes(z_m=6.0)
|
||||
left_2, right_2 = _boxes(z_m=6.5)
|
||||
ranger.update(_frame(1, [left_1]), _frame(1, [right_1]))
|
||||
ranger.reset()
|
||||
records = ranger.update(_frame(2, [left_2]), _frame(2, [right_2]))
|
||||
assert records[0].forward_distance_m == pytest.approx(6.5) # 未平滑（状态已清空）
|
||||
@@ -0,0 +1,73 @@
|
||||
"""目标隔离测试（规格 §4：track_id 优先 + 框位置最近邻、帧末老化清理）。"""
|
||||
|
||||
from stereo_ranger import StereoConfig, TargetStore
|
||||
|
||||
CFG = StereoConfig() # isolation_gate_px = 50，filter_reset_frames = 5
|
||||
|
||||
|
||||
def test_track_key_stable_across_position_move():
|
||||
store = TargetStore(CFG)
|
||||
key_1 = store.resolve(0, 7, (100.0, 100.0), 1)
|
||||
key_2 = store.resolve(0, 7, (400.0, 100.0), 2) # 位置大幅移动仍是同一目标
|
||||
assert key_1 == key_2
|
||||
assert store.count == 1
|
||||
|
||||
|
||||
def test_nearest_neighbor_keeps_identity_within_gate():
|
||||
store = TargetStore(CFG)
|
||||
key_1 = store.resolve(0, None, (100.0, 100.0), 1)
|
||||
key_2 = store.resolve(0, None, (130.0, 140.0), 2) # 距离 50 ≤ 门限
|
||||
assert key_1 == key_2
|
||||
|
||||
|
||||
def test_beyond_gate_creates_new_key():
|
||||
store = TargetStore(CFG)
|
||||
key_1 = store.resolve(0, None, (100.0, 100.0), 1)
|
||||
key_2 = store.resolve(0, None, (200.0, 100.0), 2) # 距离 100 > 门限
|
||||
assert key_1 != key_2
|
||||
assert store.count == 2
|
||||
|
||||
|
||||
def test_class_id_separates_states():
|
||||
store = TargetStore(CFG)
|
||||
assert store.resolve(0, None, (100.0, 100.0), 1) != store.resolve(1, None, (100.0, 100.0), 1)
|
||||
|
||||
|
||||
def test_two_boxes_same_frame_claim_distinct_states():
|
||||
store = TargetStore(CFG)
|
||||
key_1 = store.resolve(0, None, (100.0, 100.0), 1)
|
||||
key_2 = store.resolve(0, None, (110.0, 100.0), 1) # 同帧第二框不得接管已占用状态
|
||||
assert key_1 != key_2
|
||||
assert store.count == 2
|
||||
|
||||
|
||||
def test_state_evicted_after_reset_frames_unseen():
|
||||
store = TargetStore(CFG)
|
||||
key = store.resolve(0, None, (100.0, 100.0), 1)
|
||||
store.distance_filter.advance(key, 5.0)
|
||||
store.consistency.advance(key, 9.6)
|
||||
for frame in range(2, 7): # 第 2~6 帧：连续 5 帧缺席
|
||||
store.end_frame(frame)
|
||||
assert store.count == 0
|
||||
assert store.distance_filter.value(key) is None
|
||||
assert store.consistency.streak(key) == 0
|
||||
|
||||
|
||||
def test_present_but_invalid_keeps_position_state():
|
||||
store = TargetStore(CFG)
|
||||
key = store.resolve(0, None, (100.0, 100.0), 1)
|
||||
store.distance_filter.advance(key, 5.0)
|
||||
for frame in range(2, 8):
|
||||
store.resolve(0, None, (100.0, 100.0), frame) # 每帧仍出现
|
||||
store.distance_filter.invalidate(key)
|
||||
assert store.count == 1 # 位置状态保留
|
||||
assert store.distance_filter.value(key) is None # 滤波状态已按门限清空
|
||||
|
||||
|
||||
def test_clear_resets_everything():
|
||||
store = TargetStore(CFG)
|
||||
key = store.resolve(0, 5, (100.0, 100.0), 1)
|
||||
store.distance_filter.advance(key, 5.0)
|
||||
store.clear()
|
||||
assert store.count == 0
|
||||
assert store.distance_filter.value(key) is None
|
||||
@@ -0,0 +1,45 @@
|
||||
"""告警节流测试（规格 §5：第 1 次 + 每满 100 次，带累计计数）。"""
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import WarnThrottle
|
||||
|
||||
|
||||
def test_first_and_every_hundred_emit():
|
||||
throttle = WarnThrottle() # 默认 every=100
|
||||
assert throttle.should_emit("a") is True
|
||||
for _ in range(98):
|
||||
assert throttle.should_emit("a") is False
|
||||
assert throttle.should_emit("a") is True # 第 100 次
|
||||
for _ in range(99):
|
||||
assert throttle.should_emit("a") is False
|
||||
assert throttle.should_emit("a") is True # 第 200 次
|
||||
assert throttle.count("a") == 200
|
||||
|
||||
|
||||
def test_categories_are_independent():
|
||||
throttle = WarnThrottle()
|
||||
assert throttle.should_emit("a") is True
|
||||
assert throttle.should_emit("b") is True
|
||||
assert throttle.count("a") == 1
|
||||
assert throttle.count("b") == 1
|
||||
assert throttle.count("never_seen") == 0
|
||||
|
||||
|
||||
def test_custom_interval():
|
||||
throttle = WarnThrottle(every=3)
|
||||
# 规格语义"第 1 次 + 计数每满 every" → 第 1/3/6 次打印
|
||||
assert [throttle.should_emit("a") for _ in range(6)] == [True, False, True, False, False, True]
|
||||
|
||||
|
||||
def test_clear_resets_counts():
|
||||
throttle = WarnThrottle()
|
||||
throttle.should_emit("a")
|
||||
throttle.clear()
|
||||
assert throttle.count("a") == 0
|
||||
assert throttle.should_emit("a") is True
|
||||
|
||||
|
||||
def test_invalid_interval():
|
||||
with pytest.raises(ValueError):
|
||||
WarnThrottle(every=0)
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,96 @@
|
||||
"""StereoYoloConfig 校验与 JSON 加载测试（需求说明 §4：配置外置、fail-fast）。"""
|
||||
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_yolo import StereoYoloConfig, load_from_json
|
||||
|
||||
CONFIG_PATH = Path(__file__).resolve().parents[2] / "config" / "stereo_yolo.json"
|
||||
|
||||
|
||||
def _config_dict() -> dict:
|
||||
return json.loads(CONFIG_PATH.read_text(encoding="utf-8"))
|
||||
|
||||
|
||||
def _write(tmp_path: Path, data: dict) -> str:
|
||||
path = tmp_path / "stereo_yolo.json"
|
||||
path.write_text(json.dumps(data, ensure_ascii=False), encoding="utf-8")
|
||||
return str(path)
|
||||
|
||||
|
||||
def test_load_from_json_matches_defaults():
|
||||
assert load_from_json(str(CONFIG_PATH)) == StereoYoloConfig()
|
||||
|
||||
|
||||
def test_whitelist_normalized_to_tuple():
|
||||
cfg = StereoYoloConfig(class_whitelist=[0, 2])
|
||||
assert cfg.class_whitelist == (0, 2)
|
||||
|
||||
|
||||
def test_missing_field_rejected(tmp_path):
|
||||
data = _config_dict()
|
||||
data.pop("imgsz")
|
||||
with pytest.raises(ValueError, match="缺少配置字段"):
|
||||
load_from_json(_write(tmp_path, data))
|
||||
|
||||
|
||||
def test_unknown_field_rejected(tmp_path):
|
||||
data = _config_dict()
|
||||
data["letterbox_scale"] = 0.5
|
||||
with pytest.raises(ValueError, match="未知配置字段"):
|
||||
load_from_json(_write(tmp_path, data))
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"field,value",
|
||||
[
|
||||
("model_path", 123),
|
||||
("imgsz", "640"),
|
||||
("imgsz", True),
|
||||
("conf_threshold", "0.25"),
|
||||
("iou_threshold", None),
|
||||
("class_whitelist", 0),
|
||||
("class_whitelist", ["0"]),
|
||||
("device", 5),
|
||||
("concurrency", 1),
|
||||
],
|
||||
)
|
||||
def test_type_error_rejected(tmp_path, field, value):
|
||||
data = _config_dict()
|
||||
data[field] = value
|
||||
with pytest.raises(ValueError, match="配置类型非法"):
|
||||
load_from_json(_write(tmp_path, data))
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"field,value",
|
||||
[
|
||||
("model_path", ""),
|
||||
("imgsz", 0),
|
||||
("imgsz", 600),
|
||||
("conf_threshold", -0.1),
|
||||
("conf_threshold", 1.5),
|
||||
("iou_threshold", 0.0),
|
||||
("class_whitelist", []),
|
||||
("class_whitelist", [-1]),
|
||||
("device", " "),
|
||||
("concurrency", "batch"),
|
||||
],
|
||||
)
|
||||
def test_invalid_values_rejected(field, value):
|
||||
with pytest.raises(ValueError, match="配置非法"):
|
||||
StereoYoloConfig(**{field: value})
|
||||
|
||||
|
||||
def test_boundary_values_accepted():
|
||||
cfg = StereoYoloConfig(
|
||||
imgsz=32,
|
||||
conf_threshold=0.0,
|
||||
iou_threshold=1.0,
|
||||
class_whitelist=(0,),
|
||||
device="cpu",
|
||||
concurrency="sequential",
|
||||
)
|
||||
assert cfg.imgsz == 32
|
||||
@@ -0,0 +1,155 @@
|
||||
"""StereoYoloFrontend 出口契约测试（需求说明 §2.2/§2.3/§2.5/§5）。
|
||||
|
||||
全部用注入的假检测器驱动：不需要模型权重、不需要图像素材、不需要硬件；
|
||||
真并行用双人屏障断言（串行实现会屏障超时）。
|
||||
"""
|
||||
|
||||
import logging
|
||||
import threading
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from stereo_ranger.types import Detection
|
||||
from stereo_yolo import StereoYoloConfig, StereoYoloFrontend
|
||||
|
||||
|
||||
class ScriptedDetector:
|
||||
"""按调用顺序返回脚本项：list[Detection] 正常返回；Exception 抛出。"""
|
||||
|
||||
def __init__(self, script):
|
||||
self._script = list(script)
|
||||
self.calls = 0
|
||||
|
||||
def detect(self, image):
|
||||
self.calls += 1
|
||||
if not self._script:
|
||||
raise AssertionError("假检测器脚本已耗尽（调用次数多于预期）")
|
||||
item = self._script.pop(0)
|
||||
if isinstance(item, Exception):
|
||||
raise item
|
||||
return list(item)
|
||||
|
||||
|
||||
class BarrierDetector:
|
||||
"""两次调用必须同时在途（屏障超时即证明串行或单线程执行）。"""
|
||||
|
||||
def __init__(self, barrier: threading.Barrier, detections):
|
||||
self._barrier = barrier
|
||||
self._detections = list(detections)
|
||||
|
||||
def detect(self, image):
|
||||
self._barrier.wait(timeout=2.0)
|
||||
return list(self._detections)
|
||||
|
||||
|
||||
def _img(fill=0, shape=(8, 8, 3)) -> np.ndarray:
|
||||
return np.full(shape, fill, dtype=np.uint8)
|
||||
|
||||
|
||||
def _det(x1=1.0, y1=2.0, x2=11.0, y2=12.0, class_id=0, confidence=0.9, track_id=None) -> Detection:
|
||||
return Detection(x1, y1, x2, y2, confidence=confidence, class_id=class_id, track_id=track_id)
|
||||
|
||||
|
||||
def _frontend(script, **cfg_kwargs) -> StereoYoloFrontend:
|
||||
kwargs = {"concurrency": "sequential"}
|
||||
kwargs.update(cfg_kwargs)
|
||||
return StereoYoloFrontend(StereoYoloConfig(**kwargs), detector=ScriptedDetector(script))
|
||||
|
||||
|
||||
def test_frames_share_frame_id_and_keep_boxes():
|
||||
left_box, right_box = _det(1.0, 2.0, 11.0, 12.0, confidence=0.77), _det(3.0, 4.0, 13.0, 14.0)
|
||||
frontend = _frontend([[left_box], [right_box]])
|
||||
left_frame, right_frame = frontend.detect(_img(), _img(), frame_id=7)
|
||||
|
||||
assert left_frame.frame_id == right_frame.frame_id == 7
|
||||
assert left_frame.detections == (left_box,)
|
||||
assert right_frame.detections == (right_box,)
|
||||
assert left_frame.detections[0].confidence == pytest.approx(0.77)
|
||||
assert left_frame.detections[0].track_id is None # 无追踪器时留空（§2.1）
|
||||
assert frontend._detector.calls == 2
|
||||
|
||||
|
||||
def test_whitelist_filters_by_class():
|
||||
frontend = _frontend([[_det(class_id=3)], [_det(class_id=0)]], class_whitelist=(0,))
|
||||
left_frame, right_frame = frontend.detect(_img(), _img(), frame_id=1)
|
||||
|
||||
assert left_frame.detections == ()
|
||||
assert len(right_frame.detections) == 1 and right_frame.detections[0].class_id == 0
|
||||
|
||||
|
||||
def test_no_whitelist_keeps_all_classes():
|
||||
frontend = _frontend([[_det(class_id=3)], [_det(class_id=5)]])
|
||||
left_frame, right_frame = frontend.detect(_img(), _img(), frame_id=1)
|
||||
|
||||
assert len(left_frame.detections) == 1 and len(right_frame.detections) == 1
|
||||
|
||||
|
||||
def test_degenerate_boxes_dropped(caplog):
|
||||
nan_box = _det(x1=float("nan"))
|
||||
zero_width = _det(x1=5.0, x2=5.0)
|
||||
inverted = _det(x1=20.0, x2=10.0)
|
||||
good = _det()
|
||||
frontend = _frontend([[nan_box, zero_width, inverted, good], []])
|
||||
|
||||
with caplog.at_level(logging.WARNING, logger="stereo_yolo"):
|
||||
left_frame, right_frame = frontend.detect(_img(), _img(), frame_id=2)
|
||||
|
||||
assert left_frame.detections == (good,)
|
||||
assert right_frame.detections == ()
|
||||
assert "丢弃退化检测框" in caplog.text
|
||||
assert "累计 1 次" in caplog.text
|
||||
|
||||
|
||||
def test_boundary_touching_box_kept():
|
||||
hairline = _det(x1=10.0, x2=10.0 + 1e-9)
|
||||
frontend = _frontend([[hairline], []])
|
||||
left_frame, _ = frontend.detect(_img(), _img(), frame_id=3)
|
||||
assert left_frame.detections == (hairline,)
|
||||
|
||||
|
||||
def test_one_eye_failure_isolated(caplog):
|
||||
good = _det()
|
||||
frontend = _frontend([[good], RuntimeError("boom")])
|
||||
|
||||
with caplog.at_level(logging.WARNING, logger="stereo_yolo"):
|
||||
left_frame, right_frame = frontend.detect(_img(), _img(), frame_id=4)
|
||||
|
||||
assert left_frame.detections == (good,)
|
||||
assert right_frame.detections == ()
|
||||
assert left_frame.frame_id == right_frame.frame_id == 4
|
||||
assert "右目推理失败" in caplog.text
|
||||
|
||||
|
||||
def test_shape_mismatch_rejected(caplog):
|
||||
frontend = _frontend([]) # 脚本为空：若检测器被调用会抛断言
|
||||
|
||||
with caplog.at_level(logging.WARNING, logger="stereo_yolo"):
|
||||
left_frame, right_frame = frontend.detect(_img(shape=(8, 8, 3)), _img(shape=(8, 6, 3)), frame_id=5)
|
||||
|
||||
assert left_frame.detections == () and right_frame.detections == ()
|
||||
assert left_frame.frame_id == right_frame.frame_id == 5
|
||||
assert frontend._detector.calls == 0
|
||||
assert "左右图尺寸不一致" in caplog.text
|
||||
|
||||
|
||||
def test_parallel_eyes_run_concurrently():
|
||||
barrier = threading.Barrier(2)
|
||||
box = _det()
|
||||
frontend = StereoYoloFrontend(
|
||||
StereoYoloConfig(concurrency="parallel"),
|
||||
detector=BarrierDetector(barrier, [box]),
|
||||
)
|
||||
try:
|
||||
left_frame, right_frame = frontend.detect(_img(), _img(), frame_id=6)
|
||||
finally:
|
||||
frontend.close()
|
||||
|
||||
assert left_frame.detections == (box,)
|
||||
assert right_frame.detections == (box,)
|
||||
|
||||
|
||||
def test_close_is_idempotent():
|
||||
frontend = StereoYoloFrontend(StereoYoloConfig(concurrency="parallel"), detector=ScriptedDetector([]))
|
||||
frontend.close()
|
||||
frontend.close()
|
||||
@@ -0,0 +1,27 @@
|
||||
"""split_sbs 裁切测试（需求说明 §2.3：sbs 单流形态）。"""
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from stereo_yolo import split_sbs
|
||||
|
||||
|
||||
def test_split_returns_left_and_right_halves():
|
||||
image = np.zeros((4, 10, 3), dtype=np.uint8)
|
||||
image[:, :5] = 1
|
||||
image[:, 5:] = 2
|
||||
|
||||
left, right = split_sbs(image)
|
||||
|
||||
assert left.shape == (4, 5, 3) and right.shape == (4, 5, 3)
|
||||
assert (left == 1).all() and (right == 2).all()
|
||||
|
||||
|
||||
def test_odd_width_rejected():
|
||||
with pytest.raises(ValueError, match="宽度须为偶数"):
|
||||
split_sbs(np.zeros((4, 9, 3), dtype=np.uint8))
|
||||
|
||||
|
||||
def test_non_image_rejected():
|
||||
with pytest.raises(ValueError, match="宽度须为偶数"):
|
||||
split_sbs(np.zeros((9,), dtype=np.uint8))
|
||||
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Reference in New Issue
Block a user