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。
8.4 KiB
8.4 KiB
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 五步）
- 同帧契约：
left.frame_id != right.frame_id→ 拒收整帧、返回空列表、WARN 节流（§2.1）； - 取点：逐框取同侧（左）边缘 x 与框中心；坐标非有限值的框丢弃并 WARN（不参与解算也不发布）；
- 匹配：硬约束（类别一致、
|Δy| ≤ tau_y_px、框高相对差 ≤tau_h_ratio）+ 代价升序贪心一一配对，cost < 1才接受； - 配对目标：
D → Z = F·B/D（D ≤ 0判违例）→R参考 →c_mid = (cL + cR)/2→ 门① 域内 / 门② 误差预算 / 门③ 一致性 → 有效帧过DistanceFilter，记Z_out； - 未配对框：发布仅方位记录（
BEARING_ONLY，valid = false，右目未匹配同样发布）；帧末对未出现目标记失效、连续filter_reset_frames帧未出现整体清理。
记录顺序：先按左目框顺序（配对→完整 / 未配对→仅方位），再给未配对右目框的仅方位记录。
实现决策（v1.0 冻结，已回填规格）
规格 docs/双目测距技术路线.md 已按下列决策修订（§4/§6.1/§6.2/§6.4/§6.7/
§9/§11 均已更新），本节保留决策动机与实现落点，供重写/移植核对。
- 越界 = 有效性门，不是配对硬约束：规格 §6.2 原把"视差在域内"列入
硬约束，与 §5/§6.3/§6.4/§2.3 的表述冲突；按 §5 主流程实现并回填——
越界目标仍配对、仍发布
D与c_mid（远场方位闭环的输入基础），仅valid = false。 - 视差域权威：
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）。 - 不实现 §6.1 的"左右边缘各算一次取中值"精化：两支的 median 恒等于均值， 即框中心视差，与"同侧边缘"的理由矛盾，规格已删除该项（早期笔误），留 v1.1 亚像素精化。
- 补充配置项（原文 §9 未列、算法需要，规格 §9.3/§9.5 已回填）：
consistency_jump_px（§6.7 视差跳变门限）、isolation_gate_px（§4 最近邻 兜底的门限）。 - δD 固定 1 px：
DELTA_D_PX = 1.0（§7.1 ⚠占位）；标定回填后可参数化。 - R 与已发布 Z 同基准：
R = R_raw · z_out / z_raw（X/Y/Z 均 ∝ Z，精确换基， 保证R ≥ Z）；无效帧无Z_out时R取本帧原始值（诊断参考）。规格 §2.2/§6.5 已注明。 - 置信度映射（§6.4 ⚠占位）：
clamp(1 − cost, 0, 1)；连续跳变 ≥ N 帧乘 0.5 （_CONFIDENCE_DEGRADE_FACTOR）；判无效置 0。 - 一致性只在配对成功帧推进：仅方位帧没有 D 观测量，不推进 D 序列；但目标 未出现的帧会记一次滤波失效（§6.5 失效清理），两条状态机职责不混。
- 取点回退规则：边缘视差越窗且中心视差在域内 → 回退中心
（
CENTER_FALLBACK+ WARN）；两者均越窗 → 保留边缘值供诊断与违例告警。 - track_id 优先路径提前启用（v1.1 语义，规格 §4 已按此冻结）：左右
track_id一致且 > 0 时用(track, 类别, id)作隔离键；最近邻只在无 track_id 的状态间匹配，一一归属。 - §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 | 每次异常 |
测试与演示
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 覆盖：① 逼近（三道门依次生效 + 滤波）② 远场域外（方位照常发布） ③ 单目退化 ④ 非同帧拒收与节流 ⑤ 一致性降级/判无效 ⑥ 多目标贪心配对。