303 lines
13 KiB
Python
303 lines
13 KiB
Python
"""stereo_ranger 测距核心单元测试。
|
||
|
||
规格：docs/双目测距方案.md §4（几何）、§5（视差/有效性/误差）、§6（关联）、
|
||
§7（滤波）。场景全部由"目标在左目系 (X, Y, Z)"解析投影生成左右框，
|
||
再反推验证测距/方位输出。
|
||
"""
|
||
|
||
import math
|
||
|
||
import pytest
|
||
|
||
from stereo_ranger.config import StereoConfig
|
||
from stereo_ranger.stereo_ranger import (
|
||
DISP_SOURCE_CENTER,
|
||
DISP_SOURCE_EDGE,
|
||
StereoRanger,
|
||
focal_from_hfov,
|
||
range_error_abs,
|
||
range_error_rel,
|
||
)
|
||
from stereo_ranger.types import Detection
|
||
|
||
CFG = StereoConfig()
|
||
FB = CFG.fx_px * CFG.baseline_m # 57.6 px·m
|
||
|
||
|
||
def box(cx, cy, w=100.0, h=60.0, cls_id=0, conf=1.0, track_id=0):
|
||
return Detection(cx - w / 2, cy - h / 2, cx + w / 2, cy + h / 2,
|
||
conf, cls_id, track_id)
|
||
|
||
|
||
def target_pair(x_m, y_m, z_m, w=100.0, h=60.0, cls_id=0, conf=1.0, track_id=0):
|
||
"""目标中心在左目系 (x_m, y_m, z_m)，解析投影为左右目同尺寸框。"""
|
||
u_l = CFG.fx_px * x_m / z_m + CFG.cx_px
|
||
v_l = CFG.fy_px * y_m / z_m + CFG.cy_px
|
||
u_r = u_l - FB / z_m
|
||
return (box(u_l, v_l, w, h, cls_id, conf, track_id),
|
||
box(u_r, v_l, w, h, cls_id, conf, track_id))
|
||
|
||
|
||
def one_report(ranger, left, right, **kw):
|
||
reports = ranger.update(left, right, **kw)
|
||
assert len(reports) == len(left)
|
||
return reports
|
||
|
||
|
||
# ---- §4.2 / §5.3 几何与误差公式 ----
|
||
|
||
def test_focal_from_hfov_matches_doc_examples():
|
||
assert focal_from_hfov(1920.0, 90.0) == pytest.approx(960.0)
|
||
assert focal_from_hfov(1920.0, 60.0) == pytest.approx(1662.769)
|
||
|
||
|
||
def test_range_error_matches_doc_table():
|
||
# 文档 §5.3 示意表：B=0.06, F=960, δD=1 px
|
||
expected = {5: 0.4340, 10: 1.7361, 15: 3.9063, 20: 6.9444,
|
||
30: 15.625, 50: 43.4028, 100: 173.611}
|
||
for z, err in expected.items():
|
||
assert range_error_abs(z, 960.0, 0.06, 1.0) == pytest.approx(err, rel=1e-3)
|
||
assert range_error_rel(10.0, 960.0, 0.06, 1.0) == pytest.approx(1.7361 / 10.0, rel=1e-3)
|
||
|
||
|
||
def test_on_axis_target_z_and_bearing():
|
||
left, right = target_pair(0.0, 0.0, 10.0)
|
||
r = one_report(StereoRanger(), [left], [right], frame_sequence=7, timestamp_s=1.5)[0]
|
||
assert r.matched and r.valid
|
||
assert r.forward_distance_m == pytest.approx(10.0, rel=1e-9)
|
||
assert r.disparity_px == pytest.approx(FB / 10.0)
|
||
assert r.disparity_source == DISP_SOURCE_EDGE
|
||
assert r.lateral_m == pytest.approx(0.0, abs=1e-9)
|
||
assert r.vertical_m == pytest.approx(0.0, abs=1e-9)
|
||
assert r.slant_range_m == pytest.approx(10.0, rel=1e-9)
|
||
assert r.frame_sequence == 7 and r.timestamp_s == 1.5
|
||
# 虚拟双目中心：两目中心平均，y 取左目（§11.3）
|
||
assert r.centre_pixel_x_mid == pytest.approx((left.cx + right.cx) / 2.0)
|
||
assert r.centre_pixel_y_mid == left.cy
|
||
|
||
|
||
def test_off_axis_target_xyz_and_slant_range():
|
||
left, right = target_pair(1.2, 0.3, 10.0)
|
||
r = one_report(StereoRanger(), [left], [right])[0]
|
||
assert r.valid
|
||
assert r.lateral_m == pytest.approx(1.2, rel=1e-9)
|
||
assert r.vertical_m == pytest.approx(0.3, rel=1e-9)
|
||
assert r.slant_range_m == pytest.approx(math.sqrt(1.2**2 + 0.3**2 + 10.0**2), rel=1e-9)
|
||
assert r.error_est_m == pytest.approx(range_error_abs(10.0, CFG.fx_px, CFG.baseline_m, CFG.disp_noise_px))
|
||
|
||
|
||
def test_x_uses_left_camera_frame_not_virtual_center():
|
||
# X 定义在左目系（§4.2），用左目中心像素；方位 x_mid 是虚拟相机量（§11.3）
|
||
left, right = target_pair(0.0, 0.0, 10.0)
|
||
r = one_report(StereoRanger(), [left], [right])[0]
|
||
assert r.lateral_m == pytest.approx(0.0, abs=1e-9) # 目标在左光轴上
|
||
assert r.centre_pixel_x_mid < left.cx # 但虚拟中心相对左光轴偏左 B/2
|
||
|
||
|
||
def test_width_diff_on_right_edge_only_does_not_taint_left_edge():
|
||
# §5.1 主方案：同侧（左）边缘对视差免疫"宽度差"。右框仅向 x2 侧扩宽
|
||
# 时，左边缘不变、中心被拉偏——边缘取点仍得精确 Z。
|
||
left, right = target_pair(0.0, 0.0, 10.0)
|
||
right_extended = Detection(right.x1, right.y1, right.x2 + 40.0, right.y2)
|
||
r = one_report(StereoRanger(), [left], [right_extended])[0]
|
||
assert r.matched and r.valid
|
||
assert r.forward_distance_m == pytest.approx(10.0, rel=1e-9)
|
||
assert r.disparity_source == DISP_SOURCE_EDGE
|
||
assert right_extended.width != left.width # 确有宽度差
|
||
|
||
|
||
def test_center_fallback_when_edge_out_of_window(caplog):
|
||
# 左框过宽使左边缘视差越出窗口：回退中心并节流警告（§5.1），同实例内不刷屏
|
||
left, right = target_pair(0.0, 0.0, 10.0)
|
||
left_wider = box(left.cx, left.cy, w=300.0, h=60.0)
|
||
ranger = StereoRanger()
|
||
r = one_report(ranger, [left_wider], [right])[0]
|
||
r2 = one_report(ranger, [left_wider], [right])[0]
|
||
assert r.matched and r.valid
|
||
assert r.forward_distance_m == pytest.approx(10.0, rel=1e-9)
|
||
assert r.disparity_source == DISP_SOURCE_CENTER
|
||
assert r2.disparity_source == DISP_SOURCE_CENTER
|
||
assert len(caplog.records) == 1
|
||
|
||
|
||
def test_center_fallback_disabled_leaves_unmatched():
|
||
left, right = target_pair(0.0, 0.0, 10.0)
|
||
left_wider = box(left.cx, left.cy, w=300.0, h=60.0)
|
||
cfg = StereoConfig(use_center_fallback=False)
|
||
r = one_report(StereoRanger(cfg), [left_wider], [right])[0]
|
||
assert not r.matched and not r.valid
|
||
assert r.forward_distance_m is None
|
||
|
||
|
||
# ---- §6 关联硬约束与贪心一一配对 ----
|
||
|
||
def test_class_mismatch_rejected():
|
||
left, _ = target_pair(0.0, 0.0, 10.0)
|
||
_, right = target_pair(0.0, 0.0, 10.0, cls_id=1)
|
||
r = one_report(StereoRanger(), [left], [right])[0]
|
||
assert not r.matched
|
||
|
||
|
||
def test_vertical_residual_beyond_tau_rejected():
|
||
left, _ = target_pair(0.0, 0.0, 10.0)
|
||
right = box(CFG.cx_px - FB / 10.0, left.cy + 3.0) # |Δy| = 3 > τy = 2
|
||
r = one_report(StereoRanger(), [left], [right])[0]
|
||
assert not r.matched
|
||
|
||
|
||
def test_height_ratio_beyond_tau_rejected():
|
||
left, _ = target_pair(0.0, 0.0, 10.0)
|
||
_, right = target_pair(0.0, 0.0, 10.0, h=40.0) # |60-40|/60 > 0.25
|
||
r = one_report(StereoRanger(), [left], [right])[0]
|
||
assert not r.matched
|
||
|
||
|
||
def test_cost_equal_one_rejected():
|
||
# 文档 §6：仅当 cost < 1 时接受（严格小于）
|
||
# dy = 1 px -> 0.5，dh 比 = 0.125 -> 0.5，cost 精确 = 1.0
|
||
left, _ = target_pair(0.0, 0.0, 10.0)
|
||
x_off = FB / 10.0
|
||
right = Detection(left.x1 - x_off, left.y1 + 4.75,
|
||
left.x1 - x_off + 52.5, left.y1 + 4.75 + 52.5)
|
||
# 右框中心 = 左框中心 + 1 px（dy=1），高度 52.5（dh 比 0.125）
|
||
assert right.cy == pytest.approx(left.cy + 1.0)
|
||
r = one_report(StereoRanger(), [left], [right])[0]
|
||
assert not r.matched
|
||
|
||
|
||
def test_disparity_out_of_z_window_rejected():
|
||
# 目标过近（D > FB/z_min）与过远（D < FB/z_max）均不可配对（§6）
|
||
near_l, near_r = target_pair(0.0, 0.0, 0.5)
|
||
far_l, far_r = target_pair(0.0, 0.0, 60.0)
|
||
r = one_report(StereoRanger(), [near_l], [near_r])[0]
|
||
assert not r.matched
|
||
r = one_report(StereoRanger(), [far_l], [far_r])[0]
|
||
assert not r.matched
|
||
|
||
|
||
def test_greedy_one_to_one_crossing_indices():
|
||
# 目标 A(光轴上, Z=10) 与 B(右偏, Z=6)；右目框以下标交叉顺序传入。
|
||
# 贪心须按代价配对：L0->R_A、L1->R_B，即使 R_B 在下标 0 处。
|
||
l_a, r_a = target_pair(0.0, 0.0, 10.0)
|
||
l_b, r_b = target_pair(0.3, 0.0, 6.0)
|
||
reports = one_report(StereoRanger(), [l_a, l_b], [r_b, r_a])
|
||
assert all(r.matched for r in reports)
|
||
assert reports[0].forward_distance_m == pytest.approx(10.0, rel=1e-9)
|
||
assert reports[1].forward_distance_m == pytest.approx(6.0, rel=1e-9)
|
||
assert reports[0].ref_bbox == (l_a.x1, l_a.y1, l_a.x2, l_a.y2)
|
||
|
||
|
||
def test_track_id_continuity_preference():
|
||
# §6：存在轨迹标识时优先匹配相同 track id
|
||
l_a, r_a = target_pair(0.0, 0.0, 10.0, track_id=7)
|
||
l_b, r_b = target_pair(0.3, 0.0, 8.0, track_id=8)
|
||
reports = one_report(StereoRanger(), [l_a, l_b], [r_b, r_a])
|
||
by_track = {r.track_id: r.forward_distance_m for r in reports}
|
||
assert by_track[7] == pytest.approx(10.0, rel=1e-9)
|
||
assert by_track[8] == pytest.approx(8.0, rel=1e-9)
|
||
|
||
|
||
def test_unmatched_left_reports_bearing_only():
|
||
left, right = target_pair(0.0, 0.0, 10.0)
|
||
stray = box(500.0, 300.0, cls_id=3, conf=0.8)
|
||
reports = one_report(StereoRanger(), [left, stray], [right], frame_sequence=42, timestamp_s=1.5)
|
||
assert reports[0].matched and reports[0].valid
|
||
u = reports[1]
|
||
assert not u.matched and not u.valid
|
||
assert u.forward_distance_m is None
|
||
assert u.centre_pixel_x_mid == stray.cx and u.centre_pixel_y_mid == stray.cy
|
||
assert u.confidence == 0.8 and u.class_id == 3
|
||
assert u.frame_sequence == 42 and u.timestamp_s == 1.5
|
||
|
||
|
||
def test_empty_sides():
|
||
ranger = StereoRanger()
|
||
assert ranger.update([], []) == []
|
||
r = one_report(ranger, [box(100.0, 100.0)], [])[0]
|
||
assert not r.matched
|
||
|
||
|
||
def test_report_order_follows_left_detections():
|
||
ranger = StereoRanger()
|
||
l1, r1 = target_pair(0.0, 0.0, 10.0, cls_id=0)
|
||
l2, r2 = target_pair(0.0, 0.0, 8.0, cls_id=2)
|
||
reports = ranger.update([l2, l1], [r1, r2]) # 输出顺序与输入左目一致
|
||
assert [r.class_id for r in reports] == [2, 0]
|
||
assert reports[0].confidence == min(l2.conf, r2.conf)
|
||
|
||
|
||
# ---- §5.4 有效性判定 ----
|
||
|
||
def test_beyond_range_tolerance_invalid_but_distance_kept():
|
||
left, right = target_pair(0.0, 0.0, 15.0) # δZ ≈ 3.9 m > 2 m
|
||
r = one_report(StereoRanger(), [left], [right])[0]
|
||
assert r.matched
|
||
assert not r.valid
|
||
assert r.forward_distance_m == pytest.approx(15.0, rel=1e-9)
|
||
assert r.lateral_m is not None # 方位/坐标仍发布（§5.4 域外标记无效）
|
||
assert r.error_est_m == pytest.approx(range_error_abs(15.0, CFG.fx_px, CFG.baseline_m, CFG.disp_noise_px))
|
||
|
||
|
||
def test_within_range_tolerance_valid():
|
||
left, right = target_pair(0.0, 0.0, 10.0) # δZ ≈ 1.74 m <= 2 m
|
||
r = one_report(StereoRanger(), [left], [right])[0]
|
||
assert r.valid
|
||
|
||
|
||
def test_invalid_when_disparity_below_dmin():
|
||
# 放宽 range_tol 以单独暴露 D_min 判据：D = 2.3 px < 3 px -> 无效
|
||
cfg = StereoConfig(range_tol_m=50.0)
|
||
left, right = target_pair(0.0, 0.0, 25.0)
|
||
r = one_report(StereoRanger(cfg), [left], [right])[0]
|
||
assert r.matched and not r.valid
|
||
|
||
|
||
# ---- §7 时间滤波（可选） ----
|
||
|
||
def moving_pair(z_m, track_id=1, cls_id=0):
|
||
"""返回 (左框列表, 右框列表)：目标沿光轴移动，track_id 可配。"""
|
||
left = box(CFG.cx_px, CFG.cy_px, cls_id=cls_id, track_id=track_id)
|
||
right = box(CFG.cx_px - FB / z_m, CFG.cy_px, cls_id=cls_id, track_id=track_id)
|
||
return [left], [right]
|
||
|
||
|
||
def test_filter_smooths_and_resets_on_jump():
|
||
cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3, filter_jump_ratio=0.2,
|
||
range_tol_m=50.0) # 放宽有效域，让 10~13 m 均有效以观察滤波
|
||
ranger = StereoRanger(cfg)
|
||
z1 = one_report(ranger, *moving_pair(10.0), frame_sequence=1)[0]
|
||
assert z1.forward_distance_m == pytest.approx(10.0, rel=1e-9) # 首帧置位
|
||
z2 = one_report(ranger, *moving_pair(11.0), frame_sequence=2)[0] # 跳变 10% <= 20%：平滑
|
||
assert z2.forward_distance_m == pytest.approx(0.7 * 10.0 + 0.3 * 11.0, rel=1e-9)
|
||
z3 = one_report(ranger, *moving_pair(13.0), frame_sequence=3)[0] # 跳变 26% > 20%：重置
|
||
assert z3.forward_distance_m == pytest.approx(13.0, rel=1e-9)
|
||
|
||
|
||
def test_filter_skipped_without_track_id():
|
||
cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3)
|
||
ranger = StereoRanger(cfg)
|
||
one_report(ranger, *moving_pair(10.0, track_id=0), frame_sequence=1)
|
||
z2 = one_report(ranger, *moving_pair(11.0, track_id=0), frame_sequence=2)[0]
|
||
assert z2.forward_distance_m == pytest.approx(11.0, rel=1e-9)
|
||
|
||
|
||
def test_reset_clears_filter_state():
|
||
cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3, range_tol_m=50.0)
|
||
ranger = StereoRanger(cfg)
|
||
one_report(ranger, *moving_pair(10.0), frame_sequence=1) # 状态 = 10.0
|
||
ranger.reset()
|
||
# 若不重置，10 -> 11 会平滑为 10.3；重置后应直接输出原始 11.0
|
||
z = one_report(ranger, *moving_pair(11.0), frame_sequence=2)[0]
|
||
assert z.forward_distance_m == pytest.approx(11.0, rel=1e-9)
|
||
|
||
|
||
def test_invalid_measurements_do_not_pollute_filter_state():
|
||
cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3, range_tol_m=50.0)
|
||
ranger = StereoRanger(cfg)
|
||
one_report(ranger, *moving_pair(10.0), frame_sequence=1)
|
||
z2 = one_report(ranger, *moving_pair(25.0), frame_sequence=2)[0] # D < D_min：无效
|
||
assert not z2.valid
|
||
z3 = one_report(ranger, *moving_pair(10.5), frame_sequence=3)[0]
|
||
# 无效帧未污染状态：仍按与首帧 10.0 平滑
|
||
assert z3.forward_distance_m == pytest.approx(0.7 * 10.0 + 0.3 * 10.5, rel=1e-9)
|