新增双目测距模块v1.0（未测试）

utils/demo_stereo_ranger.py

@@ -0,0 +1,96 @@
+"""stereo_ranger demo：合成多目标场景演示单帧 update() 全流程。
+
+本 demo 不使用任何检测器：按已知 (X, Y, Z) 目标解析投影生成左右目
+检测框（模拟"外部检测器输出，框已在校正后原生坐标系"），并叠加 ±1 px
+亚像素抖动，随后用 StereoRanger 测距并对照真值。
+
+运行（pytorch 环境）：
+    python utils/demo_stereo_ranger.py [--config path/to/stereo.json]
+
+依赖仅为标准库 + stereo_ranger 包（将 utils/ 加入 PYTHONPATH 或于 utils/ 下运行）。
+"""
+
+import argparse
+import random
+import sys
+from pathlib import Path
+
+sys.path.insert(0, str(Path(__file__).resolve().parent))
+
+from stereo_ranger import Detection, StereoConfig, StereoRanger, focal_from_hfov, load_from_json, range_error_abs
+
+# 模拟相机：1920x1080，HFOV 60°（文档 §5.3 示例之一）
+IMAGE_W = 1920
+IMAGE_H = 1080
+HFOV_DEG = 60.0
+BASELINE_M = 0.06
+
+
+def project_to_boxes(x_m, y_m, z_m, fx, fy, cx, cy, b, rng, w_m=0.5, h_m=0.3):
+    """目标物理宽高 -> 左右目框；叠加亚像素抖动模拟检测噪声（种子固定）。"""
+    w_px = fx * w_m / z_m
+    h_px = fy * h_m / z_m
+    u_l = fx * x_m / z_m + cx
+    v_l = fy * y_m / z_m + cy
+    u_r = u_l - fx * b / z_m
+    jitter = lambda: rng.uniform(-1.0, 1.0)  # ±1 px 框边缘抖动
+    left = Detection(u_l - w_px / 2 + jitter(), v_l - h_px / 2 + jitter(),
+                     u_l + w_px / 2 + jitter(), v_l + h_px / 2 + jitter(),
+                     conf=0.95, cls_id=0, track_id=1)
+    right = Detection(u_r - w_px / 2 + jitter(), v_l - h_px / 2 + jitter(),
+                      u_r + w_px / 2 + jitter(), v_l + h_px / 2 + jitter(),
+                      conf=0.9, cls_id=0, track_id=1)
+    return left, right
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--config", default=None, help="StereoConfig JSON 文件（可选）")
+    args = parser.parse_args()
+
+    cfg = load_from_json(args.config) if args.config else StereoConfig(
+        fx_px=focal_from_hfov(IMAGE_W, HFOV_DEG),
+        fy_px=focal_from_hfov(IMAGE_W, HFOV_DEG),
+        cx_px=IMAGE_W / 2.0, cy_px=IMAGE_H / 2.0,
+        baseline_m=BASELINE_M,
+        range_tol_m=2.0,
+    )
+    ranger = StereoRanger(cfg)
+    rng = random.Random(20260909)
+
+    targets = [(0.0, 0.0, 5.0), (2.0, 0.5, 8.0), (-1.5, -0.4, 12.0)]
+    left_dets, right_dets = [], []
+    for x_m, y_m, z_m in targets:
+        l, r = project_to_boxes(x_m, y_m, z_m, cfg.fx_px, cfg.fy_px, cfg.cx_px, cfg.cy_px,
+                                cfg.baseline_m, rng)
+        left_dets.append(l)
+        right_dets.append(r)
+    # 一个只在左目检出的干扰框 -> 应输出 matched=False 的方位记录
+    left_dets.append(Detection(100.0, 900.0, 160.0, 950.0, conf=0.6, cls_id=9))
+
+    reports = ranger.update(left_dets, right_dets, frame_sequence=1, timestamp_s=0.0)
+
+    print(f"内参: F={cfg.fx_px:.1f}px  cx={cfg.cx_px:.0f}  B={cfg.baseline_m:.3f}m  "
+          f"Z 窗口 [{cfg.z_min_m}, {cfg.z_max_m}]m")
+    print(f"{'真值 Z':>8} {'估计 Z':>8} {'真值 X':>7} {'估计 X':>7} {'视差':>6} {'δZ估计':>7} "
+          f"{'有效':>5} {'x_mid':>7}  备注")
+    for i, r in enumerate(reports):
+        if not r.matched:
+            print(f"{'-':>8} {'-':>8} {'-':>7} {'-':>7} {'-':>6} {'-':>7} {'-':>5} "
+                  f"{r.centre_pixel_x_mid:7.1f}  左目未配对（方位直发）")
+            continue
+        truth = targets[i]
+        print(f"{truth[2]:8.2f} {r.forward_distance_m:8.2f} {truth[0]:7.2f} {r.lateral_m:7.2f} "
+              f"{r.disparity_px:6.1f} {r.error_est_m:7.2f} {str(r.valid):>5} "
+              f"{r.centre_pixel_x_mid:7.1f}  src={r.disparity_source}")
+    print(f"\n共 {len(reports)} 条记录（{sum(1 for r in reports if r.valid)} 条有效）")
+
+    print("\n误差模型示意（规格 §5.3，δD = 1 px）：")
+    print(f"{'Z (m)':>7} {'δZ (m)':>8}")
+    for z in [5, 10, 15, 20, 30]:
+        err = range_error_abs(z, cfg.fx_px, cfg.baseline_m, 1.0)
+        print(f"{z:7d} {err:8.2f}")
+
+
+if __name__ == "__main__":
+    main()

utils/stereo_ranger/README.md

@@ -0,0 +1,65 @@
+# stereo_ranger（Python）
+
+双目稀疏目标测距（规格：`docs/双目测距方案.md`）。输入左右目各自、**校正后原生坐标系**下的检测框，输出目标的 \(Z, X, Y, R\) 与方位。不做目标识别（YOLO）、不做图像采集与立体校正——对应规格 §10 中可独立替换的检测前端与采集预处理模块。
+
+仅依赖标准库（`math`/`logging`），不 import 本仓库任何其他模块。
+
+## 接口清单
+
+| 名称 | 说明 |
+| --- | --- |
+| `Detection(x1, y1, x2, y2, conf=1.0, cls_id=0, track_id=0)` | 输入检测框（px，左上/右下角） |
+| `StereoConfig(...)` | 配置（默认值 = 规格示例值，见下） |
+| `load_from_json(path)` | 从 JSON 构建配置（缺字段/未知字段/非法值抛 `ValueError`） |
+| `StereoRanger(config=None)` | 测距器主类 |
+| `.update(left_dets, right_dets, frame_sequence=0, timestamp_s=None) -> list[RangeReport]` | 单帧主流程（返回记录数与左目检测数一致、顺序一致） |
+| `.reset()` / `.config` | 清空滤波状态 / 当前配置 |
+| `RangeReport` | 输出记录（字段见 `types.py`） |
+| `focal_from_hfov(w_px, hfov_deg)` / `range_error_abs(z, fx, B, δD=1)` / `range_error_rel(...)` | 工具函数（§5.2/§5.3） |
+
+## 与检测器的约定（规格 §4.3）
+
+1. 左右图像必须使用**完全一致**的预处理几何（同 letter-box、同缩放），由检测前端保证；
+2. 传入本模块的框必须已映射回**原生分辨率**（视差在原生坐标计算），内参 `fx/fy/cx/cy` 亦对应原生分辨率；
+3. 图像对须已完成立体校正（同一目标在两目 y 坐标一致）；校正残留由 `vertical_tau_px` 容错；
+4. YOLO 推理及左右目并行执行由调用方负责，本模块只收检测结果。
+
+## 单帧流程（对照规格）
+
+硬约束过滤（类别一致、`|Δy| ≤ τy`、高度比 `≤ τh`、视差落在 `FB/z_max ~ FB/z_min` 区间）→ 代价 `|Δy|/τy + |Δh|/(τh·max(hL,hR))` 升序贪心一一配对（严格 `cost < 1`；同 `track_id` 优先）→ 主取**同侧左边缘**视差，越出关联窗口时按配置回退中心视差（节流警告）→ `Z=FB/D`、`X=(xL−cx)Z/F`、`Y=(yL−cy)Z/F`、`R=√(X²+Y²+Z²)` → 有效性 `D ≥ D_min 且 δZ ≤ Z_tol` → 可选一阶低通滤波（按 (cls, track) 分键，跳变超门限重置；`track_id=0` 不过滤）。
+
+## 输出语义
+
+- `matched=True, valid=True`：有效测距；
+- `matched=True, valid=False`：已配对但超出有效工作域（§5.4），距离/坐标仍发布供诊断，方位可用；
+- `matched=False, valid=False`：左目检测本帧未配对（§6 失败处理），仅单目方位回退。
+
+方位源 `centre_pixel_x_mid` 为基线中点虚拟相机投影（§11.3）；注意 `X` 属**左目坐标系**（§4.2），二者相差 `B/2`，勿混用。
+
+## StereoConfig 关键项
+
+| 字段 | 默认 | 说明 |
+| --- | --- | --- |
+| `fx_px/fy_px/cx_px/cy_px` | 960/960/960/540 | 校正后内参（原生分辨率） |
+| `baseline_m` | 0.06 | 基线 B |
+| `z_min_m / z_max_m` | 1.0 / 50.0 | 距离窗口（派生视差关联区间） |
+| `vertical_tau_px` / `height_ratio_tau` | 2.0 / 0.25 | 关联阈值 τy / τh |
+| `disparity_min_px` / `range_tol_m` | 3.0 / 2.0 | 有效性判据 D_min / Z_tol |
+| `disp_noise_px` | 1.0 | 误差模型 δD（用于 δZ 估计与有效性） |
+| `use_center_fallback` | true | 边缘视差越窗时回退中心 |
+| `filter_enabled` / `filter_alpha` / `filter_jump_ratio` | false / 0.3 / 0.2 | 时间滤波 |
+| `warn_interval_s` | 5.0 | 诊断日志节流间隔 |
+
+## 运行
+
+```bash
+cd utils
+"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q            # 全部测试（149 条）
+"D:/program file/conda/envs/pytorch/python.exe" demo_stereo_ranger.py    # 合成场景 demo
+```
+
+## 未覆盖边界（属其他模块/后续版本）
+
+- 图像采集、立体校正 remap、帧同步、side-by-side 裁切（规格 §3）；
+- YOLO/检测器及其预处理（由调用方按上文约定接入）；
+- 亚像素边缘精化、跨帧跟踪（`track_id` 由外部追踪器提供后，时间滤波即可用）、与主测距传感器融合（§5.5/§11）。

utils/stereo_ranger/__init__.py

@@ -0,0 +1,30 @@
+"""stereo_ranger —— 双目稀疏目标测距（规格：docs/双目测距方案.md）。
+
+纯算法包：输入校正后原生坐标系的左右目检测框，输出目标的 (Z, X, Y, R)
+与方位。不做目标识别、图像采集与立体校正；仅依赖标准库。
+"""
+
+from .config import StereoConfig, load_from_json
+from .stereo_ranger import (
+    DISP_SOURCE_CENTER,
+    DISP_SOURCE_EDGE,
+    StereoRanger,
+    focal_from_hfov,
+    range_error_abs,
+    range_error_rel,
+)
+from .types import BBoxXYXY, Detection, RangeReport
+
+__all__ = [
+    "BBoxXYXY",
+    "DISP_SOURCE_CENTER",
+    "DISP_SOURCE_EDGE",
+    "Detection",
+    "RangeReport",
+    "StereoConfig",
+    "StereoRanger",
+    "focal_from_hfov",
+    "load_from_json",
+    "range_error_abs",
+    "range_error_rel",
+]

utils/stereo_ranger/config.py

@@ -0,0 +1,82 @@
+"""StereoConfig 与 JSON 配置加载。
+
+规格 docs/双目测距方案.md：全部数值参数均为配置项（§2），默认值取文档
+示例值。配置校验在构造期一次性完成，热路径不读 JSON。
+"""
+
+import json
+from dataclasses import dataclass, fields
+from pathlib import Path
+from typing import Any, Dict, Tuple
+
+
+@dataclass
+class StereoConfig:
+    # ---- 相机参数（校正后、对应原生分辨率，§3.1 / §4.3）----
+    fx_px: float = 960.0
+    fy_px: float = 960.0
+    cx_px: float = 960.0
+    cy_px: float = 540.0
+    baseline_m: float = 0.06  # 基线 B
+
+    # ---- 目标距离窗口：关联硬约束（§6），派生视差区间 [FB/z_max, FB/z_min] ----
+    z_min_m: float = 1.0
+    z_max_m: float = 50.0
+
+    # ---- 关联阈值（§6）----
+    vertical_tau_px: float = 2.0  # 垂直残差 tau_y
+    height_ratio_tau: float = 0.25  # 相对高度差 tau_h
+
+    # ---- 视差取点与有效性（§5）----
+    disparity_min_px: float = 3.0  # D_min：D 低于此值直接无效
+    range_tol_m: float = 2.0  # Z_tol：δZ <= Z_tol 才算有效
+    disp_noise_px: float = 1.0  # δD（误差模型中的边缘定位噪声）
+    use_center_fallback: bool = True  # 左边缘视差越窗时回退中心（§5.1）
+
+    # ---- 可选时间滤波（§7，一阶低通 + 跳变重置）----
+    filter_enabled: bool = False
+    filter_alpha: float = 0.3
+    filter_jump_ratio: float = 0.2  # 相对跳变超此门限则重置滤波器
+
+    # ---- 诊断日志节流 ----
+    warn_interval_s: float = 5.0
+
+    def disparity_bounds_px(self) -> Tuple[float, float]:
+        """关联用的视差区间 (lo, hi)：由 z_min/z_max 派生（§6）。"""
+        fb = self.fx_px * self.baseline_m
+        return fb / self.z_max_m, fb / self.z_min_m
+
+    def __post_init__(self) -> None:
+        _require(self.fx_px > 0.0, "fx_px", "必须为正")
+        _require(self.fy_px > 0.0, "fy_px", "必须为正")
+        _require(self.cx_px >= 0.0, "cx_px", "必须非负")
+        _require(self.cy_px >= 0.0, "cy_px", "必须非负")
+        _require(self.baseline_m > 0.0, "baseline_m", "必须为正")
+        _require(0.0 < self.z_min_m < self.z_max_m, "z_min_m/z_max_m", "须满足 0 < z_min_m < z_max_m")
+        _require(self.vertical_tau_px > 0.0, "vertical_tau_px", "必须为正")
+        _require(self.height_ratio_tau > 0.0, "height_ratio_tau", "必须为正")
+        _require(self.disparity_min_px > 0.0, "disparity_min_px", "必须为正")
+        _require(self.range_tol_m > 0.0, "range_tol_m", "必须为正")
+        _require(self.disp_noise_px > 0.0, "disp_noise_px", "必须为正")
+        _require(0.0 < self.filter_alpha <= 1.0, "filter_alpha", "须落在 (0, 1]")
+        _require(self.filter_jump_ratio >= 0.0, "filter_jump_ratio", "必须非负")
+        _require(self.warn_interval_s >= 0.0, "warn_interval_s", "必须非负")
+
+
+def load_from_json(path: str) -> StereoConfig:
+    """从 JSON 文件构建配置。字段缺失或非法取值抛 ValueError（携带字段名）。"""
+    with open(path, encoding="utf-8") as f:
+        data: Dict[str, Any] = json.load(f)
+    known = {field.name for field in fields(StereoConfig)}
+    unknown = set(data) - known
+    if unknown:
+        raise ValueError(f"未知配置字段: {sorted(unknown)}")
+    missing = known - set(data)
+    if missing:
+        raise ValueError(f"缺少配置字段: {sorted(missing)}")
+    return StereoConfig(**data)
+
+
+def _require(ok: bool, field_name: str, message: str) -> None:
+    if not ok:
+        raise ValueError(f"配置非法: {field_name} {message}")

utils/stereo_ranger/stereo_ranger.py

@@ -0,0 +1,215 @@
+"""StereoRanger —— 双目稀疏目标测距（纯算法，规格 docs/双目测距方案.md）。
+
+输入为左右目各自、校正后原生坐标系下的检测框列表；不做目标识别，不做
+图像采集/立体校正。单帧主流程：关联（§6）→ 视差取点（§5.1）→
+Z=F·B/D 与 (X, Y, R)（§4.2）→ 有效性（§5.4）→ 可选时间滤波（§7）。
+依赖仅为标准库。
+"""
+
+import logging
+import math
+import time
+from typing import Dict, List, Optional, Sequence, Tuple
+
+from .config import StereoConfig
+from .types import BBoxXYXY, Detection, RangeReport
+
+logger = logging.getLogger("stereo_ranger")
+
+DISP_SOURCE_EDGE = "left_edge"
+DISP_SOURCE_CENTER = "center"
+
+
+def focal_from_hfov(image_width_px: float, hfov_deg: float) -> float:
+    """由水平视场与图像宽近似像素焦距：F = (W/2) / tan(HFOV/2)（§5.3）。"""
+    return (image_width_px / 2.0) / math.tan(math.radians(hfov_deg) / 2.0)
+
+
+def range_error_abs(z_m: float, fx_px: float, baseline_m: float, delta_d_px: float = 1.0) -> float:
+    """绝对距离误差估计 δZ ≈ Z^2·δD/(F·B)（§5.2）。"""
+    return z_m * z_m * delta_d_px / (fx_px * baseline_m)
+
+
+def range_error_rel(z_m: float, fx_px: float, baseline_m: float, delta_d_px: float = 1.0) -> float:
+    """相对距离误差 δZ/Z ≈ Z·δD/(F·B)（§5.2）。"""
+    return z_m * delta_d_px / (fx_px * baseline_m)
+
+
+class StereoRanger:
+    """双目稀疏目标测距器。
+
+    用法：每帧调用一次 update()，传入同一逻辑帧的左右目检测结果。
+    """
+
+    def __init__(self, config: Optional[StereoConfig] = None):
+        self._cfg = config if config is not None else StereoConfig()
+        self._z_state: Dict[Tuple[int, int], float] = {}  # (cls_id, track_id) -> 滤波 Z
+        self._warn_at: Dict[str, float] = {}
+
+    @property
+    def config(self) -> StereoConfig:
+        return self._cfg
+
+    def reset(self) -> None:
+        self._z_state.clear()
+        self._warn_at.clear()
+
+    def update(
+        self,
+        left: Sequence[Detection],
+        right: Sequence[Detection],
+        frame_sequence: int = 0,
+        timestamp_s: Optional[float] = None,
+    ) -> List[RangeReport]:
+        """单帧主流程：返回报告列表（每个左目检测一条，保持左目输入顺序）。
+
+        配对成功且测距有效的报告 valid=True；配对成功但超出有效工作域的
+        valid=False（距离仍给出，§5.4）；未配对的左目检测 matched=False，
+        仅发布单目方位（§6 失败处理）。
+        """
+        pairs = self._match(left, right)
+        matched_r: Dict[int, int] = {l_idx: r_idx for l_idx, r_idx, _ in pairs}
+        reports: List[RangeReport] = []
+        for l_idx, det_l in enumerate(left):
+            r_idx = matched_r.get(l_idx)
+            if r_idx is None:
+                reports.append(self._unmatched_report(det_l, frame_sequence, timestamp_s))
+                continue
+            report = self._build_report(det_l, right[r_idx], frame_sequence, timestamp_s)
+            reports.append(report)
+        return reports
+
+    # ---- 内部实现 ----
+
+    def _match(self, left: Sequence[Detection], right: Sequence[Detection]) -> List[Tuple[int, int, Detection]]:
+        """硬约束过滤 + 代价升序贪心一一配对（§6）。返回 (左下标, 右下标, 左框)。"""
+        cfg = self._cfg
+        d_lo, d_hi = cfg.disparity_bounds_px()
+        candidates: List[Tuple[int, int, Detection, float]] = []
+        for i, det_l in enumerate(left):
+            for j, det_r in enumerate(right):
+                if det_l.cls_id != det_r.cls_id:
+                    continue
+                dy = abs(det_l.cy - det_r.cy)
+                if dy > cfg.vertical_tau_px:
+                    continue
+                h_max = max(det_l.height, det_r.height)
+                if h_max <= 0.0:
+                    continue
+                dh_ratio = abs(det_l.height - det_r.height) / h_max
+                if dh_ratio > cfg.height_ratio_tau:
+                    continue
+                cost = dy / cfg.vertical_tau_px + dh_ratio / cfg.height_ratio_tau
+                if cost >= 1.0:
+                    continue
+                d_edge = det_l.x1 - det_r.x1
+                if not (d_lo <= d_edge <= d_hi):
+                    d_center = det_l.cx - det_r.cx
+                    if cfg.use_center_fallback and (d_lo <= d_center <= d_hi):
+                        self._warn("center_fallback", "左边缘视差越出窗口，回退中心视差（宽度差可能混入）")
+                    else:
+                        continue
+                same_track = det_l.track_id > 0 and det_l.track_id == det_r.track_id
+                candidates.append((0 if same_track else 1, cost, i, j, det_l))
+        candidates.sort(key=lambda c: (c[0], c[1]))
+        matched_l: set = set()
+        used_r: set = set()
+        pairs: List[Tuple[int, int, Detection]] = []
+        for _, _, i, j, det_l in candidates:
+            if i in matched_l or j in used_r:
+                continue
+            pairs.append((i, j, det_l))
+            matched_l.add(i)
+            used_r.add(j)
+        return pairs
+
+    def _build_report(self, det_l: Detection, det_r: Detection, seq: int, ts: Optional[float]) -> RangeReport:
+        cfg = self._cfg
+        d_edge = det_l.x1 - det_r.x1
+        d_lo, d_hi = cfg.disparity_bounds_px()
+        if d_lo <= d_edge <= d_hi:
+            d = d_edge
+            source = DISP_SOURCE_EDGE
+        else:
+            d = det_l.cx - det_r.cx
+            source = DISP_SOURCE_CENTER
+        z = cfg.fx_px * cfg.baseline_m / d
+        report = RangeReport(
+            frame_sequence=seq,
+            timestamp_s=ts,
+            class_id=det_l.cls_id,
+            track_id=det_l.track_id if det_l.track_id != 0 else det_r.track_id,
+            matched=True,
+            valid=False,  # §5.4 判定见下
+            forward_distance_m=z,
+            disparity_px=d,
+            disparity_source=source,
+            confidence=min(det_l.conf, det_r.conf),
+            centre_pixel_x_mid=(det_l.cx + det_r.cx) / 2.0,
+            centre_pixel_y_mid=det_l.cy,
+            ref_bbox=(det_l.x1, det_l.y1, det_l.x2, det_l.y2),
+        )
+        err = range_error_abs(z, cfg.fx_px, cfg.baseline_m, cfg.disp_noise_px)
+        if d >= cfg.disparity_min_px and err <= cfg.range_tol_m:
+            report.valid = True
+            z_out = self._filter_z(report.class_id, report.track_id, z)
+            report.forward_distance_m = z_out
+            report.error_est_m = range_error_abs(z_out, cfg.fx_px, cfg.baseline_m, cfg.disp_noise_px)
+        else:
+            report.error_est_m = err
+        self._fill_xyz(report, det_l.cx)
+        return report
+
+    def _fill_xyz(self, report: RangeReport, x_l_px: float) -> None:
+        """由 Z 与左目（参考目）框中心像素回算左目系 X, Y, R（§4.2）。
+
+        注意：方位源 centre_pixel_x_mid 属于基线中点虚拟相机（§11.3），
+        而 X 定义在左目坐标系，二者相差 B/2，不得混用。
+        """
+        cfg = self._cfg
+        z = report.forward_distance_m
+        x = (x_l_px - cfg.cx_px) * z / cfg.fx_px
+        y = (report.centre_pixel_y_mid - cfg.cy_px) * z / cfg.fy_px
+        report.lateral_m = x
+        report.vertical_m = y
+        report.slant_range_m = math.sqrt(x * x + y * y + z * z)
+
+    def _filter_z(self, cls_id: int, track_id: int, z: float) -> float:
+        """可选一阶低通 + 跳变重置（§7）。track_id 为 0（无轨迹）时不过滤。"""
+        cfg = self._cfg
+        if not cfg.filter_enabled or track_id == 0:
+            return z
+        key = (cls_id, track_id)
+        prev = self._z_state.get(key)
+        if prev is None:
+            self._z_state[key] = z
+            return z
+        if abs(z - prev) / prev > cfg.filter_jump_ratio:
+            self._z_state[key] = z  # 跳变：重置
+            return z
+        z_out = (1.0 - cfg.filter_alpha) * prev + cfg.filter_alpha * z
+        self._z_state[key] = z_out
+        return z_out
+
+    def _unmatched_report(self, det: Detection, seq: int, ts: Optional[float]) -> RangeReport:
+        return RangeReport(
+            frame_sequence=seq,
+            timestamp_s=ts,
+            class_id=det.cls_id,
+            track_id=det.track_id,
+            matched=False,
+            valid=False,
+            confidence=det.conf,
+            centre_pixel_x_mid=det.cx,
+            centre_pixel_y_mid=det.cy,
+            ref_bbox=(det.x1, det.y1, det.x2, det.y2),
+        )
+
+    def _warn(self, category: str, message: str) -> None:
+        """节流诊断（§7：失败日志须节流）。"""
+        now = time.monotonic()
+        last = self._warn_at.get(category)
+        if last is not None and now - last < self._cfg.warn_interval_s:
+            return
+        self._warn_at[category] = now
+        logger.warning("[%s] %s", category, message)

utils/stereo_ranger/types.py

@@ -0,0 +1,71 @@
+"""stereo_ranger 输入输出数据结构：Detection / RangeReport。
+
+规格：docs/双目测距方案.md §10 结果记录。
+"""
+
+from dataclasses import dataclass
+from typing import Optional, Tuple
+
+BBoxXYXY = Tuple[float, float, float, float]
+
+
+@dataclass
+class Detection:
+    """校正后原生坐标系下的一个检测框（单位 px）。
+
+    调用方负责：左右目预处理几何完全一致，且框坐标已映射回原生分辨率
+    （规格 §4.3）。x1/y1 为左上角，x2/y2 为右下角（x1 即"左边缘"）。
+    """
+
+    x1: float
+    y1: float
+    x2: float
+    y2: float
+    conf: float = 1.0
+    cls_id: int = 0
+    track_id: int = 0  # 未知为 0
+
+    @property
+    def width(self) -> float:
+        return self.x2 - self.x1
+
+    @property
+    def height(self) -> float:
+        return self.y2 - self.y1
+
+    @property
+    def cx(self) -> float:
+        return (self.x1 + self.x2) / 2.0
+
+    @property
+    def cy(self) -> float:
+        return (self.y1 + self.y2) / 2.0
+
+
+@dataclass
+class RangeReport:
+    """单目标单帧测距结果（一个左目检测对应一条记录）。
+
+    matched=False 表示该左目检测本帧未配对：valid 恒为 False，距离字段为
+    None，centre_pixel_x/y_mid 为单目自身中心（方位回退，规格 §11.3）。
+    matched=True 且 valid=False 表示已配对但距离超出有效工作域
+    （规格 §5.4）：距离字段仍给出估计值供诊断，方位照常发布。
+    """
+
+    frame_sequence: int
+    class_id: int
+    track_id: int
+    matched: bool
+    valid: bool
+    forward_distance_m: Optional[float] = None  # Z，前向分量（主输出）
+    lateral_m: Optional[float] = None  # X，左目系横向偏移
+    vertical_m: Optional[float] = None  # Y，左目系纵向偏移
+    slant_range_m: Optional[float] = None  # R = sqrt(X^2 + Y^2 + Z^2)
+    disparity_px: Optional[float] = None  # D = xL - xR（>0 为前方）
+    disparity_source: Optional[str] = None  # "left_edge" / "center"
+    confidence: float = 0.0
+    error_est_m: Optional[float] = None  # δZ 估计（δD = 配置 disp_noise_px）
+    centre_pixel_x_mid: float = 0.0  # 虚拟双目中心 x（方位源，§11.3）
+    centre_pixel_y_mid: float = 0.0  # y_mid（校正后 yL == yR）
+    ref_bbox: Optional[BBoxXYXY] = None  # 左目（参考目）框，供诊断
+    timestamp_s: Optional[float] = None

utils/tests/stereo_ranger/config_test.py

@@ -0,0 +1,76 @@
+"""StereoConfig 默认值、派生区间与 JSON 加载测试（规格 §2：参数全部外置）。"""
+
+import json
+
+import pytest
+
+from stereo_ranger.config import StereoConfig, load_from_json
+
+VALID = {
+    "fx_px": 1663.0, "fy_px": 1663.0, "cx_px": 960.0, "cy_px": 540.0,
+    "baseline_m": 0.12, "z_min_m": 2.0, "z_max_m": 40.0,
+    "vertical_tau_px": 3.0, "height_ratio_tau": 0.3,
+    "disparity_min_px": 4.0, "range_tol_m": 3.0, "disp_noise_px": 1.5,
+    "use_center_fallback": False, "filter_enabled": True,
+    "filter_alpha": 0.5, "filter_jump_ratio": 0.1, "warn_interval_s": 1.0,
+}
+
+
+def test_defaults_and_disparity_window():
+    cfg = StereoConfig()
+    assert cfg.baseline_m == 0.06 and cfg.fx_px == 960.0
+    # FB = 57.6，z 窗口 [1, 50] m -> D 窗口 [57.6/50, 57.6/1]
+    lo, hi = cfg.disparity_bounds_px()
+    assert lo == pytest.approx(1.152)
+    assert hi == pytest.approx(57.6)
+
+
+@pytest.mark.parametrize("kwargs", [
+    {"baseline_m": 0.0},
+    {"fx_px": -1.0},
+    {"fy_px": 0.0},
+    {"cx_px": -1.0},
+    {"z_min_m": 0.0},
+    {"z_max_m": 10.0, "z_min_m": 20.0},
+    {"vertical_tau_px": 0.0},
+    {"height_ratio_tau": 0.0},
+    {"disparity_min_px": 0.0},
+    {"range_tol_m": 0.0},
+    {"disp_noise_px": 0.0},
+    {"filter_alpha": 0.0},
+    {"filter_alpha": 1.5},
+    {"filter_jump_ratio": -0.1},
+    {"warn_interval_s": -1.0},
+])
+def test_invalid_config_raises(kwargs):
+    with pytest.raises(ValueError):
+        StereoConfig(**kwargs)
+
+
+def test_load_from_json_roundtrip(tmp_path):
+    p = tmp_path / "stereo.json"
+    p.write_text(json.dumps(VALID), encoding="utf-8")
+    cfg = load_from_json(str(p))
+    for field, value in VALID.items():
+        assert getattr(cfg, field) == value
+    lo, hi = cfg.disparity_bounds_px()
+    assert hi == pytest.approx(1663.0 * 0.12 / 2.0)  # FB / z_min
+    assert lo == pytest.approx(1663.0 * 0.12 / 40.0)  # FB / z_max
+
+
+def test_load_from_json_missing_field_reports_name(tmp_path):
+    p = tmp_path / "stereo.json"
+    data = dict(VALID)
+    del data["baseline_m"]
+    p.write_text(json.dumps(data), encoding="utf-8")
+    with pytest.raises(ValueError, match="baseline_m"):
+        load_from_json(str(p))
+
+
+def test_load_from_json_unknown_field_rejected(tmp_path):
+    p = tmp_path / "stereo.json"
+    data = dict(VALID)
+    data["magic_extra"] = 1
+    p.write_text(json.dumps(data), encoding="utf-8")
+    with pytest.raises(ValueError, match="magic_extra"):
+        load_from_json(str(p))

utils/tests/stereo_ranger/stereo_ranger_test.py

@@ -0,0 +1,302 @@
+"""stereo_ranger 测距核心单元测试。
+
+规格：docs/双目测距方案.md §4（几何）、§5（视差/有效性/误差）、§6（关联）、
+§7（滤波）。场景全部由"目标在左目系 (X, Y, Z)"解析投影生成左右框，
+再反推验证测距/方位输出。
+"""
+
+import math
+
+import pytest
+
+from stereo_ranger.config import StereoConfig
+from stereo_ranger.stereo_ranger import (
+    DISP_SOURCE_CENTER,
+    DISP_SOURCE_EDGE,
+    StereoRanger,
+    focal_from_hfov,
+    range_error_abs,
+    range_error_rel,
+)
+from stereo_ranger.types import Detection
+
+CFG = StereoConfig()
+FB = CFG.fx_px * CFG.baseline_m  # 57.6 px·m
+
+
+def box(cx, cy, w=100.0, h=60.0, cls_id=0, conf=1.0, track_id=0):
+    return Detection(cx - w / 2, cy - h / 2, cx + w / 2, cy + h / 2,
+                     conf, cls_id, track_id)
+
+
+def target_pair(x_m, y_m, z_m, w=100.0, h=60.0, cls_id=0, conf=1.0, track_id=0):
+    """目标中心在左目系 (x_m, y_m, z_m)，解析投影为左右目同尺寸框。"""
+    u_l = CFG.fx_px * x_m / z_m + CFG.cx_px
+    v_l = CFG.fy_px * y_m / z_m + CFG.cy_px
+    u_r = u_l - FB / z_m
+    return (box(u_l, v_l, w, h, cls_id, conf, track_id),
+            box(u_r, v_l, w, h, cls_id, conf, track_id))
+
+
+def one_report(ranger, left, right, **kw):
+    reports = ranger.update(left, right, **kw)
+    assert len(reports) == len(left)
+    return reports
+
+
+# ---- §4.2 / §5.3 几何与误差公式 ----
+
+def test_focal_from_hfov_matches_doc_examples():
+    assert focal_from_hfov(1920.0, 90.0) == pytest.approx(960.0)
+    assert focal_from_hfov(1920.0, 60.0) == pytest.approx(1662.769)
+
+
+def test_range_error_matches_doc_table():
+    # 文档 §5.3 示意表：B=0.06, F=960, δD=1 px
+    expected = {5: 0.4340, 10: 1.7361, 15: 3.9063, 20: 6.9444,
+                30: 15.625, 50: 43.4028, 100: 173.611}
+    for z, err in expected.items():
+        assert range_error_abs(z, 960.0, 0.06, 1.0) == pytest.approx(err, rel=1e-3)
+    assert range_error_rel(10.0, 960.0, 0.06, 1.0) == pytest.approx(1.7361 / 10.0, rel=1e-3)
+
+
+def test_on_axis_target_z_and_bearing():
+    left, right = target_pair(0.0, 0.0, 10.0)
+    r = one_report(StereoRanger(), [left], [right], frame_sequence=7, timestamp_s=1.5)[0]
+    assert r.matched and r.valid
+    assert r.forward_distance_m == pytest.approx(10.0, rel=1e-9)
+    assert r.disparity_px == pytest.approx(FB / 10.0)
+    assert r.disparity_source == DISP_SOURCE_EDGE
+    assert r.lateral_m == pytest.approx(0.0, abs=1e-9)
+    assert r.vertical_m == pytest.approx(0.0, abs=1e-9)
+    assert r.slant_range_m == pytest.approx(10.0, rel=1e-9)
+    assert r.frame_sequence == 7 and r.timestamp_s == 1.5
+    # 虚拟双目中心：两目中心平均，y 取左目（§11.3）
+    assert r.centre_pixel_x_mid == pytest.approx((left.cx + right.cx) / 2.0)
+    assert r.centre_pixel_y_mid == left.cy
+
+
+def test_off_axis_target_xyz_and_slant_range():
+    left, right = target_pair(1.2, 0.3, 10.0)
+    r = one_report(StereoRanger(), [left], [right])[0]
+    assert r.valid
+    assert r.lateral_m == pytest.approx(1.2, rel=1e-9)
+    assert r.vertical_m == pytest.approx(0.3, rel=1e-9)
+    assert r.slant_range_m == pytest.approx(math.sqrt(1.2**2 + 0.3**2 + 10.0**2), rel=1e-9)
+    assert r.error_est_m == pytest.approx(range_error_abs(10.0, CFG.fx_px, CFG.baseline_m, CFG.disp_noise_px))
+
+
+def test_x_uses_left_camera_frame_not_virtual_center():
+    # X 定义在左目系（§4.2），用左目中心像素；方位 x_mid 是虚拟相机量（§11.3）
+    left, right = target_pair(0.0, 0.0, 10.0)
+    r = one_report(StereoRanger(), [left], [right])[0]
+    assert r.lateral_m == pytest.approx(0.0, abs=1e-9)  # 目标在左光轴上
+    assert r.centre_pixel_x_mid < left.cx  # 但虚拟中心相对左光轴偏左 B/2
+
+
+def test_width_diff_on_right_edge_only_does_not_taint_left_edge():
+    # §5.1 主方案：同侧（左）边缘对视差免疫"宽度差"。右框仅向 x2 侧扩宽
+    # 时，左边缘不变、中心被拉偏——边缘取点仍得精确 Z。
+    left, right = target_pair(0.0, 0.0, 10.0)
+    right_extended = Detection(right.x1, right.y1, right.x2 + 40.0, right.y2)
+    r = one_report(StereoRanger(), [left], [right_extended])[0]
+    assert r.matched and r.valid
+    assert r.forward_distance_m == pytest.approx(10.0, rel=1e-9)
+    assert r.disparity_source == DISP_SOURCE_EDGE
+    assert right_extended.width != left.width  # 确有宽度差
+
+
+def test_center_fallback_when_edge_out_of_window(caplog):
+    # 左框过宽使左边缘视差越出窗口：回退中心并节流警告（§5.1），同实例内不刷屏
+    left, right = target_pair(0.0, 0.0, 10.0)
+    left_wider = box(left.cx, left.cy, w=300.0, h=60.0)
+    ranger = StereoRanger()
+    r = one_report(ranger, [left_wider], [right])[0]
+    r2 = one_report(ranger, [left_wider], [right])[0]
+    assert r.matched and r.valid
+    assert r.forward_distance_m == pytest.approx(10.0, rel=1e-9)
+    assert r.disparity_source == DISP_SOURCE_CENTER
+    assert r2.disparity_source == DISP_SOURCE_CENTER
+    assert len(caplog.records) == 1
+
+
+def test_center_fallback_disabled_leaves_unmatched():
+    left, right = target_pair(0.0, 0.0, 10.0)
+    left_wider = box(left.cx, left.cy, w=300.0, h=60.0)
+    cfg = StereoConfig(use_center_fallback=False)
+    r = one_report(StereoRanger(cfg), [left_wider], [right])[0]
+    assert not r.matched and not r.valid
+    assert r.forward_distance_m is None
+
+
+# ---- §6 关联硬约束与贪心一一配对 ----
+
+def test_class_mismatch_rejected():
+    left, _ = target_pair(0.0, 0.0, 10.0)
+    _, right = target_pair(0.0, 0.0, 10.0, cls_id=1)
+    r = one_report(StereoRanger(), [left], [right])[0]
+    assert not r.matched
+
+
+def test_vertical_residual_beyond_tau_rejected():
+    left, _ = target_pair(0.0, 0.0, 10.0)
+    right = box(CFG.cx_px - FB / 10.0, left.cy + 3.0)  # |Δy| = 3 > τy = 2
+    r = one_report(StereoRanger(), [left], [right])[0]
+    assert not r.matched
+
+
+def test_height_ratio_beyond_tau_rejected():
+    left, _ = target_pair(0.0, 0.0, 10.0)
+    _, right = target_pair(0.0, 0.0, 10.0, h=40.0)  # |60-40|/60 > 0.25
+    r = one_report(StereoRanger(), [left], [right])[0]
+    assert not r.matched
+
+
+def test_cost_equal_one_rejected():
+    # 文档 §6：仅当 cost < 1 时接受（严格小于）
+    # dy = 1 px -> 0.5，dh 比 = 0.125 -> 0.5，cost 精确 = 1.0
+    left, _ = target_pair(0.0, 0.0, 10.0)
+    x_off = FB / 10.0
+    right = Detection(left.x1 - x_off, left.y1 + 4.75,
+                      left.x1 - x_off + 52.5, left.y1 + 4.75 + 52.5)
+    # 右框中心 = 左框中心 + 1 px（dy=1），高度 52.5（dh 比 0.125）
+    assert right.cy == pytest.approx(left.cy + 1.0)
+    r = one_report(StereoRanger(), [left], [right])[0]
+    assert not r.matched
+
+
+def test_disparity_out_of_z_window_rejected():
+    # 目标过近（D > FB/z_min）与过远（D < FB/z_max）均不可配对（§6）
+    near_l, near_r = target_pair(0.0, 0.0, 0.5)
+    far_l, far_r = target_pair(0.0, 0.0, 60.0)
+    r = one_report(StereoRanger(), [near_l], [near_r])[0]
+    assert not r.matched
+    r = one_report(StereoRanger(), [far_l], [far_r])[0]
+    assert not r.matched
+
+
+def test_greedy_one_to_one_crossing_indices():
+    # 目标 A(光轴上, Z=10) 与 B(右偏, Z=6)；右目框以下标交叉顺序传入。
+    # 贪心须按代价配对：L0->R_A、L1->R_B，即使 R_B 在下标 0 处。
+    l_a, r_a = target_pair(0.0, 0.0, 10.0)
+    l_b, r_b = target_pair(0.3, 0.0, 6.0)
+    reports = one_report(StereoRanger(), [l_a, l_b], [r_b, r_a])
+    assert all(r.matched for r in reports)
+    assert reports[0].forward_distance_m == pytest.approx(10.0, rel=1e-9)
+    assert reports[1].forward_distance_m == pytest.approx(6.0, rel=1e-9)
+    assert reports[0].ref_bbox == (l_a.x1, l_a.y1, l_a.x2, l_a.y2)
+
+
+def test_track_id_continuity_preference():
+    # §6：存在轨迹标识时优先匹配相同 track id
+    l_a, r_a = target_pair(0.0, 0.0, 10.0, track_id=7)
+    l_b, r_b = target_pair(0.3, 0.0, 8.0, track_id=8)
+    reports = one_report(StereoRanger(), [l_a, l_b], [r_b, r_a])
+    by_track = {r.track_id: r.forward_distance_m for r in reports}
+    assert by_track[7] == pytest.approx(10.0, rel=1e-9)
+    assert by_track[8] == pytest.approx(8.0, rel=1e-9)
+
+
+def test_unmatched_left_reports_bearing_only():
+    left, right = target_pair(0.0, 0.0, 10.0)
+    stray = box(500.0, 300.0, cls_id=3, conf=0.8)
+    reports = one_report(StereoRanger(), [left, stray], [right], frame_sequence=42, timestamp_s=1.5)
+    assert reports[0].matched and reports[0].valid
+    u = reports[1]
+    assert not u.matched and not u.valid
+    assert u.forward_distance_m is None
+    assert u.centre_pixel_x_mid == stray.cx and u.centre_pixel_y_mid == stray.cy
+    assert u.confidence == 0.8 and u.class_id == 3
+    assert u.frame_sequence == 42 and u.timestamp_s == 1.5
+
+
+def test_empty_sides():
+    ranger = StereoRanger()
+    assert ranger.update([], []) == []
+    r = one_report(ranger, [box(100.0, 100.0)], [])[0]
+    assert not r.matched
+
+
+def test_report_order_follows_left_detections():
+    ranger = StereoRanger()
+    l1, r1 = target_pair(0.0, 0.0, 10.0, cls_id=0)
+    l2, r2 = target_pair(0.0, 0.0, 8.0, cls_id=2)
+    reports = ranger.update([l2, l1], [r1, r2])  # 输出顺序与输入左目一致
+    assert [r.class_id for r in reports] == [2, 0]
+    assert reports[0].confidence == min(l2.conf, r2.conf)
+
+
+# ---- §5.4 有效性判定 ----
+
+def test_beyond_range_tolerance_invalid_but_distance_kept():
+    left, right = target_pair(0.0, 0.0, 15.0)  # δZ ≈ 3.9 m > 2 m
+    r = one_report(StereoRanger(), [left], [right])[0]
+    assert r.matched
+    assert not r.valid
+    assert r.forward_distance_m == pytest.approx(15.0, rel=1e-9)
+    assert r.lateral_m is not None  # 方位/坐标仍发布（§5.4 域外标记无效）
+    assert r.error_est_m == pytest.approx(range_error_abs(15.0, CFG.fx_px, CFG.baseline_m, CFG.disp_noise_px))
+
+
+def test_within_range_tolerance_valid():
+    left, right = target_pair(0.0, 0.0, 10.0)  # δZ ≈ 1.74 m <= 2 m
+    r = one_report(StereoRanger(), [left], [right])[0]
+    assert r.valid
+
+
+def test_invalid_when_disparity_below_dmin():
+    # 放宽 range_tol 以单独暴露 D_min 判据：D = 2.3 px < 3 px -> 无效
+    cfg = StereoConfig(range_tol_m=50.0)
+    left, right = target_pair(0.0, 0.0, 25.0)
+    r = one_report(StereoRanger(cfg), [left], [right])[0]
+    assert r.matched and not r.valid
+
+
+# ---- §7 时间滤波（可选） ----
+
+def moving_pair(z_m, track_id=1, cls_id=0):
+    """返回 (左框列表, 右框列表)：目标沿光轴移动，track_id 可配。"""
+    left = box(CFG.cx_px, CFG.cy_px, cls_id=cls_id, track_id=track_id)
+    right = box(CFG.cx_px - FB / z_m, CFG.cy_px, cls_id=cls_id, track_id=track_id)
+    return [left], [right]
+
+
+def test_filter_smooths_and_resets_on_jump():
+    cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3, filter_jump_ratio=0.2,
+                       range_tol_m=50.0)  # 放宽有效域，让 10~13 m 均有效以观察滤波
+    ranger = StereoRanger(cfg)
+    z1 = one_report(ranger, *moving_pair(10.0), frame_sequence=1)[0]
+    assert z1.forward_distance_m == pytest.approx(10.0, rel=1e-9)  # 首帧置位
+    z2 = one_report(ranger, *moving_pair(11.0), frame_sequence=2)[0]  # 跳变 10% <= 20%：平滑
+    assert z2.forward_distance_m == pytest.approx(0.7 * 10.0 + 0.3 * 11.0, rel=1e-9)
+    z3 = one_report(ranger, *moving_pair(13.0), frame_sequence=3)[0]  # 跳变 26% > 20%：重置
+    assert z3.forward_distance_m == pytest.approx(13.0, rel=1e-9)
+
+
+def test_filter_skipped_without_track_id():
+    cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3)
+    ranger = StereoRanger(cfg)
+    one_report(ranger, *moving_pair(10.0, track_id=0), frame_sequence=1)
+    z2 = one_report(ranger, *moving_pair(11.0, track_id=0), frame_sequence=2)[0]
+    assert z2.forward_distance_m == pytest.approx(11.0, rel=1e-9)
+
+
+def test_reset_clears_filter_state():
+    cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3, range_tol_m=50.0)
+    ranger = StereoRanger(cfg)
+    one_report(ranger, *moving_pair(10.0), frame_sequence=1)  # 状态 = 10.0
+    ranger.reset()
+    # 若不重置，10 -> 11 会平滑为 10.3；重置后应直接输出原始 11.0
+    z = one_report(ranger, *moving_pair(11.0), frame_sequence=2)[0]
+    assert z.forward_distance_m == pytest.approx(11.0, rel=1e-9)
+
+
+def test_invalid_measurements_do_not_pollute_filter_state():
+    cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3, range_tol_m=50.0)
+    ranger = StereoRanger(cfg)
+    one_report(ranger, *moving_pair(10.0), frame_sequence=1)
+    z2 = one_report(ranger, *moving_pair(25.0), frame_sequence=2)[0]  # D < D_min：无效
+    assert not z2.valid
+    z3 = one_report(ranger, *moving_pair(10.5), frame_sequence=3)[0]
+    # 无效帧未污染状态：仍按与首帧 10.0 平滑
+    assert z3.forward_distance_m == pytest.approx(0.7 * 10.0 + 0.3 * 10.5, rel=1e-9)