新增双目yolo实现python代码

2026-09-14 15:05:03 +08:00
parent 538ae4c6c7
commit 3a8e2c7f4b
65 changed files with 2467 additions and 10 deletions
@@ -0,0 +1,292 @@
+"""全流程测试（规格 §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)  # 未平滑（状态已清空）