新增双目yolo实现python代码

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 <path>  # 指定配置
+
+⚠ 所有参数均为占位值（焦距/标定待厂商数据）：标定与地面真值验证完成前，
+  本模块输出仅限静态标靶/回放/台架验证，不得作为飞行控制决策依据
+  （规格 §12.3 红线）。
+"""
+
+import argparse
+import logging
+from pathlib import Path
+from typing import List, Optional, Tuple
+
+from stereo_ranger import (
+    Detection,
+    DetectionFrame,
+    RangeRecord,
+    StereoConfig,
+    StereoRanger,
+    load_from_json,
+)
+
+logger = logging.getLogger("demo_stereo_ranger")
+
+_DEMO_DIR = Path(__file__).resolve().parent
+_DEFAULT_CONFIG = _DEMO_DIR / "config" / "stereo_ranger.json"
+
+
+# ---------------------------------------------------------------- 合成框对
+
+
+def boxes_for(
+    cfg: StereoConfig,
+    x_center_m: float,
+    y_m: float,
+    z_m: float,
+    *,
+    w_m: float = 0.5,
+    h_m: float = 0.4,
+    class_id: int = 0,
+    track_id: Optional[int] = None,
+    confidence: float = 0.95,
+) -> Tuple[Detection, Detection]:
+    """模组中心系 (X_C, Y, Z) 的理想目标 → (左目框, 右目框)。
+
+    两目框同宽，因此边缘视差 = 中心视差 = F·B/Z（§3.2/§6.6 的精确投影）。
+    """
+    x_l = cfg.cx_px + cfg.fx_px * (x_center_m + cfg.baseline_m / 2.0) / z_m
+    x_r = cfg.cx_px + cfg.fx_px * (x_center_m - cfg.baseline_m / 2.0) / z_m
+    y = cfg.cy_px + cfg.fy_px * y_m / z_m
+    w_px = cfg.fx_px * w_m / z_m
+    h_px = cfg.fy_px * h_m / z_m
+
+    def _box(cx_px: float) -> Detection:
+        return Detection(
+            cx_px - w_px / 2.0,
+            y - h_px / 2.0,
+            cx_px + w_px / 2.0,
+            y + h_px / 2.0,
+            confidence=confidence,
+            class_id=class_id,
+            track_id=track_id,
+        )
+
+    return _box(x_l), _box(x_r)
+
+
+def pair_with_disparity(d_px: float, *, y_center: float = 500.0) -> Tuple[Detection, Detection]:
+    """构造视差恰为 d_px 的等宽框对（d 取 0.5 的整数倍时浮点精确）。"""
+    width, height = 60.0, 40.0
+    left = Detection(1000.0 - width / 2, y_center - height / 2, 1000.0 + width / 2, y_center + height / 2)
+    right = Detection(
+        1000.0 - d_px - width / 2,
+        y_center - height / 2,
+        1000.0 - d_px + width / 2,
+        y_center + height / 2,
+    )
+    return left, right
+
+
+def frame(frame_id: int, detections: List[Detection]) -> DetectionFrame:
+    return DetectionFrame(frame_id=frame_id, detections=detections)
+
+
+# ---------------------------------------------------------------- 打印
+
+
+def describe(record: RangeRecord) -> str:
+    z_text = f"{record.forward_distance_m:7.3f}" if record.forward_distance_m is not None else "   —   "
+    r_text = f"{record.slant_range_m:7.3f}" if record.slant_range_m is not None else "   —   "
+    d_text = f"{record.disparity_px:7.2f}" if record.disparity_px is not None else "   —   "
+    return (
+        f"[F{record.frame_id:02d}] {record.degradation.name:<12} valid={int(record.valid)} "
+        f"Z={z_text} m  R={r_text} m  D={d_text} px  "
+        f"c_mid=({record.center_mid_px[0]:7.1f},{record.center_mid_px[1]:6.1f})  "
+        f"conf={record.confidence:.2f}"
+    )
+
+
+def log_scene(title: str) -> None:
+    logger.info("")
+    logger.info("── %s", title)
+
+
+def log_config_summary(cfg: StereoConfig) -> None:
+    logger.info("配置：B=%.3f m  F=%.1f px  (F·B=%.2f px·m)", cfg.baseline_m, cfg.fx_px, cfg.focal_baseline_px_m)
+    logger.info(
+        "      视差域 [%.1f, %.1f] px ⇔ 距离域 [%.2f, %.2f] m（视差域权威）",
+        cfg.disparity_min_px,
+        cfg.disparity_max_px,
+        cfg.z_min_m,
+        cfg.z_max_m,
+    )
+    logger.info(
+        "      误差预算 δZ(δD=1px) ≤ %.1f m ⇒ 有效远界 %.2f m；一致性 N=%d / M=%d（跳变门限 %.1f px）",
+        cfg.max_delta_z_m,
+        cfg.z_error_budget_max_m,
+        cfg.consistency_N_frames,
+        cfg.consistency_M_frames,
+        cfg.consistency_jump_px,
+    )
+    logger.info(
+        "      滤波 α=%.2f（≈%.0f 帧群延迟）跳变重置 %.1f m；目标隔离门限 %.0f px",
+        cfg.smooth_alpha,
+        1.0 / cfg.smooth_alpha,
+        cfg.jump_reset_m,
+        cfg.isolation_gate_px,
+    )
+
+
+# ---------------------------------------------------------------- 场景
+
+
+def scene_approach(ranger: StereoRanger, cfg: StereoConfig) -> None:
+    """① 逼近：Z 20 → 6 m，三条有效性门依次生效，有效帧起低通平滑。"""
+    ranger.reset()  # 场景独立：清空跨场景的滤波/一致性/隔离状态
+    log_scene("① 逼近场景：目标沿光轴 20 → 6 m（1 m/帧），观察三道门与滤波")
+    logger.info("   %-7s %-9s %-6s %-7s %s", "真值Z", "D(px)", "valid", "输出Z", "说明")
+    user_ms = 1000
+    for index, z_true in enumerate([20.0, 18.0, 16.0, 14.0, 12.0, 11.0, 10.0, 9.0, 8.0, 7.0, 6.0], start=1):
+        left, right = boxes_for(cfg, 0.0, 0.0, z_true)
+        record = ranger.update(frame(index, [left]), frame(index, [right]), now_ms=user_ms)[0]
+        user_ms += 50
+        if record.disparity_px is not None and record.disparity_px < cfg.disparity_min_px:
+            note = "门① 视差域外 → invalid"
+        elif not record.valid and record.disparity_px is not None:
+            note = "门② 误差预算不满足 → invalid"
+        elif record.forward_distance_m is not None and abs(record.forward_distance_m - z_true) > 1e-9:
+            note = f"有效（滤波后，真值 {z_true:.2f}）"
+        else:
+            note = "有效（滤波器初始化）"
+        logger.info(
+            "   %-7.2f %-9.2f %-6d %-7s %s",
+            z_true,
+            record.disparity_px if record.disparity_px is not None else float("nan"),
+            int(record.valid),
+            f"{record.forward_distance_m:.3f}" if record.forward_distance_m is not None else "—",
+            note,
+        )
+    logger.info(
+        "   → 远场：域外或 δZ 超预算时诚实输出 invalid（不强行估计）；"
+        "有效帧之间才做一阶低通，输出滞后于真值属预期（消费侧 PID 与限幅吸收）"
+    )
+
+
+def scene_far_field_bearing(ranger: StereoRanger, cfg: StereoConfig) -> None:
+    """② 远场域外：无距离但方位照常发布（远场定速接近策略的输入基础）。"""
+    ranger.reset()
+    log_scene("② 远场域外：Z=25 m（D < 下限），valid=false 但 c_mid 仍发布")
+    x_center_m = 0.6
+    left, right = boxes_for(cfg, x_center_m, 0.0, 25.0)
+    record = ranger.update(frame(20, [left]), frame(20, [right]), now_ms=2000)[0]
+    logger.info("   %s", describe(record))
+    logger.info(
+        "   c_mid.x=%.1f = cx + F·X_C/Z = %.1f（虚拟双目中心，无近场视差偏差）",
+        record.center_mid_px[0],
+        cfg.cx_px + cfg.fx_px * x_center_m / 25.0,
+    )
+    logger.info("   → 消费侧据此走\"方位闭环 + 定速接近\"（D4），而不是使用不可靠的远场估计")
+
+
+def scene_monocular_degradation(ranger: StereoRanger, cfg: StereoConfig) -> None:
+    """③ 单目退化：仅一目检出 → 仅方位记录（v1.0 接受近场视差偏差）。"""
+    ranger.reset()
+    log_scene("③ 单目退化：仅左目检出")
+    left, _ = boxes_for(cfg, 0.3, 0.0, 4.0, confidence=0.88)
+    records = ranger.update(frame(21, [left]), frame(21, []), now_ms=3000)
+    logger.info("   %s", describe(records[0]))
+    logger.info("   → 退化 = 仅方位：报该目框中心作方位，valid=false 无距离（§2.2 记录两态）")
+
+
+def scene_frame_mismatch(ranger: StereoRanger) -> None:
+    """④ 非同帧拒收：左右帧标识不一致 → 整帧拒收（WARN 节流）。"""
+    ranger.reset()
+    log_scene("④ 同帧契约：左右帧标识不一致（F30 vs F31）")
+    left_pair = pair_with_disparity(9.6)
+    for _ in range(2):
+        records = ranger.update(frame(30, [left_pair[0]]), frame(31, [left_pair[1]]))
+    logger.info("   两次违约共输出记录 %d 条（应为 0）；告警按节流规范只在第 1 次打印", len(records))
+    logger.info("   → 不同曝光时刻的框配对解算没有物理意义，宁可拒收不可给值（§2.1）")
+
+
+def scene_consistency(ranger: StereoRanger) -> None:
+    """⑤ 一致性监控：持续视差跳变 → N 帧降置信度 → M 帧判无效。"""
+    ranger.reset()
+    log_scene("⑤ 一致性监控：视差逐帧跳变 4 px（门限 3 px）")
+    for index, d_px in enumerate([9.5, 13.5, 17.5, 21.5, 25.5, 29.5], start=40):
+        left, right = pair_with_disparity(d_px)
+        record = ranger.update(frame(index, [left]), frame(index, [right]), now_ms=4000)[0]
+        logger.info("   %s", describe(record))
+    logger.info("   → 只能降级、不能纠偏：连续异常帧数达 N 降置信度、达 M 判无效（§6.7）")
+
+
+def scene_multi_target(ranger: StereoRanger, cfg: StereoConfig) -> None:
+    """⑥ 多目标：贪心一一配对；未配对的右目框退化为仅方位记录。"""
+    ranger.reset()
+    log_scene("⑥ 多目标：两个同类别目标 + 一个孤立右目框")
+    left_a, right_a = boxes_for(cfg, -1.0, 0.0, 8.0)
+    left_b, right_b = boxes_for(cfg, 1.2, 0.0, 9.0)
+    extra_right = Detection(360.0, 480.0, 420.0, 540.0)
+    records = ranger.update(frame(50, [left_a, left_b]), frame(50, [right_a, right_b, extra_right]), now_ms=5000)
+    for record in records:
+        logger.info("   %s", describe(record))
+    logger.info("   → 目标数极少（1~2）时 O(n²) 贪心与全局最优几乎必然同解；匹配器接口可替换（§10.3）")
+
+
+# ---------------------------------------------------------------- 入口
+
+
+def run_demo(config_path: str) -> None:
+    cfg = load_from_json(config_path)
+    logger.info("配置来源：%s", config_path)
+    log_config_summary(cfg)
+    logger.warning("⚠ 占位参数未标定：仅限静态标靶/回放/台架验证，不得作为飞行控制决策依据（§12.3）")
+
+    ranger = StereoRanger(cfg)  # 演示共用一个实例，状态跨场景延续（逼近 → 后续场景）
+    scene_approach(ranger, cfg)
+    scene_far_field_bearing(ranger, cfg)
+    scene_monocular_degradation(ranger, cfg)
+    scene_frame_mismatch(ranger)
+    scene_consistency(ranger)
+    scene_multi_target(ranger, cfg)
+    logger.info("")
+    logger.info("演示结束：以上全部为合成框对的解析结果，无真实图像与硬件参与")
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description="双目稀疏目标测距演示（合成框对，离线）")
+    parser.add_argument("--config", default=str(_DEFAULT_CONFIG), help="配置 JSON 路径")
+    args = parser.parse_args()
+    logging.basicConfig(level=logging.INFO, format="%(levelname)s %(message)s")
+    run_demo(args.config)
+
+
+if __name__ == "__main__":
+    main()