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。
293 lines
12 KiB
Python
293 lines
12 KiB
Python
"""全流程测试（规格 §5/§11.4：单目标、多目标、单目退化、非同帧拒收、越界、跳变）。
|
|
|
|
合成框对驱动，无需真实图像：几何是解析可断言的（§11.4）。
|
|
"""
|
|
|
|
import logging
|
|
import math
|
|
from typing import List, Optional, Tuple
|
|
|
|
import pytest
|
|
|
|
from stereo_ranger import (
|
|
Degradation,
|
|
Detection,
|
|
DetectionFrame,
|
|
Eye,
|
|
MatchPair,
|
|
StereoConfig,
|
|
StereoRanger,
|
|
)
|
|
|
|
CFG = StereoConfig()
|
|
LOGGER_NAME = "stereo_ranger"
|
|
|
|
|
|
def _boxes(
|
|
x_center_m: float = 0.0,
|
|
y_m: float = 0.0,
|
|
z_m: float = 6.0,
|
|
*,
|
|
w_m: float = 0.5,
|
|
h_m: float = 0.3,
|
|
class_id: int = 0,
|
|
track_id: Optional[int] = None,
|
|
confidence: float = 0.95,
|
|
) -> Tuple[Detection, Detection]:
|
|
"""模组中心系 (X_C, Y, Z) 目标 → 左右目检测框（两目同宽，边缘视差 = 中心视差）。"""
|
|
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
|
|
y = CFG.cy_px + CFG.fy_px * y_m / z_m
|
|
w_px = CFG.fx_px * w_m / z_m
|
|
h_px = CFG.fy_px * h_m / z_m
|
|
|
|
def _box(cx_px: float) -> Detection:
|
|
return Detection(
|
|
cx_px - w_px / 2.0,
|
|
y - h_px / 2.0,
|
|
cx_px + w_px / 2.0,
|
|
y + h_px / 2.0,
|
|
confidence=confidence,
|
|
class_id=class_id,
|
|
track_id=track_id,
|
|
)
|
|
|
|
return _box(x_l), _box(x_r)
|
|
|
|
|
|
def _pair_with_disparity(d_px: float, *, y_center: float = 500.0, class_id: int = 0) -> Tuple[Detection, Detection]:
|
|
"""构造视差恰为 d_px 的等宽框对（d 取 0.5 的整数倍时浮点精确）。"""
|
|
width, height = 60.0, 40.0
|
|
left = Detection(1000.0 - width / 2, y_center - height / 2, 1000.0 + width / 2, y_center + height / 2, class_id=class_id)
|
|
right = Detection(
|
|
1000.0 - d_px - width / 2,
|
|
y_center - height / 2,
|
|
1000.0 - d_px + width / 2,
|
|
y_center + height / 2,
|
|
class_id=class_id,
|
|
)
|
|
return left, right
|
|
|
|
|
|
def _frame(frame_id: int, detections: List[Detection]) -> DetectionFrame:
|
|
return DetectionFrame(frame_id=frame_id, detections=detections)
|
|
|
|
|
|
def test_single_target_complete_valid_record():
|
|
ranger = StereoRanger()
|
|
left, right = _boxes(x_center_m=0.0, z_m=6.0)
|
|
records = ranger.update(_frame(1, [left]), _frame(1, [right]), now_ms=1000)
|
|
|
|
assert len(records) == 1
|
|
record = records[0]
|
|
assert record.degradation is Degradation.COMPLETE
|
|
assert record.valid is True
|
|
assert record.forward_distance_m == pytest.approx(6.0)
|
|
assert record.slant_range_m == pytest.approx(math.hypot(CFG.baseline_m / 2.0, 6.0)) # R 含左目系 +B/2 横向偏移
|
|
assert record.disparity_px == pytest.approx(CFG.focal_baseline_px_m / 6.0)
|
|
assert record.center_mid_px == pytest.approx((CFG.cx_px, CFG.cy_px))
|
|
assert record.reference_box == left.bbox
|
|
assert record.reference_eye is Eye.LEFT
|
|
assert record.confidence == pytest.approx(1.0)
|
|
assert record.frame_id == 1 and record.timestamp_ms == 1000
|
|
|
|
|
|
def test_filter_smooths_across_valid_frames():
|
|
ranger = StereoRanger()
|
|
left_1, right_1 = _boxes(z_m=6.0)
|
|
left_2, right_2 = _boxes(z_m=6.5)
|
|
ranger.update(_frame(1, [left_1]), _frame(1, [right_1]))
|
|
records = ranger.update(_frame(2, [left_2]), _frame(2, [right_2]))
|
|
assert records[0].valid is True
|
|
assert records[0].forward_distance_m == pytest.approx(6.0 + 0.3 * (6.5 - 6.0))
|
|
# 参考斜距与已发布 Z 同基准：按 z_out/z_raw 从本帧原始斜距精确缩放（R ≥ Z）
|
|
raw_slant = math.hypot(CFG.baseline_m / 2.0, 6.5)
|
|
assert records[0].slant_range_m == pytest.approx(raw_slant * 6.15 / 6.5)
|
|
|
|
|
|
def test_jump_reset_skips_smoothing():
|
|
ranger = StereoRanger()
|
|
left_1, right_1 = _boxes(z_m=4.0)
|
|
left_2, right_2 = _boxes(z_m=10.0) # 跳变 6 m > jump_reset_m = 5 m
|
|
ranger.update(_frame(1, [left_1]), _frame(1, [right_1]))
|
|
records = ranger.update(_frame(2, [left_2]), _frame(2, [right_2]))
|
|
assert records[0].forward_distance_m == pytest.approx(10.0)
|
|
|
|
|
|
def test_far_field_out_of_domain_still_matched():
|
|
# 越界 = 有效性门（决策）：配对仍成立，c_mid（无偏方位）与 D 照常发布，无 Z
|
|
ranger = StereoRanger()
|
|
left, right = _boxes(x_center_m=0.6, z_m=25.0) # D ≈ 2.3 px < 3 px
|
|
records = ranger.update(_frame(1, [left]), _frame(1, [right]))
|
|
|
|
record = records[0]
|
|
assert record.degradation is Degradation.COMPLETE
|
|
assert record.valid is False
|
|
assert record.forward_distance_m is None
|
|
assert record.disparity_px == pytest.approx(2.304)
|
|
assert record.center_mid_px == pytest.approx((CFG.cx_px + CFG.fx_px * 0.6 / 25.0, CFG.cy_px))
|
|
assert record.slant_range_m == pytest.approx(math.hypot(0.6 + CFG.baseline_m / 2.0, 25.0))
|
|
|
|
|
|
def test_error_budget_gate_blocks_far_but_in_domain():
|
|
ranger = StereoRanger()
|
|
left, right = _boxes(z_m=12.0) # D = 4.8 px 域内，δZ = 2.5 m > 2 m
|
|
record = ranger.update(_frame(1, [left]), _frame(1, [right]))[0]
|
|
assert record.disparity_px == pytest.approx(4.8)
|
|
assert record.valid is False
|
|
assert record.forward_distance_m is None
|
|
assert record.slant_range_m == pytest.approx(math.hypot(CFG.baseline_m / 2.0, 12.0))
|
|
|
|
|
|
def test_error_budget_gate_passes_just_inside():
|
|
ranger = StereoRanger()
|
|
left, right = _pair_with_disparity(5.8) # Z ≈ 9.93 m，δZ ≈ 1.71 m ≤ 2 m
|
|
record = ranger.update(_frame(1, [left]), _frame(1, [right]))[0]
|
|
assert record.valid is True
|
|
assert record.forward_distance_m == pytest.approx(CFG.focal_baseline_px_m / 5.8)
|
|
|
|
|
|
def test_negative_disparity_violation(caplog):
|
|
ranger = StereoRanger()
|
|
left = Detection(1000.0, 500.0, 1060.0, 540.0)
|
|
right = Detection(1010.0, 500.0, 1070.0, 540.0)
|
|
with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
|
|
record = ranger.update(_frame(1, [left]), _frame(1, [right]))[0]
|
|
assert record.degradation is Degradation.COMPLETE
|
|
assert record.valid is False
|
|
assert record.forward_distance_m is None
|
|
assert record.slant_range_m is None
|
|
assert record.disparity_px == pytest.approx(-10.0)
|
|
assert any("disparity_violation" in message for message in caplog.messages)
|
|
|
|
|
|
def test_center_fallback_keeps_target_valid(caplog):
|
|
ranger = StereoRanger()
|
|
left = Detection(1000.0, 500.0, 1060.0, 540.0) # 宽 60, center 1030
|
|
right = Detection(940.0, 500.0, 1040.0, 540.0) # 宽 100, center 990
|
|
with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
|
|
record = ranger.update(_frame(1, [left]), _frame(1, [right]))[0]
|
|
assert record.disparity_px == pytest.approx(40.0) # 边缘 60 px 越窗 → 中心 40 px
|
|
assert record.valid is True
|
|
assert record.forward_distance_m == pytest.approx(CFG.focal_baseline_px_m / 40.0)
|
|
assert any("center_fallback" in message for message in caplog.messages)
|
|
|
|
|
|
def test_single_eye_degradation_left_and_right():
|
|
ranger = StereoRanger()
|
|
left, right = _boxes(x_center_m=0.3, z_m=4.0)
|
|
|
|
left_only = ranger.update(_frame(1, [left]), _frame(1, []))
|
|
assert len(left_only) == 1
|
|
record = left_only[0]
|
|
assert record.degradation is Degradation.BEARING_ONLY
|
|
assert record.valid is False
|
|
assert record.center_mid_px == pytest.approx((left.cx, left.cy))
|
|
assert record.forward_distance_m is None and record.disparity_px is None
|
|
assert record.reference_eye is Eye.LEFT
|
|
assert record.confidence == pytest.approx(left.confidence)
|
|
|
|
right_only = ranger.update(_frame(2, []), _frame(2, [right]))
|
|
assert len(right_only) == 1
|
|
assert right_only[0].reference_eye is Eye.RIGHT
|
|
assert right_only[0].center_mid_px == pytest.approx((right.cx, right.cy))
|
|
|
|
|
|
def test_records_order_left_first_then_unmatched_right():
|
|
ranger = StereoRanger()
|
|
left, right_matched = _boxes(z_m=6.0, class_id=0)
|
|
_, right_extra = _boxes(z_m=6.0, class_id=0, y_m=0.0)
|
|
right_extra = Detection(400.0, 500.0, 460.0, 540.0) # 远处无对应左框
|
|
records = ranger.update(_frame(1, [left]), _frame(1, [right_matched, right_extra]))
|
|
assert len(records) == 2
|
|
assert records[0].degradation is Degradation.COMPLETE
|
|
assert records[0].valid is True
|
|
assert records[1].degradation is Degradation.BEARING_ONLY
|
|
assert records[1].reference_eye is Eye.RIGHT
|
|
|
|
|
|
def test_frame_id_mismatch_rejects_frame_and_throttles_warning(caplog):
|
|
ranger = StereoRanger()
|
|
left, right = _boxes()
|
|
with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
|
|
assert ranger.update(_frame(1, [left]), _frame(2, [right])) == []
|
|
assert ranger.update(_frame(3, [left]), _frame(4, [right])) == []
|
|
warnings = [message for message in caplog.messages if "frame_mismatch" in message]
|
|
assert len(warnings) == 1 # 第 1 次打印，第 2 次被节流
|
|
assert "累计 1 次" in warnings[0]
|
|
|
|
|
|
def test_empty_input_returns_empty_without_warning(caplog):
|
|
ranger = StereoRanger()
|
|
with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
|
|
assert ranger.update(_frame(1, []), _frame(1, [])) == []
|
|
assert caplog.messages == []
|
|
|
|
|
|
def test_non_finite_box_dropped_with_warning(caplog):
|
|
ranger = StereoRanger()
|
|
bad = Detection(float("nan"), 500.0, 1060.0, 540.0)
|
|
good_left, good_right = _boxes(z_m=6.0)
|
|
with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
|
|
records = ranger.update(_frame(1, [bad, good_left]), _frame(1, [good_right]))
|
|
assert len(records) == 1
|
|
assert records[0].valid is True
|
|
assert any("invalid_box" in message for message in caplog.messages)
|
|
|
|
|
|
def test_consistency_degrades_then_invalidates(caplog):
|
|
ranger = StereoRanger()
|
|
disparities = [9.5, 13.5, 17.5, 21.5, 25.5, 29.5] # 每帧跳变 4 px > 3 px
|
|
records = []
|
|
with caplog.at_level(logging.WARNING, logger=LOGGER_NAME):
|
|
for index, d_px in enumerate(disparities, start=1):
|
|
left, right = _pair_with_disparity(d_px)
|
|
records.append(ranger.update(_frame(index, [left]), _frame(index, [right]))[0])
|
|
|
|
assert [record.valid for record in records] == [True, True, True, True, True, False]
|
|
assert records[3].confidence == pytest.approx(0.5) # 第 3 帧起置信度降级（N = 3）
|
|
assert records[5].forward_distance_m is None
|
|
assert records[5].confidence == pytest.approx(0.0)
|
|
assert any("consistency_invalid" in message for message in caplog.messages)
|
|
|
|
|
|
def test_track_id_keeps_state_across_large_pixel_move():
|
|
ranger = StereoRanger()
|
|
left_1, right_1 = _boxes(x_center_m=0.6, z_m=4.0, track_id=7)
|
|
left_2, right_2 = _boxes(x_center_m=0.0, z_m=4.5, track_id=7) # c_mid 移动 144 px > 门限
|
|
ranger.update(_frame(1, [left_1]), _frame(1, [right_1]))
|
|
records = ranger.update(_frame(2, [left_2]), _frame(2, [right_2]))
|
|
assert records[0].track_id == 7
|
|
assert records[0].forward_distance_m == pytest.approx(4.0 + 0.3 * (4.5 - 4.0))
|
|
|
|
|
|
def test_without_track_id_large_move_starts_new_state():
|
|
ranger = StereoRanger()
|
|
left_1, right_1 = _boxes(x_center_m=0.6, z_m=4.0)
|
|
left_2, right_2 = _boxes(x_center_m=0.0, z_m=4.5) # 最近邻越门限 → 新目标状态
|
|
ranger.update(_frame(1, [left_1]), _frame(1, [right_1]))
|
|
records = ranger.update(_frame(2, [left_2]), _frame(2, [right_2]))
|
|
assert records[0].forward_distance_m == pytest.approx(4.5) # 未平滑
|
|
|
|
|
|
def test_custom_matcher_injection():
|
|
class _NoMatchMatcher:
|
|
def match(self, left, right, config) -> List[MatchPair]:
|
|
return []
|
|
|
|
ranger = StereoRanger(matcher=_NoMatchMatcher())
|
|
left, right = _boxes(z_m=6.0)
|
|
records = ranger.update(_frame(1, [left]), _frame(1, [right]))
|
|
assert len(records) == 2
|
|
assert all(record.degradation is Degradation.BEARING_ONLY for record in records)
|
|
|
|
|
|
def test_reset_clears_state():
|
|
ranger = StereoRanger()
|
|
left_1, right_1 = _boxes(z_m=6.0)
|
|
left_2, right_2 = _boxes(z_m=6.5)
|
|
ranger.update(_frame(1, [left_1]), _frame(1, [right_1]))
|
|
ranger.reset()
|
|
records = ranger.update(_frame(2, [left_2]), _frame(2, [right_2]))
|
|
assert records[0].forward_distance_m == pytest.approx(6.5) # 未平滑（状态已清空）
|