85 lines
3.5 KiB
Python
85 lines
3.5 KiB
Python
"""测距计算与有效性判定（§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))
|