diff --git a/__pycache__/test.cpython-312.pyc b/__pycache__/test.cpython-312.pyc new file mode 100644 index 0000000..d96a712 Binary files /dev/null and b/__pycache__/test.cpython-312.pyc differ diff --git a/docs/双目YOLO目标识别需求说明.md b/docs/双目YOLO目标识别需求说明.md new file mode 100644 index 0000000..8e5a0a2 --- /dev/null +++ b/docs/双目YOLO目标识别需求说明.md @@ -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 约束——标定完成前内参等数值均为占位值,仅限台架/回放验证,不得接入实飞控制回路。 diff --git a/models/yolo26n-drone.pt b/models/yolo26n-drone.pt new file mode 100644 index 0000000..fcfea4c Binary files /dev/null and b/models/yolo26n-drone.pt differ diff --git a/utils/README.md b/utils/README.md index 3829630..c2c6764 100644 --- a/utils/README.md +++ b/utils/README.md @@ -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 合成图,真实双目素材到位后需重跑联调。 diff --git a/utils/__pycache__/demo_stereo_yolo.cpython-312.pyc b/utils/__pycache__/demo_stereo_yolo.cpython-312.pyc new file mode 100644 index 0000000..7794ef4 Binary files /dev/null and b/utils/__pycache__/demo_stereo_yolo.cpython-312.pyc differ diff --git a/utils/__pycache__/demo_track_control.cpython-312.pyc b/utils/__pycache__/demo_track_control.cpython-312.pyc new file mode 100644 index 0000000..7103de5 Binary files /dev/null and b/utils/__pycache__/demo_track_control.cpython-312.pyc differ diff --git a/utils/assets/stereo_yolo/drone_sky.png b/utils/assets/stereo_yolo/drone_sky.png new file mode 100644 index 0000000..9f22d89 Binary files /dev/null and b/utils/assets/stereo_yolo/drone_sky.png differ diff --git a/utils/config/stereo_ranger.json b/utils/config/stereo_ranger.json new file mode 100644 index 0000000..95d1443 --- /dev/null +++ b/utils/config/stereo_ranger.json @@ -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 +} diff --git a/utils/config/stereo_yolo.json b/utils/config/stereo_yolo.json new file mode 100644 index 0000000..d0fc841 --- /dev/null +++ b/utils/config/stereo_yolo.json @@ -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" +} diff --git a/utils/demo_stereo_ranger.py b/utils/demo_stereo_ranger.py new file mode 100644 index 0000000..518af95 --- /dev/null +++ b/utils/demo_stereo_ranger.py @@ -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 # 指定配置 + +⚠ 所有参数均为占位值(焦距/标定待厂商数据):标定与地面真值验证完成前, + 本模块输出仅限静态标靶/回放/台架验证,不得作为飞行控制决策依据 + (规格 §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() diff --git a/utils/demo_stereo_yolo.py b/utils/demo_stereo_yolo.py new file mode 100644 index 0000000..4c7579a --- /dev/null +++ b/utils/demo_stereo_yolo.py @@ -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() diff --git a/utils/pytest.ini b/utils/pytest.ini index eb6a8ff..e339b30 100644 --- a/utils/pytest.ini +++ b/utils/pytest.ini @@ -1,3 +1,6 @@ [pytest] testpaths = tests python_files = *_test.py test_*.py +; 各包测试文件名与 C++ 测试文件对齐,跨包存在同名(如 config_test.py): +; importlib 模式使 pytest 不再按裸模块名导入,避免 basename 冲突。 +addopts = --import-mode=importlib diff --git a/utils/stereo_ranger/__pycache__/__init__.cpython-312.pyc b/utils/stereo_ranger/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 0000000..d35fad2 Binary files /dev/null and b/utils/stereo_ranger/__pycache__/__init__.cpython-312.pyc differ diff --git a/utils/stereo_ranger/__pycache__/config.cpython-312.pyc b/utils/stereo_ranger/__pycache__/config.cpython-312.pyc new file mode 100644 index 0000000..9a20814 Binary files /dev/null and b/utils/stereo_ranger/__pycache__/config.cpython-312.pyc differ diff --git a/utils/stereo_ranger/__pycache__/consistency.cpython-312.pyc b/utils/stereo_ranger/__pycache__/consistency.cpython-312.pyc new file mode 100644 index 0000000..74faed1 Binary files /dev/null and b/utils/stereo_ranger/__pycache__/consistency.cpython-312.pyc differ diff --git a/utils/stereo_ranger/__pycache__/distance_filter.cpython-312.pyc b/utils/stereo_ranger/__pycache__/distance_filter.cpython-312.pyc new file mode 100644 index 0000000..0f2c03b Binary files /dev/null and b/utils/stereo_ranger/__pycache__/distance_filter.cpython-312.pyc differ diff --git a/utils/stereo_ranger/__pycache__/geometry.cpython-312.pyc b/utils/stereo_ranger/__pycache__/geometry.cpython-312.pyc new file mode 100644 index 0000000..867f375 Binary files /dev/null and b/utils/stereo_ranger/__pycache__/geometry.cpython-312.pyc differ diff --git a/utils/stereo_ranger/__pycache__/matching.cpython-312.pyc b/utils/stereo_ranger/__pycache__/matching.cpython-312.pyc new file mode 100644 index 0000000..e185f58 Binary files /dev/null and b/utils/stereo_ranger/__pycache__/matching.cpython-312.pyc differ diff --git a/utils/stereo_ranger/__pycache__/sampling.cpython-312.pyc b/utils/stereo_ranger/__pycache__/sampling.cpython-312.pyc new file mode 100644 index 0000000..1e465a3 Binary files /dev/null and b/utils/stereo_ranger/__pycache__/sampling.cpython-312.pyc differ diff --git a/utils/stereo_ranger/__pycache__/stereo_ranger.cpython-312.pyc b/utils/stereo_ranger/__pycache__/stereo_ranger.cpython-312.pyc new file mode 100644 index 0000000..70df4cc Binary files /dev/null and b/utils/stereo_ranger/__pycache__/stereo_ranger.cpython-312.pyc differ diff --git a/utils/stereo_ranger/__pycache__/target_store.cpython-312.pyc b/utils/stereo_ranger/__pycache__/target_store.cpython-312.pyc new file mode 100644 index 0000000..37b17a2 Binary files /dev/null and b/utils/stereo_ranger/__pycache__/target_store.cpython-312.pyc differ diff --git a/utils/stereo_ranger/__pycache__/types.cpython-312.pyc b/utils/stereo_ranger/__pycache__/types.cpython-312.pyc new file mode 100644 index 0000000..60e0174 Binary files /dev/null and b/utils/stereo_ranger/__pycache__/types.cpython-312.pyc differ diff --git a/utils/stereo_ranger/__pycache__/warn_throttle.cpython-312.pyc b/utils/stereo_ranger/__pycache__/warn_throttle.cpython-312.pyc new file mode 100644 index 0000000..52747a4 Binary files /dev/null and b/utils/stereo_ranger/__pycache__/warn_throttle.cpython-312.pyc differ diff --git a/utils/stereo_yolo/README.md b/utils/stereo_yolo/README.md new file mode 100644 index 0000000..d2b9647 --- /dev/null +++ b/utils/stereo_yolo/README.md @@ -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) | diff --git a/utils/stereo_yolo/__init__.py b/utils/stereo_yolo/__init__.py new file mode 100644 index 0000000..ac58164 --- /dev/null +++ b/utils/stereo_yolo/__init__.py @@ -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", +] diff --git a/utils/stereo_yolo/__pycache__/__init__.cpython-312.pyc b/utils/stereo_yolo/__pycache__/__init__.cpython-312.pyc new file mode 100644 index 0000000..5e4c54e Binary files /dev/null and b/utils/stereo_yolo/__pycache__/__init__.cpython-312.pyc differ diff --git a/utils/stereo_yolo/__pycache__/config.cpython-312.pyc b/utils/stereo_yolo/__pycache__/config.cpython-312.pyc new file mode 100644 index 0000000..147678c Binary files /dev/null and b/utils/stereo_yolo/__pycache__/config.cpython-312.pyc differ diff --git a/utils/stereo_yolo/__pycache__/detector.cpython-312.pyc b/utils/stereo_yolo/__pycache__/detector.cpython-312.pyc new file mode 100644 index 0000000..b85d3cc Binary files /dev/null and b/utils/stereo_yolo/__pycache__/detector.cpython-312.pyc differ diff --git a/utils/stereo_yolo/__pycache__/frontend.cpython-312.pyc b/utils/stereo_yolo/__pycache__/frontend.cpython-312.pyc new file mode 100644 index 0000000..4078101 Binary files /dev/null and b/utils/stereo_yolo/__pycache__/frontend.cpython-312.pyc differ diff --git a/utils/stereo_yolo/__pycache__/sbs.cpython-312.pyc b/utils/stereo_yolo/__pycache__/sbs.cpython-312.pyc new file mode 100644 index 0000000..93fab62 Binary files /dev/null and b/utils/stereo_yolo/__pycache__/sbs.cpython-312.pyc differ diff --git a/utils/stereo_yolo/__pycache__/warn_throttle.cpython-312.pyc b/utils/stereo_yolo/__pycache__/warn_throttle.cpython-312.pyc new file mode 100644 index 0000000..681ae27 Binary files /dev/null and b/utils/stereo_yolo/__pycache__/warn_throttle.cpython-312.pyc differ diff --git a/utils/stereo_yolo/config.py b/utils/stereo_yolo/config.py new file mode 100644 index 0000000..ae3fea4 --- /dev/null +++ b/utils/stereo_yolo/config.py @@ -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}") diff --git a/utils/stereo_yolo/detector.py b/utils/stereo_yolo/detector.py new file mode 100644 index 0000000..c8e58c4 --- /dev/null +++ b/utils/stereo_yolo/detector.py @@ -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() diff --git a/utils/stereo_yolo/frontend.py b/utils/stereo_yolo/frontend.py new file mode 100644 index 0000000..d616f3e --- /dev/null +++ b/utils/stereo_yolo/frontend.py @@ -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 diff --git a/utils/stereo_yolo/sbs.py b/utils/stereo_yolo/sbs.py new file mode 100644 index 0000000..e9b8613 --- /dev/null +++ b/utils/stereo_yolo/sbs.py @@ -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() diff --git a/utils/stereo_yolo/warn_throttle.py b/utils/stereo_yolo/warn_throttle.py new file mode 100644 index 0000000..fcc66d2 --- /dev/null +++ b/utils/stereo_yolo/warn_throttle.py @@ -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) diff --git a/utils/tests/stereo_ranger/__pycache__/config_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_ranger/__pycache__/config_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..512adf7 Binary files /dev/null and b/utils/tests/stereo_ranger/__pycache__/config_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/stereo_ranger/__pycache__/consistency_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_ranger/__pycache__/consistency_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..d496e09 Binary files /dev/null and b/utils/tests/stereo_ranger/__pycache__/consistency_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/stereo_ranger/__pycache__/distance_filter_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_ranger/__pycache__/distance_filter_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..4fceab3 Binary files /dev/null and b/utils/tests/stereo_ranger/__pycache__/distance_filter_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/stereo_ranger/__pycache__/geometry_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_ranger/__pycache__/geometry_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..9c90b65 Binary files /dev/null and b/utils/tests/stereo_ranger/__pycache__/geometry_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/stereo_ranger/__pycache__/matching_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_ranger/__pycache__/matching_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..1bd698d Binary files /dev/null and b/utils/tests/stereo_ranger/__pycache__/matching_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/stereo_ranger/__pycache__/sampling_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_ranger/__pycache__/sampling_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..a4a0409 Binary files /dev/null and b/utils/tests/stereo_ranger/__pycache__/sampling_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/stereo_ranger/__pycache__/stereo_ranger_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_ranger/__pycache__/stereo_ranger_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..7327cc5 Binary files /dev/null and b/utils/tests/stereo_ranger/__pycache__/stereo_ranger_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/stereo_ranger/__pycache__/target_store_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_ranger/__pycache__/target_store_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..10e67c5 Binary files /dev/null and b/utils/tests/stereo_ranger/__pycache__/target_store_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/stereo_ranger/__pycache__/warn_throttle_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_ranger/__pycache__/warn_throttle_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..81acf03 Binary files /dev/null and b/utils/tests/stereo_ranger/__pycache__/warn_throttle_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/stereo_ranger/config_test.py b/utils/tests/stereo_ranger/config_test.py new file mode 100644 index 0000000..6d2dba5 --- /dev/null +++ b/utils/tests/stereo_ranger/config_test.py @@ -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 diff --git a/utils/tests/stereo_ranger/consistency_test.py b/utils/tests/stereo_ranger/consistency_test.py new file mode 100644 index 0000000..7bd3374 --- /dev/null +++ b/utils/tests/stereo_ranger/consistency_test.py @@ -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) diff --git a/utils/tests/stereo_ranger/distance_filter_test.py b/utils/tests/stereo_ranger/distance_filter_test.py new file mode 100644 index 0000000..e08adce --- /dev/null +++ b/utils/tests/stereo_ranger/distance_filter_test.py @@ -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) diff --git a/utils/tests/stereo_ranger/geometry_test.py b/utils/tests/stereo_ranger/geometry_test.py new file mode 100644 index 0000000..c9d74e5 --- /dev/null +++ b/utils/tests/stereo_ranger/geometry_test.py @@ -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) diff --git a/utils/tests/stereo_ranger/matching_test.py b/utils/tests/stereo_ranger/matching_test.py new file mode 100644 index 0000000..6fbd5de --- /dev/null +++ b/utils/tests/stereo_ranger/matching_test.py @@ -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) == [] diff --git a/utils/tests/stereo_ranger/sampling_test.py b/utils/tests/stereo_ranger/sampling_test.py new file mode 100644 index 0000000..0400619 --- /dev/null +++ b/utils/tests/stereo_ranger/sampling_test.py @@ -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 diff --git a/utils/tests/stereo_ranger/stereo_ranger_test.py b/utils/tests/stereo_ranger/stereo_ranger_test.py new file mode 100644 index 0000000..8dc4477 --- /dev/null +++ b/utils/tests/stereo_ranger/stereo_ranger_test.py @@ -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) # 未平滑(状态已清空) diff --git a/utils/tests/stereo_ranger/target_store_test.py b/utils/tests/stereo_ranger/target_store_test.py new file mode 100644 index 0000000..f0f2e72 --- /dev/null +++ b/utils/tests/stereo_ranger/target_store_test.py @@ -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 diff --git a/utils/tests/stereo_ranger/warn_throttle_test.py b/utils/tests/stereo_ranger/warn_throttle_test.py new file mode 100644 index 0000000..bd96854 --- /dev/null +++ b/utils/tests/stereo_ranger/warn_throttle_test.py @@ -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) diff --git a/utils/tests/stereo_yolo/__pycache__/config_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_yolo/__pycache__/config_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..ee6a2b5 Binary files /dev/null and b/utils/tests/stereo_yolo/__pycache__/config_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/stereo_yolo/__pycache__/frontend_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_yolo/__pycache__/frontend_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..03e2558 Binary files /dev/null and b/utils/tests/stereo_yolo/__pycache__/frontend_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/stereo_yolo/__pycache__/sbs_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/stereo_yolo/__pycache__/sbs_test.cpython-312-pytest-9.1.1.pyc new file mode 100644 index 0000000..565f880 Binary files /dev/null and b/utils/tests/stereo_yolo/__pycache__/sbs_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/stereo_yolo/config_test.py b/utils/tests/stereo_yolo/config_test.py new file mode 100644 index 0000000..1d50c35 --- /dev/null +++ b/utils/tests/stereo_yolo/config_test.py @@ -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 diff --git a/utils/tests/stereo_yolo/frontend_test.py b/utils/tests/stereo_yolo/frontend_test.py new file mode 100644 index 0000000..4514e8b --- /dev/null +++ b/utils/tests/stereo_yolo/frontend_test.py @@ -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() diff --git a/utils/tests/stereo_yolo/sbs_test.py b/utils/tests/stereo_yolo/sbs_test.py new file mode 100644 index 0000000..8637334 --- /dev/null +++ b/utils/tests/stereo_yolo/sbs_test.py @@ -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)) diff --git a/utils/tests/visual_track_control/__pycache__/tracking_control_law_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/tracking_control_law_test.cpython-312-pytest-9.1.1.pyc index e8f9888..71535d5 100644 Binary files a/utils/tests/visual_track_control/__pycache__/tracking_control_law_test.cpython-312-pytest-9.1.1.pyc and b/utils/tests/visual_track_control/__pycache__/tracking_control_law_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/tests/visual_track_control/__pycache__/vtc_config_test.cpython-312-pytest-9.1.1.pyc b/utils/tests/visual_track_control/__pycache__/vtc_config_test.cpython-312-pytest-9.1.1.pyc index 3b76ce2..24c532d 100644 Binary files a/utils/tests/visual_track_control/__pycache__/vtc_config_test.cpython-312-pytest-9.1.1.pyc and b/utils/tests/visual_track_control/__pycache__/vtc_config_test.cpython-312-pytest-9.1.1.pyc differ diff --git a/utils/visual_track_control/__pycache__/config.cpython-312.pyc b/utils/visual_track_control/__pycache__/config.cpython-312.pyc index c320817..35c5618 100644 Binary files a/utils/visual_track_control/__pycache__/config.cpython-312.pyc and b/utils/visual_track_control/__pycache__/config.cpython-312.pyc differ diff --git a/utils/visual_track_control/__pycache__/tracking_control_law.cpython-312.pyc b/utils/visual_track_control/__pycache__/tracking_control_law.cpython-312.pyc index 20b3895..19ae3d7 100644 Binary files a/utils/visual_track_control/__pycache__/tracking_control_law.cpython-312.pyc and b/utils/visual_track_control/__pycache__/tracking_control_law.cpython-312.pyc differ diff --git a/utils/visual_track_control/__pycache__/types.cpython-312.pyc b/utils/visual_track_control/__pycache__/types.cpython-312.pyc index 528ab34..09cc3b1 100644 Binary files a/utils/visual_track_control/__pycache__/types.cpython-312.pyc and b/utils/visual_track_control/__pycache__/types.cpython-312.pyc differ