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。
104 lines
3.9 KiB
Python
104 lines
3.9 KiB
Python
"""测距计算与有效性判定测试（规格 §3.2/§6.4/§6.6/§7.1/§11.4）。"""
|
||
|
||
import math
|
||
|
||
import pytest
|
||
|
||
from stereo_ranger import (
|
||
Detection,
|
||
StereoConfig,
|
||
confidence_from_cost,
|
||
evaluate_validity,
|
||
forward_distance_m,
|
||
range_error_abs_m,
|
||
range_error_rel,
|
||
solve_range,
|
||
virtual_center_px,
|
||
)
|
||
|
||
CFG = StereoConfig()
|
||
|
||
|
||
def _boxes_for_point(x_m: float, y_m: float, z_m: float, width_px: float = 50.0, height_px: float = 40.0):
|
||
"""由左目系 (X, Y, Z) 反推左右目框（同宽，边缘视差 = 中心视差 = F·B/Z）。"""
|
||
d_px = CFG.focal_baseline_px_m / z_m
|
||
x_l = CFG.cx_px + CFG.fx_px * x_m / z_m
|
||
y_l = CFG.cy_px + CFG.fy_px * y_m / z_m
|
||
left = Detection(x_l - width_px / 2, y_l - height_px / 2, x_l + width_px / 2, y_l + height_px / 2)
|
||
right = Detection(
|
||
x_l - width_px / 2 - d_px, y_l - height_px / 2, x_l + width_px / 2 - d_px, y_l + height_px / 2
|
||
)
|
||
return left, right, d_px
|
||
|
||
|
||
def test_solve_range_recovers_ground_truth():
|
||
# §11.4：已知 Z 反推 D 构造框对，断言 Z = F·B/D 回收
|
||
left, right, d_px = _boxes_for_point(0.6, 0.3, 6.0)
|
||
solution = solve_range(left, right, d_px, CFG)
|
||
assert solution.z_m == pytest.approx(6.0)
|
||
assert solution.x_m == pytest.approx(0.6)
|
||
assert solution.y_m == pytest.approx(0.3)
|
||
assert solution.r_m == pytest.approx(math.sqrt(0.6**2 + 0.3**2 + 6.0**2))
|
||
|
||
|
||
def test_forward_distance_rejects_non_positive_disparity():
|
||
with pytest.raises(ValueError):
|
||
forward_distance_m(0.0, CFG)
|
||
with pytest.raises(ValueError):
|
||
forward_distance_m(-2.0, CFG)
|
||
|
||
|
||
def test_virtual_center_equals_midline_camera_projection():
|
||
# §6.6 恒等式：xL = cx + F(X_C + B/2)/Z，xR = cx + F(X_C − B/2)/Z ⇒ c_mid = cx + F·X_C/Z
|
||
z_m = 5.0
|
||
x_center_m = 0.57
|
||
x_l = CFG.cx_px + CFG.fx_px * (x_center_m + CFG.baseline_m / 2.0) / z_m
|
||
x_r = CFG.cx_px + CFG.fx_px * (x_center_m - CFG.baseline_m / 2.0) / z_m
|
||
left = Detection(x_l - 30.0, 480.0, x_l + 30.0, 520.0)
|
||
right = Detection(x_r - 30.0, 480.0, x_r + 30.0, 520.0)
|
||
center_mid = virtual_center_px(left, right)
|
||
assert center_mid[0] == pytest.approx(CFG.cx_px + CFG.fx_px * x_center_m / z_m)
|
||
assert center_mid[1] == pytest.approx(500.0)
|
||
|
||
|
||
def test_error_model_is_quadratic_in_z():
|
||
assert range_error_abs_m(10.0, CFG) == pytest.approx(10.0**2 / CFG.focal_baseline_px_m)
|
||
assert range_error_rel(10.0, CFG) == pytest.approx(10.0 / CFG.focal_baseline_px_m)
|
||
assert range_error_abs_m(20.0, CFG) == pytest.approx(4.0 * range_error_abs_m(10.0, CFG))
|
||
|
||
|
||
def test_validity_gate_1_out_of_domain():
|
||
decision = evaluate_validity(CFG.disparity_min_px - 0.1, 30.0, CFG)
|
||
assert not decision.in_domain and not decision.valid
|
||
|
||
|
||
def test_validity_gate_2_error_budget_blocks_far_field():
|
||
# 域内（D = 4.8 px → Z = 12 m）但 δZ = 2.5 m > 2 m，预算门拦截
|
||
d_px = 4.8
|
||
z_m = CFG.focal_baseline_px_m / d_px
|
||
decision = evaluate_validity(d_px, z_m, CFG)
|
||
assert decision.in_domain and not decision.budget_ok and not decision.valid
|
||
|
||
|
||
def test_validity_gate_2_boundary_sides():
|
||
z_limit = CFG.z_error_budget_max_m
|
||
assert evaluate_validity(CFG.focal_baseline_px_m / z_limit, z_limit * 0.999, CFG).budget_ok
|
||
assert not evaluate_validity(CFG.focal_baseline_px_m / z_limit, z_limit * 1.001, CFG).budget_ok
|
||
|
||
|
||
def test_validity_requires_finite_z():
|
||
assert not evaluate_validity(5.0, None, CFG).valid
|
||
assert not evaluate_validity(5.0, float("inf"), CFG).valid
|
||
|
||
|
||
def test_domain_boundaries_are_inclusive():
|
||
assert evaluate_validity(CFG.disparity_min_px, 19.2, CFG).in_domain
|
||
assert evaluate_validity(CFG.disparity_max_px, 1.0, CFG).in_domain
|
||
|
||
|
||
def test_confidence_from_cost_linear_clamped():
|
||
assert confidence_from_cost(0.0) == pytest.approx(1.0)
|
||
assert confidence_from_cost(0.4) == pytest.approx(0.6)
|
||
assert confidence_from_cost(1.0) == pytest.approx(0.0)
|
||
assert confidence_from_cost(1.5) == pytest.approx(0.0)
|