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

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