97 lines
4.0 KiB
Python
97 lines
4.0 KiB
Python
"""stereo_ranger demo：合成多目标场景演示单帧 update() 全流程。
|
||
|
||
本 demo 不使用任何检测器：按已知 (X, Y, Z) 目标解析投影生成左右目
|
||
检测框（模拟"外部检测器输出，框已在校正后原生坐标系"），并叠加 ±1 px
|
||
亚像素抖动，随后用 StereoRanger 测距并对照真值。
|
||
|
||
运行（pytorch 环境）：
|
||
python utils/demo_stereo_ranger.py [--config path/to/stereo.json]
|
||
|
||
依赖仅为标准库 + stereo_ranger 包（将 utils/ 加入 PYTHONPATH 或于 utils/ 下运行）。
|
||
"""
|
||
|
||
import argparse
|
||
import random
|
||
import sys
|
||
from pathlib import Path
|
||
|
||
sys.path.insert(0, str(Path(__file__).resolve().parent))
|
||
|
||
from stereo_ranger import Detection, StereoConfig, StereoRanger, focal_from_hfov, load_from_json, range_error_abs
|
||
|
||
# 模拟相机：1920x1080，HFOV 60°（文档 §5.3 示例之一）
|
||
IMAGE_W = 1920
|
||
IMAGE_H = 1080
|
||
HFOV_DEG = 60.0
|
||
BASELINE_M = 0.06
|
||
|
||
|
||
def project_to_boxes(x_m, y_m, z_m, fx, fy, cx, cy, b, rng, w_m=0.5, h_m=0.3):
|
||
"""目标物理宽高 -> 左右目框；叠加亚像素抖动模拟检测噪声（种子固定）。"""
|
||
w_px = fx * w_m / z_m
|
||
h_px = fy * h_m / z_m
|
||
u_l = fx * x_m / z_m + cx
|
||
v_l = fy * y_m / z_m + cy
|
||
u_r = u_l - fx * b / z_m
|
||
jitter = lambda: rng.uniform(-1.0, 1.0) # ±1 px 框边缘抖动
|
||
left = Detection(u_l - w_px / 2 + jitter(), v_l - h_px / 2 + jitter(),
|
||
u_l + w_px / 2 + jitter(), v_l + h_px / 2 + jitter(),
|
||
conf=0.95, cls_id=0, track_id=1)
|
||
right = Detection(u_r - w_px / 2 + jitter(), v_l - h_px / 2 + jitter(),
|
||
u_r + w_px / 2 + jitter(), v_l + h_px / 2 + jitter(),
|
||
conf=0.9, cls_id=0, track_id=1)
|
||
return left, right
|
||
|
||
|
||
def main() -> None:
|
||
parser = argparse.ArgumentParser(description=__doc__)
|
||
parser.add_argument("--config", default=None, help="StereoConfig JSON 文件（可选）")
|
||
args = parser.parse_args()
|
||
|
||
cfg = load_from_json(args.config) if args.config else StereoConfig(
|
||
fx_px=focal_from_hfov(IMAGE_W, HFOV_DEG),
|
||
fy_px=focal_from_hfov(IMAGE_W, HFOV_DEG),
|
||
cx_px=IMAGE_W / 2.0, cy_px=IMAGE_H / 2.0,
|
||
baseline_m=BASELINE_M,
|
||
range_tol_m=2.0,
|
||
)
|
||
ranger = StereoRanger(cfg)
|
||
rng = random.Random(20260909)
|
||
|
||
targets = [(0.0, 0.0, 5.0), (2.0, 0.5, 8.0), (-1.5, -0.4, 12.0)]
|
||
left_dets, right_dets = [], []
|
||
for x_m, y_m, z_m in targets:
|
||
l, r = project_to_boxes(x_m, y_m, z_m, cfg.fx_px, cfg.fy_px, cfg.cx_px, cfg.cy_px,
|
||
cfg.baseline_m, rng)
|
||
left_dets.append(l)
|
||
right_dets.append(r)
|
||
# 一个只在左目检出的干扰框 -> 应输出 matched=False 的方位记录
|
||
left_dets.append(Detection(100.0, 900.0, 160.0, 950.0, conf=0.6, cls_id=9))
|
||
|
||
reports = ranger.update(left_dets, right_dets, frame_sequence=1, timestamp_s=0.0)
|
||
|
||
print(f"内参: F={cfg.fx_px:.1f}px cx={cfg.cx_px:.0f} B={cfg.baseline_m:.3f}m "
|
||
f"Z 窗口 [{cfg.z_min_m}, {cfg.z_max_m}]m")
|
||
print(f"{'真值 Z':>8} {'估计 Z':>8} {'真值 X':>7} {'估计 X':>7} {'视差':>6} {'δZ估计':>7} "
|
||
f"{'有效':>5} {'x_mid':>7} 备注")
|
||
for i, r in enumerate(reports):
|
||
if not r.matched:
|
||
print(f"{'-':>8} {'-':>8} {'-':>7} {'-':>7} {'-':>6} {'-':>7} {'-':>5} "
|
||
f"{r.centre_pixel_x_mid:7.1f} 左目未配对（方位直发）")
|
||
continue
|
||
truth = targets[i]
|
||
print(f"{truth[2]:8.2f} {r.forward_distance_m:8.2f} {truth[0]:7.2f} {r.lateral_m:7.2f} "
|
||
f"{r.disparity_px:6.1f} {r.error_est_m:7.2f} {str(r.valid):>5} "
|
||
f"{r.centre_pixel_x_mid:7.1f} src={r.disparity_source}")
|
||
print(f"\n共 {len(reports)} 条记录（{sum(1 for r in reports if r.valid)} 条有效）")
|
||
|
||
print("\n误差模型示意（规格 §5.3，δD = 1 px）：")
|
||
print(f"{'Z (m)':>7} {'δZ (m)':>8}")
|
||
for z in [5, 10, 15, 20, 30]:
|
||
err = range_error_abs(z, cfg.fx_px, cfg.baseline_m, 1.0)
|
||
print(f"{z:7d} {err:8.2f}")
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|