双目测距方案重构与其他小修改

utils/stereo_ranger/geometry.py

@@ -0,0 +1,84 @@
+"""测距计算与有效性判定（§3.2/§6.4/§7.1）：纯函数、无状态。
+
+Z = F·B/D（前向分量，距离通道唯一消费量，§10.5）；X/Y 以左目系表达；
+R = sqrt(X² + Y² + Z²) 仅参考输出；c_mid = 两目框中心等权平均（虚拟双目
+中心，§6.6，逐点精确、无近似）。
+"""
+
+import math
+from dataclasses import dataclass
+from typing import Optional, Tuple
+
+from .config import StereoConfig
+from .types import Detection
+
+DELTA_D_PX = 1.0  # 误差预算的δD 基准（§7.2 定义 δZ(δD = 1px)）
+
+
+@dataclass(frozen=True)
+class RangeSolution:
+    """测距解算结果（Z 为前向分量，X/Y 属左目系，§3.2）。"""
+
+    z_m: float
+    x_m: float
+    y_m: float
+    r_m: float          # 斜距（参考值，不进控制回路）
+
+
+@dataclass(frozen=True)
+class ValidityDecision:
+    """有效性判定结果（§6.4 三道门的前两道；门③一致性由调用方合流）。"""
+
+    in_domain: bool  # ① 域内：D ∈ [D_min, D_max]
+    budget_ok: bool  # ② 误差预算：δZ(δD = 1px) ≤ δZ_max
+    valid: bool      # ① 且 ②
+
+
+def forward_distance_m(disparity_px: float, config: StereoConfig) -> float:
+    """Z = F·B/D；调用方须先判 D ≤ 0 物理违例（§6.3/§11.2 除零纪律）。"""
+    if disparity_px <= 0.0:
+        raise ValueError(f"视差须为正（先判违例再除）: D={disparity_px}")
+    return config.focal_baseline_px_m / disparity_px
+
+
+def solve_range(left_box: Detection, right_box: Detection, disparity_px: float, config: StereoConfig) -> RangeSolution:
+    """由视差与参考目（左目）框中心解算 (Z, X, Y, R)（§3.2）。"""
+    z_m = forward_distance_m(disparity_px, config)
+    x_m = (left_box.cx - config.cx_px) * z_m / config.fx_px
+    y_m = (left_box.cy - config.cy_px) * z_m / config.fy_px
+    r_m = math.sqrt(x_m * x_m + y_m * y_m + z_m * z_m)
+    return RangeSolution(z_m=z_m, x_m=x_m, y_m=y_m, r_m=r_m)
+
+
+def virtual_center_px(left_box: Detection, right_box: Detection) -> Tuple[float, float]:
+    """虚拟双目中心 c_mid = (cL + cR)/2（§6.6：等权平均 = 把相机搬到模组中线）。
+
+    与视差取点（同侧边缘）是两个不同的取点，不得混用（§6.6）。
+    """
+    return ((left_box.cx + right_box.cx) / 2.0, (left_box.cy + right_box.cy) / 2.0)
+
+
+def range_error_abs_m(z_m: float, config: StereoConfig, delta_d_px: float = DELTA_D_PX) -> float:
+    """绝对误差估计 δZ ≈ Z²·δD/(F·B)（§7.1）。"""
+    return z_m * z_m * delta_d_px / config.focal_baseline_px_m
+
+
+def range_error_rel(z_m: float, config: StereoConfig, delta_d_px: float = DELTA_D_PX) -> float:
+    """相对误差估计 δZ/Z ≈ Z·δD/(F·B)（§7.1）。"""
+    return z_m * delta_d_px / config.focal_baseline_px_m
+
+
+def evaluate_validity(disparity_px: float, z_m: Optional[float], config: StereoConfig) -> ValidityDecision:
+    """有效性门①②（§6.4）：域内 + 误差预算；z_m=None（D ≤ 0）时预算判失败。"""
+    in_domain = config.disparity_min_px <= disparity_px <= config.disparity_max_px
+    budget_ok = (
+        z_m is not None
+        and math.isfinite(z_m)
+        and range_error_abs_m(z_m, config) <= config.max_delta_z_m
+    )
+    return ValidityDecision(in_domain=in_domain, budget_ok=budget_ok, valid=in_domain and budget_ok)
+
+
+def confidence_from_cost(cost: float) -> float:
+    """匹配代价 → 置信度基值：线性映射 clamp(1 − cost, 0, 1)（§6.4 ⚠占位映射）。"""
+    return min(1.0, max(0.0, 1.0 - cost))