keita
df91b1110e
归档 legacy/：stereo_yolo/stereo_ranger/target_tracker 三包 + 175 条测试 + demo/配置/权重/三份规格；运行链路不再依赖，默认 pytest 446 条，归档测试按需运行。 目标有效性第二判据改为 LOS 运动学一致性（pod_bridge/los_consistency.py），删除 require_vision；pod_locked() 叠加该判据，状态机随之后退重搜。 自定义返航（task_runner/return_home.py）：保持转场高度回起飞点 → 恢复初始航向 → 垂直降落，不再爬 RTL_RETURN_ALT；未起飞/无起飞点/超时/异常仍退回 PX4 原生 RTL。 任务链安全兜底：主循环异常→紧急返航、stop 幂等、landing_stop 保留遥测、in_air 流复用+超时、解锁/起飞与吊舱识别仅执行一次、rtl_timeout 上限。 入口共享件拆到 utils/sitl_support.py；pod_sitl_test.py→l1_sitl_test.py；删除旧架构 test.py；相机脚本移入 tools/。 新增 .gitignore 并剔除历史误跟踪的 59 个 .pyc；requirements 按实际依赖重写；文档全面对齐，新增 legacy/README.md 与 tools/README.md。
113 lines
8.4 KiB
Markdown
113 lines
8.4 KiB
Markdown
# stereo_ranger（Python 版）
|
||
|
||
双目稀疏目标测距：对**同一时刻**左右两目已立体校正图像上的目标检测框做立体
|
||
几何解算，输出少量目标的**前向距离 Z + 斜距 R（参考）+ 虚拟双目中心方位
|
||
c_mid + 有效性/置信度**。不做目标识别、图像采集/立体校正、稠密深度。
|
||
|
||
规格：`docs/双目测距技术路线.md`（算法与语义权威）。
|
||
|
||
⚠ **全部参数为占位值**（焦距/标定待厂商数据）：标定与地面真值验证完成前，
|
||
本模块输出**仅限静态标靶/回放/台架验证，不得作为飞行控制决策依据**（§12.3）。
|
||
|
||
## 接口清单
|
||
|
||
| 名称 | 说明 |
|
||
| --- | --- |
|
||
| `StereoRanger(config=None, matcher=None)` | 主入口；`matcher` 可注入自定义匹配器 |
|
||
| `.update(left, right, now_ms=0) -> List[RangeRecord]` | 单周期处理（§5 五步），纯计算、无 IO |
|
||
| `.reset()` | 清空全部运行期状态（滤波/一致性/隔离/节流计数） |
|
||
| `.config` / `.matcher` | 只读访问（配置与匹配器） |
|
||
| `StereoConfig` / `load_from_json(path)` | 17 项配置；缺项/未知字段/类型错误/非法取值一律 `ValueError`（fail-fast） |
|
||
| `.focal_baseline_px_m` / `.z_min_m` / `.z_max_m` / `.z_error_budget_max_m` | 派生只读量（F·B 常量、视差域互推的距离域、误差预算远界） |
|
||
| `Detection(x1,y1,x2,y2, confidence, class_id, track_id)` | 单框（校正后坐标系；含 `cx/cy/width/height/bbox` 属性） |
|
||
| `DetectionFrame(frame_id, detections)` | 单目单帧 |
|
||
| `RangeRecord` | 输出记录：`frame_id / class_id / degradation / valid / track_id / forward_distance_m(Z) / slant_range_m(R) / disparity_px(D) / center_mid_px(c_mid) / reference_box / reference_eye / confidence / timestamp_ms` |
|
||
| `Degradation` / `Eye` / `DisparitySource` | 枚举：完整/仅方位；左/右目；同侧边缘/中心回退 |
|
||
| `sample_point` / `SamplePoint` / `select_disparity` / `Disparity` | 取点器（§6.1，纯函数） |
|
||
| `Matcher`（Protocol）/ `GreedyMatcher` / `MatchPair` | 匹配器（§6.2，接口注入，可换匈牙利等） |
|
||
| `solve_range` / `RangeSolution` / `virtual_center_px` / `forward_distance_m` | 测距与方位源（§3.2/§6.6，纯函数） |
|
||
| `evaluate_validity` / `ValidityDecision` / `range_error_abs_m` / `range_error_rel` / `confidence_from_cost` / `DELTA_D_PX` | 有效性三道门与误差模型（§6.4/§7.1，纯函数） |
|
||
| `ConsistencyMonitor` / `ConsistencyStatus` | 一致性监控（§6.7，每目标状态） |
|
||
| `DistanceFilter` | 距离低通 + 跳变重置（§6.5，每目标状态） |
|
||
| `TargetStore` | 目标隔离与老化清理（§4，track_id 优先 + 最近邻兜底） |
|
||
| `WarnThrottle` | 告警节流（第 1 次 + 每满 100 次，带累计计数） |
|
||
|
||
`logger` 名称为 `stereo_ranger`，走标准 `logging`。
|
||
|
||
## 单帧流程（`update` 五步）
|
||
|
||
1. **同帧契约**：`left.frame_id != right.frame_id` → 拒收整帧、返回空列表、WARN 节流（§2.1）；
|
||
2. **取点**：逐框取同侧（左）边缘 x 与框中心；坐标非有限值的框丢弃并 WARN（不参与解算也不发布）；
|
||
3. **匹配**：硬约束（类别一致、`|Δy| ≤ tau_y_px`、框高相对差 ≤ `tau_h_ratio`）+ 代价升序贪心一一配对，`cost < 1` 才接受；
|
||
4. **配对目标**：`D → Z = F·B/D`（`D ≤ 0` 判违例）→ `R` 参考 → `c_mid = (cL + cR)/2` → 门① 域内 / 门② 误差预算 / 门③ 一致性 → 有效帧过 `DistanceFilter`，记 `Z_out`；
|
||
5. **未配对框**：发布仅方位记录（`BEARING_ONLY`，`valid = false`，右目未匹配同样发布）；帧末对未出现目标记失效、连续 `filter_reset_frames` 帧未出现整体清理。
|
||
|
||
记录顺序：先按左目框顺序（配对→完整 / 未配对→仅方位），再给未配对右目框的仅方位记录。
|
||
|
||
## 实现决策（v1.0 冻结，已回填规格）
|
||
|
||
规格 `docs/双目测距技术路线.md` 已按下列决策修订（§4/§6.1/§6.2/§6.4/§6.7/
|
||
§9/§11 均已更新），本节保留决策动机与实现落点，供重写/移植核对。
|
||
|
||
1. **越界 = 有效性门，不是配对硬约束**：规格 §6.2 原把"视差在域内"列入
|
||
硬约束，与 §5/§6.3/§6.4/§2.3 的表述冲突；按 §5 主流程实现并回填——
|
||
越界目标仍配对、仍发布 `D` 与 `c_mid`（远场方位闭环的输入基础），仅
|
||
`valid = false`。
|
||
2. **视差域权威**：`disparity_min_px / disparity_max_px` 为真源，`z_min_m / z_max_m`
|
||
为派生只读属性；原 §9.2 占位值不自洽（`D_min = 3 px ⇔ 19.2 m ≠ 20 m`），
|
||
距离域配置项已从规格删除（§6.4/§9.2）。
|
||
3. **不实现 §6.1 的"左右边缘各算一次取中值"精化**：两支的 median 恒等于均值，
|
||
即框中心视差，与"同侧边缘"的理由矛盾，规格已删除该项（早期笔误），留
|
||
v1.1 亚像素精化。
|
||
4. **补充配置项**（原文 §9 未列、算法需要，规格 §9.3/§9.5 已回填）：
|
||
`consistency_jump_px`（§6.7 视差跳变门限）、`isolation_gate_px`（§4 最近邻
|
||
兜底的门限）。
|
||
5. **δD 固定 1 px**：`DELTA_D_PX = 1.0`（§7.1 ⚠占位）；标定回填后可参数化。
|
||
6. **R 与已发布 Z 同基准**：`R = R_raw · z_out / z_raw`（X/Y/Z 均 ∝ Z，精确换基，
|
||
保证 `R ≥ Z`）；无效帧无 `Z_out` 时 `R` 取本帧原始值（诊断参考）。规格
|
||
§2.2/§6.5 已注明。
|
||
7. **置信度映射**（§6.4 ⚠占位）：`clamp(1 − cost, 0, 1)`；连续跳变 ≥ N 帧乘 0.5
|
||
（`_CONFIDENCE_DEGRADE_FACTOR`）；判无效置 0。
|
||
8. **一致性只在配对成功帧推进**：仅方位帧没有 D 观测量，不推进 D 序列；但目标
|
||
未出现的帧会记一次滤波失效（§6.5 失效清理），两条状态机职责不混。
|
||
9. **取点回退规则**：边缘视差越窗且中心视差在域内 → 回退中心
|
||
（`CENTER_FALLBACK` + WARN）；两者均越窗 → 保留边缘值供诊断与违例告警。
|
||
10. **track_id 优先路径提前启用**（v1.1 语义，规格 §4 已按此冻结）：左右
|
||
`track_id` 一致且 > 0 时用 `(track, 类别, id)` 作隔离键；最近邻只在无
|
||
track_id 的状态间匹配，一一归属。
|
||
11. **§3.2 勘误**：原文 `X_C = X + B/2` 与 §6.6 的符号相反（应为 `X_C = X − B/2`），
|
||
规格已修正；代码不输出 `X_C`，以 §6.6 恒等式为准。
|
||
|
||
## 运行期状态清单（§11.1，漏一个就变行为）
|
||
|
||
| 状态 | 所属 | 初值 | 改变时机 |
|
||
| --- | --- | --- | --- |
|
||
| 连续异常计数 ×目标 | `ConsistencyMonitor` | 0 | 每帧异常 +1 / 正常帧清零 |
|
||
| 上次滤波值、初始化标志、连续失效计数 ×目标 | `DistanceFilter` | 无 / false / 0 | 有效帧写入 / 首有效帧置位 / 无效帧 +1 |
|
||
| 最近位置、最近出现帧 ×目标 | `TargetStore` | 首帧写入 | 每次 `resolve` |
|
||
| 帧序号 | `StereoRanger` | 0 | 每帧 +1（拒收帧不加） |
|
||
| 告警计数 ×异常类 | `WarnThrottle` | 0 | 每次异常 |
|
||
|
||
## 测试与演示
|
||
|
||
```bash
|
||
cd utils
|
||
"D:/program file/conda/envs/pytorch/python.exe" -m pytest tests/stereo_ranger -q # 111 条
|
||
"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q # 全量 220 条
|
||
python utils/demo_stereo_ranger.py # 六场景合成框对演示
|
||
```
|
||
|
||
构件级测试逐条对应 §11.4（取点器/匹配器/测距/有效性/滤波器/一致性/全流程），
|
||
**全部用合成框对驱动，无需真实图像**（几何是解析可断言的）：
|
||
|
||
- 取点：同侧边缘取值、框宽差不进入视差、中心对照、越窗回退、边界含等号、负视差；
|
||
- 匹配：硬约束各一例、`cost ≥ 1` 拒绝、贪心排序、一一配对；
|
||
- 测距：`Z = F·B/D` 回收、`D ≤ 0` 违例、`c_mid` 等权平均恒等式、误差平方增长；
|
||
- 有效性：三道门与边界归类、预算门通过/拦截两侧；
|
||
- 滤波/一致性：低通收敛、跳变重置不拖尾、失效清理、N 帧降级 / M 帧判无效 / 正常帧清零；
|
||
- 全流程：单目标、滤波、远场越界仍配对、单目退化、记录顺序、非同帧拒收与节流、
|
||
空输入、非有限框、track_id 跨大位移保持状态、最近邻越门限新建状态、匹配器注入、`reset`。
|
||
|
||
demo 覆盖：① 逼近（三道门依次生效 + 滤波）② 远场域外（方位照常发布）
|
||
③ 单目退化 ④ 非同帧拒收与节流 ⑤ 一致性降级/判无效 ⑥ 多目标贪心配对。
|