keita
df91b1110e
归档 legacy/：stereo_yolo/stereo_ranger/target_tracker 三包 + 175 条测试 + demo/配置/权重/三份规格；运行链路不再依赖，默认 pytest 446 条，归档测试按需运行。 目标有效性第二判据改为 LOS 运动学一致性（pod_bridge/los_consistency.py），删除 require_vision；pod_locked() 叠加该判据，状态机随之后退重搜。 自定义返航（task_runner/return_home.py）：保持转场高度回起飞点 → 恢复初始航向 → 垂直降落，不再爬 RTL_RETURN_ALT；未起飞/无起飞点/超时/异常仍退回 PX4 原生 RTL。 任务链安全兜底：主循环异常→紧急返航、stop 幂等、landing_stop 保留遥测、in_air 流复用+超时、解锁/起飞与吊舱识别仅执行一次、rtl_timeout 上限。 入口共享件拆到 utils/sitl_support.py；pod_sitl_test.py→l1_sitl_test.py；删除旧架构 test.py；相机脚本移入 tools/。 新增 .gitignore 并剔除历史误跟踪的 59 个 .pyc；requirements 按实际依赖重写；文档全面对齐，新增 legacy/README.md 与 tools/README.md。
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))
|