精简仓库结构并增强任务链路：归档双目可选后端、新增LOS判据与自定义返航
归档 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。
This commit is contained in:
keita
Binary file not shown.
|
After Width: | Height: | Size: 821 KiB |
@@ -0,0 +1,19 @@
|
||||
{
|
||||
"fx_px": 960.0,
|
||||
"fy_px": 960.0,
|
||||
"cx_px": 960.0,
|
||||
"cy_px": 540.0,
|
||||
"baseline_m": 0.06,
|
||||
"tau_y_px": 2.0,
|
||||
"tau_h_ratio": 0.25,
|
||||
"disparity_min_px": 3.0,
|
||||
"disparity_max_px": 57.6,
|
||||
"max_delta_z_m": 2.0,
|
||||
"consistency_N_frames": 3,
|
||||
"consistency_M_frames": 5,
|
||||
"consistency_jump_px": 3.0,
|
||||
"smooth_alpha": 0.3,
|
||||
"jump_reset_m": 5.0,
|
||||
"filter_reset_frames": 5,
|
||||
"isolation_gate_px": 50.0
|
||||
}
|
||||
@@ -0,0 +1,9 @@
|
||||
{
|
||||
"model_path": "models/yolo26n-drone.pt",
|
||||
"imgsz": 640,
|
||||
"conf_threshold": 0.25,
|
||||
"iou_threshold": 0.7,
|
||||
"class_whitelist": null,
|
||||
"device": null,
|
||||
"concurrency": "parallel"
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
"""pytest 根配置（归档目录专用）。
|
||||
|
||||
归档的 175 条测试仍 import `stereo_yolo` / `stereo_ranger` / `target_tracker`
|
||||
（三者都在本目录的 utils/ 下），部分还复用运行链路的 `visual_track_control`
|
||||
（留在仓库根的 utils/）。因此这里把本目录的 utils/ 与仓库根的 utils/ 都加入
|
||||
sys.path，使 `cd legacy/utils && pytest` 可直接运行。
|
||||
|
||||
默认（从仓库根 utils/ 跑）不会收集本目录，主链路测试不受影响。
|
||||
"""
|
||||
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
_LEGACY_UTILS = Path(__file__).resolve().parent # legacy/utils
|
||||
_REPO_UTILS = _LEGACY_UTILS.parents[1] / "utils" # 仓库根/utils
|
||||
|
||||
for _p in (_LEGACY_UTILS, _REPO_UTILS):
|
||||
if str(_p) not in sys.path:
|
||||
sys.path.insert(0, str(_p))
|
||||
@@ -0,0 +1,269 @@
|
||||
"""demo_stereo_ranger.py —— 双目稀疏目标测距演示（规格 docs/双目测距技术路线.md）。
|
||||
|
||||
全部场景用**解析构造**的左右目检测框驱动：不需要相机、不需要图像、不需要
|
||||
飞控，几何是闭式可验证的（规格 §11.4）。覆盖：
|
||||
|
||||
① 逼近：三条有效性门依次生效（域外 → 预算门 → 有效）+ 时间滤波平滑
|
||||
② 远场域外：valid = false 但 c_mid（虚拟双目中心方位）照常发布
|
||||
③ 单目退化：仅一目检出 → 仅方位记录
|
||||
④ 非同帧拒收：左右帧标识不一致 → 整帧拒收 + 告警节流
|
||||
⑤ 一致性监控：持续视差跳变 → N 帧降置信度 → M 帧判无效
|
||||
⑥ 多目标：贪心一一配对，各自独立状态；未匹配右目退化为仅方位
|
||||
|
||||
用法：
|
||||
python utils/demo_stereo_ranger.py # 默认配置
|
||||
python utils/demo_stereo_ranger.py --config <path> # 指定配置
|
||||
|
||||
⚠ 所有参数均为占位值（焦距/标定待厂商数据）：标定与地面真值验证完成前，
|
||||
本模块输出仅限静态标靶/回放/台架验证，不得作为飞行控制决策依据
|
||||
（规格 §12.3 红线）。
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import logging
|
||||
from pathlib import Path
|
||||
from typing import List, Optional, Tuple
|
||||
|
||||
from stereo_ranger import (
|
||||
Detection,
|
||||
DetectionFrame,
|
||||
RangeRecord,
|
||||
StereoConfig,
|
||||
StereoRanger,
|
||||
load_from_json,
|
||||
)
|
||||
|
||||
logger = logging.getLogger("demo_stereo_ranger")
|
||||
|
||||
_DEMO_DIR = Path(__file__).resolve().parent
|
||||
_DEFAULT_CONFIG = _DEMO_DIR / "config" / "stereo_ranger.json"
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 合成框对
|
||||
|
||||
|
||||
def boxes_for(
|
||||
cfg: StereoConfig,
|
||||
x_center_m: float,
|
||||
y_m: float,
|
||||
z_m: float,
|
||||
*,
|
||||
w_m: float = 0.5,
|
||||
h_m: float = 0.4,
|
||||
class_id: int = 0,
|
||||
track_id: Optional[int] = None,
|
||||
confidence: float = 0.95,
|
||||
) -> Tuple[Detection, Detection]:
|
||||
"""模组中心系 (X_C, Y, Z) 的理想目标 → (左目框, 右目框)。
|
||||
|
||||
两目框同宽，因此边缘视差 = 中心视差 = F·B/Z（§3.2/§6.6 的精确投影）。
|
||||
"""
|
||||
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) -> 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)
|
||||
right = Detection(
|
||||
1000.0 - d_px - width / 2,
|
||||
y_center - height / 2,
|
||||
1000.0 - d_px + width / 2,
|
||||
y_center + height / 2,
|
||||
)
|
||||
return left, right
|
||||
|
||||
|
||||
def frame(frame_id: int, detections: List[Detection]) -> DetectionFrame:
|
||||
return DetectionFrame(frame_id=frame_id, detections=detections)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 打印
|
||||
|
||||
|
||||
def describe(record: RangeRecord) -> str:
|
||||
z_text = f"{record.forward_distance_m:7.3f}" if record.forward_distance_m is not None else " — "
|
||||
r_text = f"{record.slant_range_m:7.3f}" if record.slant_range_m is not None else " — "
|
||||
d_text = f"{record.disparity_px:7.2f}" if record.disparity_px is not None else " — "
|
||||
return (
|
||||
f"[F{record.frame_id:02d}] {record.degradation.name:<12} valid={int(record.valid)} "
|
||||
f"Z={z_text} m R={r_text} m D={d_text} px "
|
||||
f"c_mid=({record.center_mid_px[0]:7.1f},{record.center_mid_px[1]:6.1f}) "
|
||||
f"conf={record.confidence:.2f}"
|
||||
)
|
||||
|
||||
|
||||
def log_scene(title: str) -> None:
|
||||
logger.info("")
|
||||
logger.info("── %s", title)
|
||||
|
||||
|
||||
def log_config_summary(cfg: StereoConfig) -> None:
|
||||
logger.info("配置：B=%.3f m F=%.1f px (F·B=%.2f px·m)", cfg.baseline_m, cfg.fx_px, cfg.focal_baseline_px_m)
|
||||
logger.info(
|
||||
" 视差域 [%.1f, %.1f] px ⇔ 距离域 [%.2f, %.2f] m（视差域权威）",
|
||||
cfg.disparity_min_px,
|
||||
cfg.disparity_max_px,
|
||||
cfg.z_min_m,
|
||||
cfg.z_max_m,
|
||||
)
|
||||
logger.info(
|
||||
" 误差预算 δZ(δD=1px) ≤ %.1f m ⇒ 有效远界 %.2f m；一致性 N=%d / M=%d（跳变门限 %.1f px）",
|
||||
cfg.max_delta_z_m,
|
||||
cfg.z_error_budget_max_m,
|
||||
cfg.consistency_N_frames,
|
||||
cfg.consistency_M_frames,
|
||||
cfg.consistency_jump_px,
|
||||
)
|
||||
logger.info(
|
||||
" 滤波 α=%.2f（≈%.0f 帧群延迟）跳变重置 %.1f m；目标隔离门限 %.0f px",
|
||||
cfg.smooth_alpha,
|
||||
1.0 / cfg.smooth_alpha,
|
||||
cfg.jump_reset_m,
|
||||
cfg.isolation_gate_px,
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 场景
|
||||
|
||||
|
||||
def scene_approach(ranger: StereoRanger, cfg: StereoConfig) -> None:
|
||||
"""① 逼近：Z 20 → 6 m，三条有效性门依次生效，有效帧起低通平滑。"""
|
||||
ranger.reset() # 场景独立：清空跨场景的滤波/一致性/隔离状态
|
||||
log_scene("① 逼近场景：目标沿光轴 20 → 6 m（1 m/帧），观察三道门与滤波")
|
||||
logger.info(" %-7s %-9s %-6s %-7s %s", "真值Z", "D(px)", "valid", "输出Z", "说明")
|
||||
user_ms = 1000
|
||||
for index, z_true in enumerate([20.0, 18.0, 16.0, 14.0, 12.0, 11.0, 10.0, 9.0, 8.0, 7.0, 6.0], start=1):
|
||||
left, right = boxes_for(cfg, 0.0, 0.0, z_true)
|
||||
record = ranger.update(frame(index, [left]), frame(index, [right]), now_ms=user_ms)[0]
|
||||
user_ms += 50
|
||||
if record.disparity_px is not None and record.disparity_px < cfg.disparity_min_px:
|
||||
note = "门① 视差域外 → invalid"
|
||||
elif not record.valid and record.disparity_px is not None:
|
||||
note = "门② 误差预算不满足 → invalid"
|
||||
elif record.forward_distance_m is not None and abs(record.forward_distance_m - z_true) > 1e-9:
|
||||
note = f"有效（滤波后，真值 {z_true:.2f}）"
|
||||
else:
|
||||
note = "有效（滤波器初始化）"
|
||||
logger.info(
|
||||
" %-7.2f %-9.2f %-6d %-7s %s",
|
||||
z_true,
|
||||
record.disparity_px if record.disparity_px is not None else float("nan"),
|
||||
int(record.valid),
|
||||
f"{record.forward_distance_m:.3f}" if record.forward_distance_m is not None else "—",
|
||||
note,
|
||||
)
|
||||
logger.info(
|
||||
" → 远场：域外或 δZ 超预算时诚实输出 invalid（不强行估计）；"
|
||||
"有效帧之间才做一阶低通，输出滞后于真值属预期（消费侧 PID 与限幅吸收）"
|
||||
)
|
||||
|
||||
|
||||
def scene_far_field_bearing(ranger: StereoRanger, cfg: StereoConfig) -> None:
|
||||
"""② 远场域外：无距离但方位照常发布（远场定速接近策略的输入基础）。"""
|
||||
ranger.reset()
|
||||
log_scene("② 远场域外：Z=25 m（D < 下限），valid=false 但 c_mid 仍发布")
|
||||
x_center_m = 0.6
|
||||
left, right = boxes_for(cfg, x_center_m, 0.0, 25.0)
|
||||
record = ranger.update(frame(20, [left]), frame(20, [right]), now_ms=2000)[0]
|
||||
logger.info(" %s", describe(record))
|
||||
logger.info(
|
||||
" c_mid.x=%.1f = cx + F·X_C/Z = %.1f（虚拟双目中心，无近场视差偏差）",
|
||||
record.center_mid_px[0],
|
||||
cfg.cx_px + cfg.fx_px * x_center_m / 25.0,
|
||||
)
|
||||
logger.info(" → 消费侧据此走\"方位闭环 + 定速接近\"（D4），而不是使用不可靠的远场估计")
|
||||
|
||||
|
||||
def scene_monocular_degradation(ranger: StereoRanger, cfg: StereoConfig) -> None:
|
||||
"""③ 单目退化：仅一目检出 → 仅方位记录（v1.0 接受近场视差偏差）。"""
|
||||
ranger.reset()
|
||||
log_scene("③ 单目退化：仅左目检出")
|
||||
left, _ = boxes_for(cfg, 0.3, 0.0, 4.0, confidence=0.88)
|
||||
records = ranger.update(frame(21, [left]), frame(21, []), now_ms=3000)
|
||||
logger.info(" %s", describe(records[0]))
|
||||
logger.info(" → 退化 = 仅方位：报该目框中心作方位，valid=false 无距离（§2.2 记录两态）")
|
||||
|
||||
|
||||
def scene_frame_mismatch(ranger: StereoRanger) -> None:
|
||||
"""④ 非同帧拒收：左右帧标识不一致 → 整帧拒收（WARN 节流）。"""
|
||||
ranger.reset()
|
||||
log_scene("④ 同帧契约：左右帧标识不一致（F30 vs F31）")
|
||||
left_pair = pair_with_disparity(9.6)
|
||||
for _ in range(2):
|
||||
records = ranger.update(frame(30, [left_pair[0]]), frame(31, [left_pair[1]]))
|
||||
logger.info(" 两次违约共输出记录 %d 条（应为 0）；告警按节流规范只在第 1 次打印", len(records))
|
||||
logger.info(" → 不同曝光时刻的框配对解算没有物理意义，宁可拒收不可给值（§2.1）")
|
||||
|
||||
|
||||
def scene_consistency(ranger: StereoRanger) -> None:
|
||||
"""⑤ 一致性监控：持续视差跳变 → N 帧降置信度 → M 帧判无效。"""
|
||||
ranger.reset()
|
||||
log_scene("⑤ 一致性监控：视差逐帧跳变 4 px（门限 3 px）")
|
||||
for index, d_px in enumerate([9.5, 13.5, 17.5, 21.5, 25.5, 29.5], start=40):
|
||||
left, right = pair_with_disparity(d_px)
|
||||
record = ranger.update(frame(index, [left]), frame(index, [right]), now_ms=4000)[0]
|
||||
logger.info(" %s", describe(record))
|
||||
logger.info(" → 只能降级、不能纠偏：连续异常帧数达 N 降置信度、达 M 判无效（§6.7）")
|
||||
|
||||
|
||||
def scene_multi_target(ranger: StereoRanger, cfg: StereoConfig) -> None:
|
||||
"""⑥ 多目标：贪心一一配对；未配对的右目框退化为仅方位记录。"""
|
||||
ranger.reset()
|
||||
log_scene("⑥ 多目标：两个同类别目标 + 一个孤立右目框")
|
||||
left_a, right_a = boxes_for(cfg, -1.0, 0.0, 8.0)
|
||||
left_b, right_b = boxes_for(cfg, 1.2, 0.0, 9.0)
|
||||
extra_right = Detection(360.0, 480.0, 420.0, 540.0)
|
||||
records = ranger.update(frame(50, [left_a, left_b]), frame(50, [right_a, right_b, extra_right]), now_ms=5000)
|
||||
for record in records:
|
||||
logger.info(" %s", describe(record))
|
||||
logger.info(" → 目标数极少（1~2）时 O(n²) 贪心与全局最优几乎必然同解；匹配器接口可替换（§10.3）")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 入口
|
||||
|
||||
|
||||
def run_demo(config_path: str) -> None:
|
||||
cfg = load_from_json(config_path)
|
||||
logger.info("配置来源：%s", config_path)
|
||||
log_config_summary(cfg)
|
||||
logger.warning("⚠ 占位参数未标定：仅限静态标靶/回放/台架验证，不得作为飞行控制决策依据（§12.3）")
|
||||
|
||||
ranger = StereoRanger(cfg) # 演示共用一个实例，状态跨场景延续（逼近 → 后续场景）
|
||||
scene_approach(ranger, cfg)
|
||||
scene_far_field_bearing(ranger, cfg)
|
||||
scene_monocular_degradation(ranger, cfg)
|
||||
scene_frame_mismatch(ranger)
|
||||
scene_consistency(ranger)
|
||||
scene_multi_target(ranger, cfg)
|
||||
logger.info("")
|
||||
logger.info("演示结束：以上全部为合成框对的解析结果，无真实图像与硬件参与")
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description="双目稀疏目标测距演示（合成框对，离线）")
|
||||
parser.add_argument("--config", default=str(_DEFAULT_CONFIG), help="配置 JSON 路径")
|
||||
args = parser.parse_args()
|
||||
logging.basicConfig(level=logging.INFO, format="%(levelname)s %(message)s")
|
||||
run_demo(args.config)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,286 @@
|
||||
"""demo_stereo_yolo.py —— 双目 YOLO 检测前端演示（规格 docs/双目YOLO目标识别需求说明.md）。
|
||||
|
||||
用真实模型（models/yolo26n-drone.pt）与一张占位素材（utils/assets/stereo_yolo/
|
||||
drone_sky.png，AI 合成图、非真实飞行数据）驱动，覆盖：
|
||||
|
||||
① 单图检测冒烟：真实推理 → 原生坐标框 + 出口自检（§2.2）
|
||||
② 合成双目对（已知视差）：同一无人机 patch 按已知视差贴到左右图 →
|
||||
真检测框喂 StereoRanger.update() → 与解析真值 Z=F·B/D 对比（§6 下游对接）
|
||||
③ 空场景：无目标 → 两目空帧 → 测距端返回空列表（正常空帧，不告警）
|
||||
④ 形状不一致：左右图分辨率不同 → 整帧拒收输出空（§2.2 第 2 条）
|
||||
⑤ 帧周期：双流满负荷计时（concurrency=parallel，§5 并发要求）
|
||||
|
||||
用法（从仓库根运行）：
|
||||
python utils/demo_stereo_yolo.py
|
||||
python utils/demo_stereo_yolo.py --frames 20
|
||||
|
||||
⚠ 占位参数 + 合成素材：仅限台架/回放验证，不得接入实飞控制回路（§7 红线）。
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import logging
|
||||
import time
|
||||
from dataclasses import replace
|
||||
from pathlib import Path
|
||||
from typing import List, Optional, Tuple
|
||||
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
from stereo_ranger import (
|
||||
Detection,
|
||||
DetectionFrame,
|
||||
RangeRecord,
|
||||
StereoConfig,
|
||||
StereoRanger,
|
||||
load_from_json as load_ranger_config,
|
||||
)
|
||||
from stereo_yolo import StereoYoloConfig, StereoYoloFrontend, load_from_json
|
||||
|
||||
logger = logging.getLogger("demo_stereo_yolo")
|
||||
|
||||
_DEMO_DIR = Path(__file__).resolve().parent # utils/
|
||||
_REPO_ROOT = _DEMO_DIR.parent
|
||||
_DEFAULT_CONFIG = _DEMO_DIR / "config" / "stereo_yolo.json"
|
||||
_RANGER_CONFIG = _DEMO_DIR / "config" / "stereo_ranger.json"
|
||||
_ASSET = _DEMO_DIR / "assets" / "stereo_yolo" / "drone_sky.png"
|
||||
|
||||
# 合成双目对参数：粘贴同一 patch 的整数水平偏移 = 精确视差
|
||||
_DISPARITY_PX = 12 # ⇔ Z ≈ 4.8 m（F·B=57.6 px·m，域内）
|
||||
_LEFT_CENTER_X = 1200
|
||||
_PATCH_Y_CENTER = 560
|
||||
_CANVAS = (1920, 1080)
|
||||
_FEATHER_PX = 32 # 羽化混合宽度：硬边接缝会被检测器误当目标边缘（实测数 px 框偏移）
|
||||
_RANGER_NOW_MS = 1000
|
||||
|
||||
|
||||
def log_scene(title: str) -> None:
|
||||
logger.info("")
|
||||
logger.info("── %s", title)
|
||||
|
||||
|
||||
def describe(record: RangeRecord) -> str:
|
||||
z_text = f"{record.forward_distance_m:7.3f}" if record.forward_distance_m is not None else " — "
|
||||
d_text = f"{record.disparity_px:7.2f}" if record.disparity_px is not None else " — "
|
||||
return (
|
||||
f"[F{record.frame_id:02d}] {record.degradation.name:<12} valid={int(record.valid)} "
|
||||
f"Z={z_text} m D={d_text} px "
|
||||
f"c_mid=({record.center_mid_px[0]:7.1f},{record.center_mid_px[1]:6.1f}) "
|
||||
f"conf={record.confidence:.2f}"
|
||||
)
|
||||
|
||||
|
||||
def log_frame(eye: str, frame: DetectionFrame) -> None:
|
||||
if not frame.detections:
|
||||
logger.info(" %s：无检出", eye)
|
||||
return
|
||||
for detection in frame.detections:
|
||||
logger.info(
|
||||
" %s：框=(%7.1f,%7.1f,%7.1f,%7.1f) conf=%.3f class=%d",
|
||||
eye,
|
||||
detection.x1,
|
||||
detection.y1,
|
||||
detection.x2,
|
||||
detection.y2,
|
||||
detection.confidence,
|
||||
detection.class_id,
|
||||
)
|
||||
|
||||
|
||||
def _best_box(frame: DetectionFrame) -> Optional[Detection]:
|
||||
if not frame.detections:
|
||||
return None
|
||||
return max(frame.detections, key=lambda detection: detection.confidence)
|
||||
|
||||
|
||||
def _crop_patch(asset: np.ndarray, box: Detection, *, margin_x: float = 0.3, margin_y: float = 0.5) -> np.ndarray:
|
||||
width, height = box.width, box.height
|
||||
x1 = max(0, int(box.x1 - margin_x * width))
|
||||
x2 = min(asset.shape[1], int(box.x2 + margin_x * width))
|
||||
y1 = max(0, int(box.y1 - margin_y * height))
|
||||
y2 = min(asset.shape[0], int(box.y2 + margin_y * height))
|
||||
return asset[y1:y2, x1:x2].copy()
|
||||
|
||||
|
||||
def _sky_canvas(asset: np.ndarray) -> np.ndarray:
|
||||
"""用素材顶部天空条拉伸成画布，避免 patch 与背景出现色差硬边。"""
|
||||
strip = asset[0 : max(8, asset.shape[0] // 12), :, :]
|
||||
return cv2.resize(strip, (_CANVAS[0], _CANVAS[1]))
|
||||
|
||||
|
||||
def _paste_blended(canvas: np.ndarray, patch: np.ndarray, center_x: int, center_y: int) -> None:
|
||||
"""羽化混合粘贴：硬边接缝会被检测器当作目标边缘，造成数 px 的框偏移。"""
|
||||
height, width = patch.shape[:2]
|
||||
alpha = np.zeros((height, width), dtype=np.float32)
|
||||
alpha[_FEATHER_PX : height - _FEATHER_PX, _FEATHER_PX : width - _FEATHER_PX] = 1.0
|
||||
alpha = cv2.GaussianBlur(alpha, (0, 0), _FEATHER_PX * 0.5)[..., None]
|
||||
x0, y0 = int(center_x - width / 2), int(center_y - height / 2)
|
||||
region = canvas[y0 : y0 + height, x0 : x0 + width]
|
||||
canvas[y0 : y0 + height, x0 : x0 + width] = (patch * alpha + region * (1.0 - alpha)).astype(np.uint8)
|
||||
|
||||
|
||||
def build_synthetic_pair(frontend: StereoYoloFrontend, asset: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
|
||||
"""素材上检出无人机 → 取 patch → 同一 patch 按已知整数视差贴到左右天空画布。"""
|
||||
frame, _ = frontend.detect(asset, asset, frame_id=0)
|
||||
box = _best_box(frame)
|
||||
if box is None:
|
||||
raise RuntimeError("占位素材未检出无人机，请更换 utils/assets/stereo_yolo/drone_sky.png")
|
||||
patch = _crop_patch(asset, box)
|
||||
left_img, right_img = _sky_canvas(asset), _sky_canvas(asset)
|
||||
_paste_blended(left_img, patch, _LEFT_CENTER_X, _PATCH_Y_CENTER)
|
||||
_paste_blended(right_img, patch, _LEFT_CENTER_X - _DISPARITY_PX, _PATCH_Y_CENTER)
|
||||
return left_img, right_img
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 场景
|
||||
|
||||
|
||||
def scene_single_image(frontend: StereoYoloFrontend, asset: np.ndarray) -> None:
|
||||
log_scene("① 单图检测冒烟：真实模型 + 占位素材（同一图喂左右目，仅看输出契约）")
|
||||
left_frame, right_frame = frontend.detect(asset, asset, frame_id=1)
|
||||
log_frame("左目", left_frame)
|
||||
log_frame("右目", right_frame)
|
||||
logger.info(
|
||||
" → 框位于输入图原生坐标系（%d×%d）；出口自检/白名单已在识别端完成（§2.2 第 6 条）",
|
||||
asset.shape[1],
|
||||
asset.shape[0],
|
||||
)
|
||||
|
||||
|
||||
def scene_synthetic_stereo(
|
||||
frontend: StereoYoloFrontend,
|
||||
ranger: StereoRanger,
|
||||
ranger_cfg: StereoConfig,
|
||||
asset: np.ndarray,
|
||||
) -> Tuple[np.ndarray, np.ndarray]:
|
||||
log_scene(f"② 合成双目对（粘贴真值视差 {_DISPARITY_PX} px）→ StereoRanger.update()")
|
||||
left_img, right_img = build_synthetic_pair(frontend, asset)
|
||||
left_frame, right_frame = frontend.detect(left_img, right_img, frame_id=2)
|
||||
log_frame("左目", left_frame)
|
||||
log_frame("右目", right_frame)
|
||||
|
||||
left_box, right_box = _best_box(left_frame), _best_box(right_frame)
|
||||
if left_box is not None and right_box is not None:
|
||||
disparity = left_box.x1 - right_box.x1
|
||||
vertical_residual = abs(left_box.y1 - right_box.y1)
|
||||
logger.info(
|
||||
" 实测边缘视差 D=x1_L−x1_R=%.2f px（真值 %d px）；|Δy|=%.2f px",
|
||||
disparity,
|
||||
_DISPARITY_PX,
|
||||
vertical_residual,
|
||||
)
|
||||
logger.info(
|
||||
" 解析对照 Z=F·B/D=%.3f m（F·B=%.2f px·m）",
|
||||
ranger_cfg.focal_baseline_px_m / disparity,
|
||||
ranger_cfg.focal_baseline_px_m,
|
||||
)
|
||||
|
||||
records = ranger.update(left_frame, right_frame, now_ms=_RANGER_NOW_MS)
|
||||
for record in records:
|
||||
logger.info(" %s", describe(record))
|
||||
if left_box is not None and right_box is not None:
|
||||
logger.info(
|
||||
" → 误差来源：检测框边缘定位抖动（本合成对 |ΔD|=%.2f px），小视差下被 Z=F·B/D 放大（§7.1）",
|
||||
abs(disparity - _DISPARITY_PX),
|
||||
)
|
||||
logger.info(" → 与真值投影法（demo_stereo_ranger）同量级；真值验证待真实双目素材回填（§6）")
|
||||
return left_img, right_img
|
||||
|
||||
|
||||
def scene_empty(frontend: StereoYoloFrontend, ranger: StereoRanger, sky: np.ndarray) -> None:
|
||||
log_scene("③ 空场景：无目标 → 两目空帧 → 测距端空列表（正常空帧，不告警）")
|
||||
left_frame, right_frame = frontend.detect(sky, sky, frame_id=3)
|
||||
records = ranger.update(left_frame, right_frame, now_ms=2000)
|
||||
logger.info(
|
||||
" 两目检出数：%d / %d；测距记录数：%d（应为 0）",
|
||||
len(left_frame.detections),
|
||||
len(right_frame.detections),
|
||||
len(records),
|
||||
)
|
||||
logger.info(" → 无检测即无输出；\"目标丢失就该没有距离\"（§2.5 第 3 条）")
|
||||
|
||||
|
||||
def scene_shape_mismatch(frontend: StereoYoloFrontend, sky: np.ndarray) -> None:
|
||||
log_scene("④ 形状不一致拒收：左 1920×1080 vs 右 1280×720")
|
||||
resized = cv2.resize(sky, (1280, 720))
|
||||
left_frame, right_frame = frontend.detect(sky, resized, frame_id=4)
|
||||
logger.info(" 左右检出数：%d / %d（应为 0 / 0，WARN 节流打印）", len(left_frame.detections), len(right_frame.detections))
|
||||
logger.info(" → 禁止左右各自缩放/填充，否则破坏校正后的极线对齐（§2.2 第 2 条）")
|
||||
|
||||
|
||||
def scene_timing(frontend: StereoYoloFrontend, left_img: np.ndarray, right_img: np.ndarray, frames: int) -> None:
|
||||
log_scene(f"⑤ 帧周期：双流满负荷 {frames} 帧（concurrency=parallel）")
|
||||
for index in range(2): # 预热
|
||||
frontend.detect(left_img, right_img, frame_id=100 + index)
|
||||
started = time.perf_counter()
|
||||
for index in range(frames):
|
||||
frontend.detect(left_img, right_img, frame_id=200 + index)
|
||||
per_frame_ms = (time.perf_counter() - started) / frames * 1000.0
|
||||
logger.info(" 平均 %.1f ms/帧（%.1f fps）；30 fps 预算 33.3 ms/帧（§5）", per_frame_ms, 1000.0 / per_frame_ms)
|
||||
logger.info(" → 本值为台架开发机实测（device 配置 None = 自动选择）；平台算力预算另见平台工程文档")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 入口
|
||||
|
||||
|
||||
def _rebase_model_path(cfg: StereoYoloConfig) -> StereoYoloConfig:
|
||||
"""demo 便利：相对 model_path 按仓库根解析（库内规则是启动工作目录）。"""
|
||||
path = Path(cfg.model_path).expanduser()
|
||||
if not path.is_absolute() and not path.is_file():
|
||||
candidate = _REPO_ROOT / path
|
||||
if candidate.is_file():
|
||||
return replace(cfg, model_path=str(candidate))
|
||||
return cfg
|
||||
|
||||
|
||||
def run_demo(config_path: str, frames: int) -> None:
|
||||
cfg = _rebase_model_path(load_from_json(config_path))
|
||||
ranger_cfg = load_ranger_config(str(_RANGER_CONFIG))
|
||||
logger.info("配置来源：%s", config_path)
|
||||
logger.info(
|
||||
"检测前端：model=%s imgsz=%d conf=%.2f iou=%.2f whitelist=%s device=%s concurrency=%s",
|
||||
Path(cfg.model_path).name,
|
||||
cfg.imgsz,
|
||||
cfg.conf_threshold,
|
||||
cfg.iou_threshold,
|
||||
cfg.class_whitelist,
|
||||
cfg.device,
|
||||
cfg.concurrency,
|
||||
)
|
||||
logger.info(
|
||||
"测距几何（唯一来源 utils/config/stereo_ranger.json）：F=%.1f px B=%.3f m F·B=%.2f px·m",
|
||||
ranger_cfg.fx_px,
|
||||
ranger_cfg.baseline_m,
|
||||
ranger_cfg.focal_baseline_px_m,
|
||||
)
|
||||
logger.warning("⚠ 占位参数与合成素材：仅限台架/回放验证，不得接入实飞控制回路（§7 红线同源）")
|
||||
|
||||
asset = cv2.imread(str(_ASSET))
|
||||
if asset is None:
|
||||
raise FileNotFoundError(f"占位素材缺失: {_ASSET}")
|
||||
|
||||
frontend = StereoYoloFrontend(cfg)
|
||||
ranger = StereoRanger(ranger_cfg)
|
||||
try:
|
||||
scene_single_image(frontend, asset)
|
||||
left_img, right_img = scene_synthetic_stereo(frontend, ranger, ranger_cfg, asset)
|
||||
scene_empty(frontend, ranger, _sky_canvas(asset))
|
||||
scene_shape_mismatch(frontend, _sky_canvas(asset))
|
||||
scene_timing(frontend, left_img, right_img, frames)
|
||||
finally:
|
||||
frontend.close()
|
||||
logger.info("")
|
||||
logger.info("演示结束：素材为 AI 生成合成图（替换 utils/assets/stereo_yolo/drone_sky.png 可换真实素材）")
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description="双目 YOLO 检测前端演示（真实模型 + 占位素材）")
|
||||
parser.add_argument("--config", default=str(_DEFAULT_CONFIG), help="检测前端配置 JSON 路径")
|
||||
parser.add_argument("--frames", type=int, default=10, help="帧周期场景的测量帧数")
|
||||
args = parser.parse_args()
|
||||
logging.basicConfig(level=logging.INFO, format="%(levelname)s %(message)s")
|
||||
run_demo(args.config, args.frames)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,207 @@
|
||||
"""demo_track_control.py —— 视觉跟踪控制链路集成演示（胶水层）。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/visual_track_control/examples/track_control_demo.cpp。
|
||||
|
||||
串联三个组件（target_tracker 与 visual_track_control 互不 import，
|
||||
本脚本是唯一的转换点）：
|
||||
模拟检测序列 → TargetTracker（8 维恒速卡尔曼多目标追踪）
|
||||
→ TrackResult 转 TrackState（解耦边界）
|
||||
→ TrackingControlLaw（三通道控制律 + 限幅 + 状态机）
|
||||
→ CommandSender（mock 记录 / MAVSDK offboard 实发）
|
||||
|
||||
场景时间线（20 Hz，--speedup 可加速）：
|
||||
0~ 6s 正常跟踪：目标匀速横移 + 双目距离 30→10 m 前向逼近
|
||||
6~10s 目标消失：追踪器 Coast 预测（60 帧 = 3 s 后轨道判丢，
|
||||
控制律自动切换零速刹车）
|
||||
10~13s 持续判丢：维持零速刹车
|
||||
|
||||
用法：
|
||||
python utils/demo_track_control.py # mock 传输
|
||||
python utils/demo_track_control.py --transport mavsdk # MAVSDK（需飞控/SITL）
|
||||
python utils/demo_track_control.py --speedup 4 # 4 倍速
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import asyncio
|
||||
import logging
|
||||
from pathlib import Path
|
||||
|
||||
from target_tracker.target_tracker import Detection, TargetTracker, TrackerConfig
|
||||
from visual_track_control.config import load_from_json
|
||||
from visual_track_control.mavsdk_gateway import MavsdkCommandGateway
|
||||
from visual_track_control.tracking_control_law import TrackingControlLaw
|
||||
from visual_track_control.types import (ControlMode, ControlSnapshot,
|
||||
ControlOutput, TrackState)
|
||||
|
||||
logger = logging.getLogger("demo")
|
||||
|
||||
_DEMO_DIR = Path(__file__).resolve().parent
|
||||
_DEFAULT_CONFIG = _DEMO_DIR / "config" / "track_control.json"
|
||||
|
||||
# 模拟场景参数
|
||||
_NORMAL_SECS = 6.0 # 正常跟踪阶段
|
||||
_COAST_SECS = 4.0 # 目标消失 → Coast（含轨道判亡）
|
||||
_LOST_SECS = 3.0 # 判丢后持续刹车阶段
|
||||
_DEMO_MAX_LOST_FRAMES = 60 # 追踪器 Coast 窗口（20 Hz 下 = 3 s）
|
||||
_STATUS_EVERY_TICKS = 10 # 每 0.5 s 打印一行状态
|
||||
|
||||
|
||||
def track_result_to_state(result) -> TrackState:
|
||||
"""两包之间的唯一转换点：TrackResult（box 为对象）→ TrackState（plain tuple）。"""
|
||||
box = result.box
|
||||
raw = result.raw
|
||||
return TrackState(
|
||||
tracked=result.tracked,
|
||||
primary_id=result.primary_id,
|
||||
center=result.center,
|
||||
box=(box.x1, box.y1, box.x2, box.y2) if box is not None else None,
|
||||
confidence=result.confidence,
|
||||
is_predicted=result.is_predicted,
|
||||
lost_frames=result.lost_frames,
|
||||
n_active=result.n_active,
|
||||
raw=(raw.x1, raw.y1, raw.x2, raw.y2) if raw is not None else None,
|
||||
)
|
||||
|
||||
|
||||
def simulate_detections(tick: int, normal_ticks: int, period_s: float):
|
||||
"""场景检测源：正常阶段目标匀速横移，之后消失。"""
|
||||
if tick >= normal_ticks:
|
||||
return []
|
||||
cx = 960.0 + 3.0 * tick # 3 px/帧 匀速右移
|
||||
cy = 540.0
|
||||
w, h = 120.0, 160.0
|
||||
return [Detection(cx - w / 2.0, cy - h / 2.0, cx + w / 2.0, cy + h / 2.0, 0.9)]
|
||||
|
||||
|
||||
def simulate_distance(tick: int, normal_ticks: int, period_s: float):
|
||||
"""场景双目距离源：正常阶段有效、距离 30→10 m；之后无效。"""
|
||||
if tick >= normal_ticks:
|
||||
return 0.0, False
|
||||
distance = max(10.0, 30.0 - tick * period_s * 3.0)
|
||||
return distance, True
|
||||
|
||||
|
||||
class MockCommandSender:
|
||||
"""CommandSender 的 mock 实现：记录最近一条指令描述（无实机验证能力）。"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
self.last_desc = "-"
|
||||
|
||||
async def start(self) -> bool:
|
||||
logger.info("[MockTransport] offboard 启动（模拟）")
|
||||
return True
|
||||
|
||||
async def send_velocity_heading(self, output: ControlOutput,
|
||||
time_boot_ms=None) -> bool:
|
||||
self.last_desc = (f"vx={output.vx_mps:+.2f} vy={output.vy_mps:+.2f} "
|
||||
f"vz={output.vz_mps:+.2f} "
|
||||
f"yaw_rate={output.yaw_rate_dps:+.2f}°/s "
|
||||
f"yaw={output.yaw_deg:+.2f}°")
|
||||
return True
|
||||
|
||||
async def send_brake_hover(self, time_boot_ms=None) -> bool:
|
||||
self.last_desc = "零速刹车（全零机体速度设定点）"
|
||||
return True
|
||||
|
||||
async def stop(self) -> None:
|
||||
logger.info("[MockTransport] offboard 停止（模拟）")
|
||||
|
||||
|
||||
async def make_sender(args) -> object:
|
||||
if args.transport == "mock":
|
||||
return MockCommandSender()
|
||||
from mavsdk import System
|
||||
drone = System()
|
||||
logger.info("连接飞控 %s ...", args.conn)
|
||||
await drone.connect(system_address=args.conn)
|
||||
async for state in drone.core.connection_state():
|
||||
if state.is_connected:
|
||||
logger.info("飞控已连接")
|
||||
break
|
||||
return MavsdkCommandGateway(drone)
|
||||
|
||||
|
||||
async def dispatch(sender, out: ControlOutput, now_ms: int):
|
||||
"""按模式分发指令；DEGRADED_HOLD 不发送（禁止正常控制）。"""
|
||||
if out.mode is ControlMode.VELOCITY_HEADING and out.valid:
|
||||
return await sender.send_velocity_heading(out, now_ms)
|
||||
if out.mode is ControlMode.BRAKE_HOVER and out.valid:
|
||||
return await sender.send_brake_hover(now_ms)
|
||||
return None
|
||||
|
||||
|
||||
async def run_demo(args) -> None:
|
||||
cfg = load_from_json(args.config)
|
||||
period_s = 1.0 / cfg.control.frequency_hz
|
||||
period_ms = int(round(period_s * 1000.0))
|
||||
|
||||
tracker = TargetTracker(TrackerConfig(max_lost_frames=_DEMO_MAX_LOST_FRAMES))
|
||||
law = TrackingControlLaw(cfg)
|
||||
sender = await make_sender(args)
|
||||
|
||||
normal_ticks = int(round(_NORMAL_SECS / period_s))
|
||||
coast_ticks = int(round(_COAST_SECS / period_s))
|
||||
lost_ticks = int(round(_LOST_SECS / period_s))
|
||||
total_ticks = normal_ticks + coast_ticks + lost_ticks
|
||||
|
||||
if not await sender.start():
|
||||
logger.error("offboard 启动失败，退出")
|
||||
return
|
||||
|
||||
logger.info("开始 %d 拍（%.1f s，%.1f Hz）：正常 %.1fs → Coast %.1fs → 判丢 %.1fs",
|
||||
total_ticks, total_ticks * period_s, cfg.control.frequency_hz,
|
||||
_NORMAL_SECS, _COAST_SECS, _LOST_SECS)
|
||||
try:
|
||||
for tick in range(total_ticks):
|
||||
now_ms = 1000 + tick * period_ms
|
||||
detections = simulate_detections(tick, normal_ticks, period_s)
|
||||
distance_m, distance_valid = simulate_distance(tick, normal_ticks, period_s)
|
||||
|
||||
result = tracker.update(detections)
|
||||
snap = ControlSnapshot(
|
||||
track=track_result_to_state(result),
|
||||
track_time_ms=now_ms, # 每拍取样即打戳（视觉始终新鲜）
|
||||
target_distance_m=distance_m,
|
||||
distance_valid=distance_valid,
|
||||
distance_time_ms=now_ms,
|
||||
attitude_present=True,
|
||||
attitude_time_ms=now_ms, # 模拟姿态源持续可用
|
||||
yaw_rad=0.0, # 机头朝北（NED 旋转恒等）
|
||||
)
|
||||
out = law.update(snap, now_ms)
|
||||
await dispatch(sender, out, now_ms)
|
||||
|
||||
if tick % _STATUS_EVERY_TICKS == 0:
|
||||
logger.info(
|
||||
"t=%5.2fs 检测=%d 追踪[trk=%d pred=%d lost=%d] | "
|
||||
"[%s] vx=%+.2f vy=%+.2f vz=%+.2f yaw_rate=%+.2f coast=%d | 发送: %s",
|
||||
(tick * period_s), len(detections), result.tracked,
|
||||
result.is_predicted, result.lost_frames, out.mode.name,
|
||||
out.vx_mps, out.vy_mps, out.vz_mps, out.yaw_rate_dps,
|
||||
out.coast_active, sender.last_desc)
|
||||
await asyncio.sleep(period_s / args.speedup)
|
||||
finally:
|
||||
await sender.stop()
|
||||
logger.info("演示结束")
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description="视觉跟踪控制链路集成演示")
|
||||
parser.add_argument("--transport", choices=["mock", "mavsdk"], default="mock",
|
||||
help="指令发送方式：mock=本地记录，mavsdk=offboard 实发")
|
||||
parser.add_argument("--conn", default="udp://:14540",
|
||||
help="MAVSDK 连接串（仅 --transport mavsdk 生效）")
|
||||
parser.add_argument("--speedup", type=float, default=1.0,
|
||||
help="时间倍速（>1 加速演示）")
|
||||
parser.add_argument("--config", default=str(_DEFAULT_CONFIG),
|
||||
help="控制律配置文件路径")
|
||||
args = parser.parse_args()
|
||||
|
||||
logging.basicConfig(level=logging.INFO,
|
||||
format="%(asctime)s.%(msecs)03d %(levelname)s %(message)s",
|
||||
datefmt="%H:%M:%S")
|
||||
asyncio.run(run_demo(args))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,6 @@
|
||||
[pytest]
|
||||
testpaths = tests
|
||||
python_files = *_test.py test_*.py
|
||||
; 与运行链路 utils/pytest.ini 同口径：各包存在同名测试文件（如 config_test.py），
|
||||
; importlib 模式避免按裸模块名导入时的 basename 冲突。
|
||||
addopts = --import-mode=importlib
|
||||
@@ -0,0 +1,112 @@
|
||||
# stereo_ranger（Python 版）
|
||||
|
||||
双目稀疏目标测距：对**同一时刻**左右两目已立体校正图像上的目标检测框做立体
|
||||
几何解算，输出少量目标的**前向距离 Z + 斜距 R（参考）+ 虚拟双目中心方位
|
||||
c_mid + 有效性/置信度**。不做目标识别、图像采集/立体校正、稠密深度。
|
||||
|
||||
规格：`docs/双目测距技术路线.md`（算法与语义权威）。
|
||||
|
||||
⚠ **全部参数为占位值**（焦距/标定待厂商数据）：标定与地面真值验证完成前，
|
||||
本模块输出**仅限静态标靶/回放/台架验证，不得作为飞行控制决策依据**（§12.3）。
|
||||
|
||||
## 接口清单
|
||||
|
||||
| 名称 | 说明 |
|
||||
| --- | --- |
|
||||
| `StereoRanger(config=None, matcher=None)` | 主入口；`matcher` 可注入自定义匹配器 |
|
||||
| `.update(left, right, now_ms=0) -> List[RangeRecord]` | 单周期处理（§5 五步），纯计算、无 IO |
|
||||
| `.reset()` | 清空全部运行期状态（滤波/一致性/隔离/节流计数） |
|
||||
| `.config` / `.matcher` | 只读访问（配置与匹配器） |
|
||||
| `StereoConfig` / `load_from_json(path)` | 17 项配置；缺项/未知字段/类型错误/非法取值一律 `ValueError`（fail-fast） |
|
||||
| `.focal_baseline_px_m` / `.z_min_m` / `.z_max_m` / `.z_error_budget_max_m` | 派生只读量（F·B 常量、视差域互推的距离域、误差预算远界） |
|
||||
| `Detection(x1,y1,x2,y2, confidence, class_id, track_id)` | 单框（校正后坐标系；含 `cx/cy/width/height/bbox` 属性） |
|
||||
| `DetectionFrame(frame_id, detections)` | 单目单帧 |
|
||||
| `RangeRecord` | 输出记录：`frame_id / class_id / degradation / valid / track_id / forward_distance_m(Z) / slant_range_m(R) / disparity_px(D) / center_mid_px(c_mid) / reference_box / reference_eye / confidence / timestamp_ms` |
|
||||
| `Degradation` / `Eye` / `DisparitySource` | 枚举：完整/仅方位；左/右目；同侧边缘/中心回退 |
|
||||
| `sample_point` / `SamplePoint` / `select_disparity` / `Disparity` | 取点器（§6.1，纯函数） |
|
||||
| `Matcher`（Protocol）/ `GreedyMatcher` / `MatchPair` | 匹配器（§6.2，接口注入，可换匈牙利等） |
|
||||
| `solve_range` / `RangeSolution` / `virtual_center_px` / `forward_distance_m` | 测距与方位源（§3.2/§6.6，纯函数） |
|
||||
| `evaluate_validity` / `ValidityDecision` / `range_error_abs_m` / `range_error_rel` / `confidence_from_cost` / `DELTA_D_PX` | 有效性三道门与误差模型（§6.4/§7.1，纯函数） |
|
||||
| `ConsistencyMonitor` / `ConsistencyStatus` | 一致性监控（§6.7，每目标状态） |
|
||||
| `DistanceFilter` | 距离低通 + 跳变重置（§6.5，每目标状态） |
|
||||
| `TargetStore` | 目标隔离与老化清理（§4，track_id 优先 + 最近邻兜底） |
|
||||
| `WarnThrottle` | 告警节流（第 1 次 + 每满 100 次，带累计计数） |
|
||||
|
||||
`logger` 名称为 `stereo_ranger`，走标准 `logging`。
|
||||
|
||||
## 单帧流程（`update` 五步）
|
||||
|
||||
1. **同帧契约**：`left.frame_id != right.frame_id` → 拒收整帧、返回空列表、WARN 节流（§2.1）；
|
||||
2. **取点**：逐框取同侧（左）边缘 x 与框中心；坐标非有限值的框丢弃并 WARN（不参与解算也不发布）；
|
||||
3. **匹配**：硬约束（类别一致、`|Δy| ≤ tau_y_px`、框高相对差 ≤ `tau_h_ratio`）+ 代价升序贪心一一配对，`cost < 1` 才接受；
|
||||
4. **配对目标**：`D → Z = F·B/D`（`D ≤ 0` 判违例）→ `R` 参考 → `c_mid = (cL + cR)/2` → 门① 域内 / 门② 误差预算 / 门③ 一致性 → 有效帧过 `DistanceFilter`，记 `Z_out`；
|
||||
5. **未配对框**：发布仅方位记录（`BEARING_ONLY`，`valid = false`，右目未匹配同样发布）；帧末对未出现目标记失效、连续 `filter_reset_frames` 帧未出现整体清理。
|
||||
|
||||
记录顺序：先按左目框顺序（配对→完整 / 未配对→仅方位），再给未配对右目框的仅方位记录。
|
||||
|
||||
## 实现决策（v1.0 冻结，已回填规格）
|
||||
|
||||
规格 `docs/双目测距技术路线.md` 已按下列决策修订（§4/§6.1/§6.2/§6.4/§6.7/
|
||||
§9/§11 均已更新），本节保留决策动机与实现落点，供重写/移植核对。
|
||||
|
||||
1. **越界 = 有效性门，不是配对硬约束**：规格 §6.2 原把"视差在域内"列入
|
||||
硬约束，与 §5/§6.3/§6.4/§2.3 的表述冲突；按 §5 主流程实现并回填——
|
||||
越界目标仍配对、仍发布 `D` 与 `c_mid`（远场方位闭环的输入基础），仅
|
||||
`valid = false`。
|
||||
2. **视差域权威**：`disparity_min_px / disparity_max_px` 为真源，`z_min_m / z_max_m`
|
||||
为派生只读属性；原 §9.2 占位值不自洽（`D_min = 3 px ⇔ 19.2 m ≠ 20 m`），
|
||||
距离域配置项已从规格删除（§6.4/§9.2）。
|
||||
3. **不实现 §6.1 的"左右边缘各算一次取中值"精化**：两支的 median 恒等于均值，
|
||||
即框中心视差，与"同侧边缘"的理由矛盾，规格已删除该项（早期笔误），留
|
||||
v1.1 亚像素精化。
|
||||
4. **补充配置项**（原文 §9 未列、算法需要，规格 §9.3/§9.5 已回填）：
|
||||
`consistency_jump_px`（§6.7 视差跳变门限）、`isolation_gate_px`（§4 最近邻
|
||||
兜底的门限）。
|
||||
5. **δD 固定 1 px**：`DELTA_D_PX = 1.0`（§7.1 ⚠占位）；标定回填后可参数化。
|
||||
6. **R 与已发布 Z 同基准**：`R = R_raw · z_out / z_raw`（X/Y/Z 均 ∝ Z，精确换基，
|
||||
保证 `R ≥ Z`）；无效帧无 `Z_out` 时 `R` 取本帧原始值（诊断参考）。规格
|
||||
§2.2/§6.5 已注明。
|
||||
7. **置信度映射**（§6.4 ⚠占位）：`clamp(1 − cost, 0, 1)`；连续跳变 ≥ N 帧乘 0.5
|
||||
（`_CONFIDENCE_DEGRADE_FACTOR`）；判无效置 0。
|
||||
8. **一致性只在配对成功帧推进**：仅方位帧没有 D 观测量，不推进 D 序列；但目标
|
||||
未出现的帧会记一次滤波失效（§6.5 失效清理），两条状态机职责不混。
|
||||
9. **取点回退规则**：边缘视差越窗且中心视差在域内 → 回退中心
|
||||
（`CENTER_FALLBACK` + WARN）；两者均越窗 → 保留边缘值供诊断与违例告警。
|
||||
10. **track_id 优先路径提前启用**（v1.1 语义，规格 §4 已按此冻结）：左右
|
||||
`track_id` 一致且 > 0 时用 `(track, 类别, id)` 作隔离键；最近邻只在无
|
||||
track_id 的状态间匹配，一一归属。
|
||||
11. **§3.2 勘误**：原文 `X_C = X + B/2` 与 §6.6 的符号相反（应为 `X_C = X − B/2`），
|
||||
规格已修正；代码不输出 `X_C`，以 §6.6 恒等式为准。
|
||||
|
||||
## 运行期状态清单（§11.1，漏一个就变行为）
|
||||
|
||||
| 状态 | 所属 | 初值 | 改变时机 |
|
||||
| --- | --- | --- | --- |
|
||||
| 连续异常计数 ×目标 | `ConsistencyMonitor` | 0 | 每帧异常 +1 / 正常帧清零 |
|
||||
| 上次滤波值、初始化标志、连续失效计数 ×目标 | `DistanceFilter` | 无 / false / 0 | 有效帧写入 / 首有效帧置位 / 无效帧 +1 |
|
||||
| 最近位置、最近出现帧 ×目标 | `TargetStore` | 首帧写入 | 每次 `resolve` |
|
||||
| 帧序号 | `StereoRanger` | 0 | 每帧 +1（拒收帧不加） |
|
||||
| 告警计数 ×异常类 | `WarnThrottle` | 0 | 每次异常 |
|
||||
|
||||
## 测试与演示
|
||||
|
||||
```bash
|
||||
cd utils
|
||||
"D:/program file/conda/envs/pytorch/python.exe" -m pytest tests/stereo_ranger -q # 111 条
|
||||
"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q # 全量 220 条
|
||||
python utils/demo_stereo_ranger.py # 六场景合成框对演示
|
||||
```
|
||||
|
||||
构件级测试逐条对应 §11.4（取点器/匹配器/测距/有效性/滤波器/一致性/全流程），
|
||||
**全部用合成框对驱动，无需真实图像**（几何是解析可断言的）：
|
||||
|
||||
- 取点：同侧边缘取值、框宽差不进入视差、中心对照、越窗回退、边界含等号、负视差；
|
||||
- 匹配：硬约束各一例、`cost ≥ 1` 拒绝、贪心排序、一一配对；
|
||||
- 测距：`Z = F·B/D` 回收、`D ≤ 0` 违例、`c_mid` 等权平均恒等式、误差平方增长；
|
||||
- 有效性：三道门与边界归类、预算门通过/拦截两侧；
|
||||
- 滤波/一致性：低通收敛、跳变重置不拖尾、失效清理、N 帧降级 / M 帧判无效 / 正常帧清零；
|
||||
- 全流程：单目标、滤波、远场越界仍配对、单目退化、记录顺序、非同帧拒收与节流、
|
||||
空输入、非有限框、track_id 跨大位移保持状态、最近邻越门限新建状态、匹配器注入、`reset`。
|
||||
|
||||
demo 覆盖：① 逼近（三道门依次生效 + 滤波）② 远场域外（方位照常发布）
|
||||
③ 单目退化 ④ 非同帧拒收与节流 ⑤ 一致性降级/判无效 ⑥ 多目标贪心配对。
|
||||
@@ -0,0 +1,72 @@
|
||||
"""stereo_ranger —— 双目稀疏目标测距（规格：docs/双目测距技术路线.md）。
|
||||
|
||||
纯算法包：输入校正后坐标系的同帧左右目检测框，输出目标距离记录（Z/R/D、
|
||||
虚拟双目中心方位、valid/confidence）。不做目标识别、图像采集与立体校正；
|
||||
仅依赖标准库。⚠ 全部数值参数为占位值，标定与地面验证完成前不得作为飞行
|
||||
控制决策依据（§12.3 红线）。
|
||||
"""
|
||||
|
||||
from .config import StereoConfig, load_from_json
|
||||
from .consistency import ConsistencyMonitor, ConsistencyStatus
|
||||
from .distance_filter import DistanceFilter
|
||||
from .geometry import (
|
||||
DELTA_D_PX,
|
||||
RangeSolution,
|
||||
ValidityDecision,
|
||||
confidence_from_cost,
|
||||
evaluate_validity,
|
||||
forward_distance_m,
|
||||
range_error_abs_m,
|
||||
range_error_rel,
|
||||
solve_range,
|
||||
virtual_center_px,
|
||||
)
|
||||
from .matching import GreedyMatcher, MatchPair, Matcher
|
||||
from .sampling import Disparity, SamplePoint, sample_point, select_disparity
|
||||
from .stereo_ranger import StereoRanger
|
||||
from .target_store import TargetStore
|
||||
from .types import (
|
||||
BBoxXYXY,
|
||||
Degradation,
|
||||
Detection,
|
||||
DetectionFrame,
|
||||
DisparitySource,
|
||||
Eye,
|
||||
RangeRecord,
|
||||
)
|
||||
from .warn_throttle import WarnThrottle
|
||||
|
||||
__all__ = [
|
||||
"BBoxXYXY",
|
||||
"DELTA_D_PX",
|
||||
"ConsistencyMonitor",
|
||||
"ConsistencyStatus",
|
||||
"Degradation",
|
||||
"Detection",
|
||||
"DetectionFrame",
|
||||
"Disparity",
|
||||
"DisparitySource",
|
||||
"DistanceFilter",
|
||||
"Eye",
|
||||
"GreedyMatcher",
|
||||
"MatchPair",
|
||||
"Matcher",
|
||||
"RangeRecord",
|
||||
"RangeSolution",
|
||||
"SamplePoint",
|
||||
"StereoConfig",
|
||||
"StereoRanger",
|
||||
"TargetStore",
|
||||
"ValidityDecision",
|
||||
"WarnThrottle",
|
||||
"confidence_from_cost",
|
||||
"evaluate_validity",
|
||||
"forward_distance_m",
|
||||
"load_from_json",
|
||||
"range_error_abs_m",
|
||||
"range_error_rel",
|
||||
"sample_point",
|
||||
"select_disparity",
|
||||
"solve_range",
|
||||
"virtual_center_px",
|
||||
]
|
||||
@@ -0,0 +1,123 @@
|
||||
"""StereoConfig 与 JSON 配置加载（§9 配置参数全量表）。
|
||||
|
||||
全部参数配置外置、构造期一次加载并校验：缺项、类型错误、非法取值一律拒绝
|
||||
进入运行期（fail-fast）。所有默认值均为 ⚠占位值，未整定前不得用于飞行决策
|
||||
（§12.3 红线 1/2）。
|
||||
|
||||
域权威（实现决策）：视差域 disparity_min_px / disparity_max_px 为权威，
|
||||
距离域 z_min_m / z_max_m 由 Z = F·B/D 派生为只读属性——域判定使用视差
|
||||
（§11.2 直接观测量），两套阈值不再冗余存储、避免互推舍入不一致（§6.4）。
|
||||
"""
|
||||
|
||||
import json
|
||||
import math
|
||||
from dataclasses import dataclass, fields
|
||||
from typing import Any, Dict
|
||||
|
||||
# JSON 加载时的整数型字段（其余按数值校验）
|
||||
_INT_FIELDS = frozenset({"consistency_N_frames", "consistency_M_frames", "filter_reset_frames"})
|
||||
|
||||
|
||||
@dataclass
|
||||
class StereoConfig:
|
||||
# ---- 几何组（来自标定 P1/P2，§9.1；⚠全部为占位值）----
|
||||
fx_px: float = 960.0
|
||||
fy_px: float = 960.0
|
||||
cx_px: float = 960.0
|
||||
cy_px: float = 540.0
|
||||
baseline_m: float = 0.06
|
||||
|
||||
# ---- 匹配组（§9.2）----
|
||||
tau_y_px: float = 2.0
|
||||
tau_h_ratio: float = 0.25
|
||||
disparity_min_px: float = 3.0
|
||||
disparity_max_px: float = 57.6 # ⚠占位：F=960/B=0.06 下 ⇔ 近界 1.0 m
|
||||
|
||||
# ---- 有效性组（§9.3）----
|
||||
max_delta_z_m: float = 2.0 # 误差预算 δZ(δD = 1px) 上限
|
||||
consistency_N_frames: int = 3 # 连续跳变降置信度的帧数
|
||||
consistency_M_frames: int = 5 # 连续异常判 invalid 的帧数
|
||||
consistency_jump_px: float = 3.0 # ⚠补充项：§9 未列，§6.7 的视差跳变门限
|
||||
|
||||
# ---- 滤波组（§9.4）----
|
||||
smooth_alpha: float = 0.3
|
||||
jump_reset_m: float = 5.0
|
||||
filter_reset_frames: int = 5
|
||||
|
||||
# ---- 目标隔离（§4：类别 + 框位置最近邻；⚠补充项）----
|
||||
isolation_gate_px: float = 50.0
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
_require(self.fx_px > 0.0, "fx_px", "必须为正")
|
||||
_require(self.fy_px > 0.0, "fy_px", "必须为正")
|
||||
_require(self.cx_px >= 0.0, "cx_px", "必须非负")
|
||||
_require(self.cy_px >= 0.0, "cy_px", "必须非负")
|
||||
_require(self.baseline_m > 0.0, "baseline_m", "必须为正")
|
||||
_require(self.tau_y_px > 0.0, "tau_y_px", "必须为正")
|
||||
_require(self.tau_h_ratio > 0.0, "tau_h_ratio", "必须为正")
|
||||
_require(
|
||||
0.0 < self.disparity_min_px < self.disparity_max_px,
|
||||
"disparity_min_px/disparity_max_px",
|
||||
"须满足 0 < 下限 < 上限",
|
||||
)
|
||||
_require(self.max_delta_z_m > 0.0, "max_delta_z_m", "必须为正")
|
||||
_require(self.consistency_N_frames >= 1, "consistency_N_frames", "必须 ≥ 1")
|
||||
_require(
|
||||
self.consistency_M_frames >= self.consistency_N_frames,
|
||||
"consistency_M_frames",
|
||||
"必须 ≥ consistency_N_frames",
|
||||
)
|
||||
_require(self.consistency_jump_px > 0.0, "consistency_jump_px", "必须为正")
|
||||
_require(0.0 < self.smooth_alpha <= 1.0, "smooth_alpha", "须落在 (0, 1]")
|
||||
_require(self.jump_reset_m > 0.0, "jump_reset_m", "必须为正")
|
||||
_require(self.filter_reset_frames >= 1, "filter_reset_frames", "必须 ≥ 1")
|
||||
_require(self.isolation_gate_px > 0.0, "isolation_gate_px", "必须为正")
|
||||
|
||||
@property
|
||||
def focal_baseline_px_m(self) -> float:
|
||||
"""F·B：构造期一次算好的常量，热路径只做乘除（§11.1）。"""
|
||||
return self.fx_px * self.baseline_m
|
||||
|
||||
@property
|
||||
def z_min_m(self) -> float:
|
||||
"""近界（由 disparity_max_px 派生）。"""
|
||||
return self.focal_baseline_px_m / self.disparity_max_px
|
||||
|
||||
@property
|
||||
def z_max_m(self) -> float:
|
||||
"""远界（由 disparity_min_px 派生）。"""
|
||||
return self.focal_baseline_px_m / self.disparity_min_px
|
||||
|
||||
@property
|
||||
def z_error_budget_max_m(self) -> float:
|
||||
"""误差预算门限对应的最远距离 sqrt(δZ_max·F·B)（δD = 1px，§7.2）。"""
|
||||
return math.sqrt(self.max_delta_z_m * self.focal_baseline_px_m)
|
||||
|
||||
|
||||
def load_from_json(path: str) -> StereoConfig:
|
||||
"""从 JSON 文件构建配置；缺项/未知字段/类型错误/非法取值一律抛 ValueError。"""
|
||||
with open(path, encoding="utf-8") as handle:
|
||||
data: Dict[str, Any] = json.load(handle)
|
||||
if not isinstance(data, dict):
|
||||
raise ValueError("配置非法: 顶层须为 JSON 对象")
|
||||
known = {item.name for item in fields(StereoConfig)}
|
||||
unknown = sorted(set(data) - known)
|
||||
if unknown:
|
||||
raise ValueError(f"未知配置字段: {unknown}")
|
||||
missing = sorted(known - set(data))
|
||||
if missing:
|
||||
raise ValueError(f"缺少配置字段: {missing}")
|
||||
for name, value in data.items():
|
||||
if isinstance(value, bool):
|
||||
raise ValueError(f"配置类型非法: {name} 须为数值")
|
||||
if name in _INT_FIELDS:
|
||||
if not isinstance(value, int):
|
||||
raise ValueError(f"配置类型非法: {name} 须为整数")
|
||||
elif not isinstance(value, (int, float)):
|
||||
raise ValueError(f"配置类型非法: {name} 须为数值")
|
||||
return StereoConfig(**data)
|
||||
|
||||
|
||||
def _require(condition: bool, field_name: str, message: str) -> None:
|
||||
if not condition:
|
||||
raise ValueError(f"配置非法: {field_name} {message}")
|
||||
@@ -0,0 +1,74 @@
|
||||
"""一致性监控（§6.7）：时序层面质量自检，每目标独立状态。
|
||||
|
||||
机载激光移除后无外部距离基准，飞行中只能靠自身时序一致性监控数据质量：
|
||||
**只能降级、不能纠偏**（绝对精度靠地面静态验证）。匹配层面的质量（|Δy|
|
||||
残差、框高一致性、代价门限）由匹配硬约束承担，本节只管时序层面。
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Dict, Hashable, Optional
|
||||
|
||||
|
||||
@dataclass
|
||||
class ConsistencyStatus:
|
||||
"""单目标单帧一致性判定结果（§6.7）。"""
|
||||
|
||||
jump_px: Optional[float] # |D − 上次 D|；首帧无参考为 None
|
||||
abnormal: bool # 本帧是否跳变超门限
|
||||
streak: int # 连续异常帧数（含本帧，正常帧清零）
|
||||
confidence_degraded: bool # streak ≥ N：置信度降级
|
||||
invalid: bool # streak ≥ M：质量语义判无效
|
||||
|
||||
|
||||
class ConsistencyMonitor:
|
||||
"""每目标：上次视差 + 连续异常计数（§11.1 必须持久化的状态）。"""
|
||||
|
||||
def __init__(self, jump_px: float, degrade_frames: int, invalid_frames: int) -> None:
|
||||
if jump_px <= 0.0:
|
||||
raise ValueError("jump_px 必须为正")
|
||||
if degrade_frames < 1:
|
||||
raise ValueError("degrade_frames 必须 ≥ 1")
|
||||
if invalid_frames < degrade_frames:
|
||||
raise ValueError("invalid_frames 必须 ≥ degrade_frames")
|
||||
self._jump_px = jump_px
|
||||
self._degrade_frames = degrade_frames
|
||||
self._invalid_frames = invalid_frames
|
||||
self._last_d: Dict[Hashable, float] = {}
|
||||
self._streak: Dict[Hashable, int] = {}
|
||||
|
||||
def advance(self, key: Hashable, disparity_px: float) -> ConsistencyStatus:
|
||||
"""推进目标 key 的一致性判定（配对成功帧每帧一次）。"""
|
||||
last = self._last_d.get(key)
|
||||
if last is None:
|
||||
jump: Optional[float] = None
|
||||
abnormal = False
|
||||
streak = 0
|
||||
else:
|
||||
jump = abs(disparity_px - last)
|
||||
abnormal = jump > self._jump_px
|
||||
streak = self._streak.get(key, 0) + 1 if abnormal else 0
|
||||
self._last_d[key] = disparity_px
|
||||
if streak:
|
||||
self._streak[key] = streak
|
||||
else:
|
||||
self._streak.pop(key, None)
|
||||
return ConsistencyStatus(
|
||||
jump_px=jump,
|
||||
abnormal=abnormal,
|
||||
streak=streak,
|
||||
confidence_degraded=streak >= self._degrade_frames,
|
||||
invalid=streak >= self._invalid_frames,
|
||||
)
|
||||
|
||||
def streak(self, key: Hashable) -> int:
|
||||
"""目标 key 当前连续异常计数。"""
|
||||
return self._streak.get(key, 0)
|
||||
|
||||
def drop(self, key: Hashable) -> None:
|
||||
"""清理目标 key 的状态（目标老化清理时调用）。"""
|
||||
self._last_d.pop(key, None)
|
||||
self._streak.pop(key, None)
|
||||
|
||||
def clear(self) -> None:
|
||||
self._last_d.clear()
|
||||
self._streak.clear()
|
||||
@@ -0,0 +1,68 @@
|
||||
"""时间滤波器（§6.5）：一阶低通 + 单帧跳变重置，每目标独立状态。
|
||||
|
||||
- 单帧毛刺由跳变重置处理，持续性异常由一致性监控（§6.7）处理，两者职责
|
||||
不同不得合并；
|
||||
- 滤波只在**有效帧**之间进行，无效/缺席帧不参与平滑；α = 1 等效不滤波；
|
||||
- 连续失效（无效或缺席）达 filter_reset_frames 帧清空状态，下次有效帧重新
|
||||
初始化（防止用陈旧状态平滑新测量）。
|
||||
"""
|
||||
|
||||
from typing import Dict, Hashable, Optional
|
||||
|
||||
|
||||
class DistanceFilter:
|
||||
"""每目标：上次滤波值 + 连续失效计数（§11.1 必须持久化的状态）。"""
|
||||
|
||||
def __init__(self, alpha: float, jump_reset_m: float, reset_frames: int) -> None:
|
||||
if not 0.0 < alpha <= 1.0:
|
||||
raise ValueError("alpha 须落在 (0, 1]")
|
||||
if jump_reset_m <= 0.0:
|
||||
raise ValueError("jump_reset_m 必须为正")
|
||||
if reset_frames < 1:
|
||||
raise ValueError("reset_frames 必须 ≥ 1")
|
||||
self._alpha = alpha
|
||||
self._jump_reset_m = jump_reset_m
|
||||
self._reset_frames = reset_frames
|
||||
self._values: Dict[Hashable, float] = {}
|
||||
self._invalid_streak: Dict[Hashable, int] = {}
|
||||
|
||||
def advance(self, key: Hashable, z_m: float) -> float:
|
||||
"""有效帧推进：未初始化→直接置值；跳变超门限→重置；否则一阶低通。"""
|
||||
previous = self._values.get(key)
|
||||
self._invalid_streak.pop(key, None)
|
||||
if previous is None:
|
||||
self._values[key] = z_m
|
||||
return z_m
|
||||
if abs(z_m - previous) > self._jump_reset_m:
|
||||
self._values[key] = z_m # 直接重置而非滤波，防止拖尾
|
||||
return z_m
|
||||
smoothed = previous + self._alpha * (z_m - previous)
|
||||
self._values[key] = smoothed
|
||||
return smoothed
|
||||
|
||||
def invalidate(self, key: Hashable) -> None:
|
||||
"""无效/缺席帧：连续失效达门限后清空该目标滤波状态（§6.5 记失效）。"""
|
||||
if key not in self._values:
|
||||
return
|
||||
streak = self._invalid_streak.get(key, 0) + 1
|
||||
if streak >= self._reset_frames:
|
||||
self._values.pop(key, None)
|
||||
self._invalid_streak.pop(key, None)
|
||||
else:
|
||||
self._invalid_streak[key] = streak
|
||||
|
||||
def value(self, key: Hashable) -> Optional[float]:
|
||||
"""目标 key 当前滤波值；未初始化/已清空为 None。"""
|
||||
return self._values.get(key)
|
||||
|
||||
def invalid_streak(self, key: Hashable) -> int:
|
||||
"""目标 key 当前连续失效计数（内部状态，供测试/诊断）。"""
|
||||
return self._invalid_streak.get(key, 0)
|
||||
|
||||
def drop(self, key: Hashable) -> None:
|
||||
self._values.pop(key, None)
|
||||
self._invalid_streak.pop(key, None)
|
||||
|
||||
def clear(self) -> None:
|
||||
self._values.clear()
|
||||
self._invalid_streak.clear()
|
||||
@@ -0,0 +1,84 @@
|
||||
"""测距计算与有效性判定（§3.2/§6.4/§7.1）：纯函数、无状态。
|
||||
|
||||
Z = F·B/D（前向分量，距离通道唯一消费量，§10.5）；X/Y 以左目系表达；
|
||||
R = sqrt(X² + Y² + Z²) 仅参考输出；c_mid = 两目框中心等权平均（虚拟双目
|
||||
中心，§6.6，逐点精确、无近似）。
|
||||
"""
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional, Tuple
|
||||
|
||||
from .config import StereoConfig
|
||||
from .types import Detection
|
||||
|
||||
DELTA_D_PX = 1.0 # 误差预算的δD 基准（§7.2 定义 δZ(δD = 1px)）
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RangeSolution:
|
||||
"""测距解算结果（Z 为前向分量，X/Y 属左目系，§3.2）。"""
|
||||
|
||||
z_m: float
|
||||
x_m: float
|
||||
y_m: float
|
||||
r_m: float # 斜距（参考值，不进控制回路）
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ValidityDecision:
|
||||
"""有效性判定结果（§6.4 三道门的前两道；门③一致性由调用方合流）。"""
|
||||
|
||||
in_domain: bool # ① 域内：D ∈ [D_min, D_max]
|
||||
budget_ok: bool # ② 误差预算：δZ(δD = 1px) ≤ δZ_max
|
||||
valid: bool # ① 且 ②
|
||||
|
||||
|
||||
def forward_distance_m(disparity_px: float, config: StereoConfig) -> float:
|
||||
"""Z = F·B/D；调用方须先判 D ≤ 0 物理违例（§6.3/§11.2 除零纪律）。"""
|
||||
if disparity_px <= 0.0:
|
||||
raise ValueError(f"视差须为正（先判违例再除）: D={disparity_px}")
|
||||
return config.focal_baseline_px_m / disparity_px
|
||||
|
||||
|
||||
def solve_range(left_box: Detection, right_box: Detection, disparity_px: float, config: StereoConfig) -> RangeSolution:
|
||||
"""由视差与参考目（左目）框中心解算 (Z, X, Y, R)（§3.2）。"""
|
||||
z_m = forward_distance_m(disparity_px, config)
|
||||
x_m = (left_box.cx - config.cx_px) * z_m / config.fx_px
|
||||
y_m = (left_box.cy - config.cy_px) * z_m / config.fy_px
|
||||
r_m = math.sqrt(x_m * x_m + y_m * y_m + z_m * z_m)
|
||||
return RangeSolution(z_m=z_m, x_m=x_m, y_m=y_m, r_m=r_m)
|
||||
|
||||
|
||||
def virtual_center_px(left_box: Detection, right_box: Detection) -> Tuple[float, float]:
|
||||
"""虚拟双目中心 c_mid = (cL + cR)/2（§6.6：等权平均 = 把相机搬到模组中线）。
|
||||
|
||||
与视差取点（同侧边缘）是两个不同的取点，不得混用（§6.6）。
|
||||
"""
|
||||
return ((left_box.cx + right_box.cx) / 2.0, (left_box.cy + right_box.cy) / 2.0)
|
||||
|
||||
|
||||
def range_error_abs_m(z_m: float, config: StereoConfig, delta_d_px: float = DELTA_D_PX) -> float:
|
||||
"""绝对误差估计 δZ ≈ Z²·δD/(F·B)（§7.1）。"""
|
||||
return z_m * z_m * delta_d_px / config.focal_baseline_px_m
|
||||
|
||||
|
||||
def range_error_rel(z_m: float, config: StereoConfig, delta_d_px: float = DELTA_D_PX) -> float:
|
||||
"""相对误差估计 δZ/Z ≈ Z·δD/(F·B)（§7.1）。"""
|
||||
return z_m * delta_d_px / config.focal_baseline_px_m
|
||||
|
||||
|
||||
def evaluate_validity(disparity_px: float, z_m: Optional[float], config: StereoConfig) -> ValidityDecision:
|
||||
"""有效性门①②（§6.4）：域内 + 误差预算；z_m=None（D ≤ 0）时预算判失败。"""
|
||||
in_domain = config.disparity_min_px <= disparity_px <= config.disparity_max_px
|
||||
budget_ok = (
|
||||
z_m is not None
|
||||
and math.isfinite(z_m)
|
||||
and range_error_abs_m(z_m, config) <= config.max_delta_z_m
|
||||
)
|
||||
return ValidityDecision(in_domain=in_domain, budget_ok=budget_ok, valid=in_domain and budget_ok)
|
||||
|
||||
|
||||
def confidence_from_cost(cost: float) -> float:
|
||||
"""匹配代价 → 置信度基值：线性映射 clamp(1 − cost, 0, 1)（§6.4 ⚠占位映射）。"""
|
||||
return min(1.0, max(0.0, 1.0 - cost))
|
||||
@@ -0,0 +1,82 @@
|
||||
"""左右匹配器（§6.2）：硬约束 + 代价升序贪心一一配对；接口注入。
|
||||
|
||||
默认 GreedyMatcher 为纯函数（§4 构件表）；调用方可通过
|
||||
StereoRanger(matcher=...) 注入匈牙利等替换实现而不动调用方（§10.3）。
|
||||
|
||||
硬约束（任一不满足直接拒绝配对，不进代价排序）：类别一致、|Δy| ≤ τ_y、
|
||||
框高相对差 ≤ τ_h。视差域 [D_min, D_max] 不参与配对：越界属有效性门
|
||||
（§6.2 已回填，与 §5 失败路径表/§6.3/§6.4/§2.3 一致）。
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import List, Protocol, Sequence
|
||||
|
||||
from .config import StereoConfig
|
||||
from .sampling import SamplePoint
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MatchPair:
|
||||
"""一一配对结果：左右下标 + 匹配代价（§6.2）。"""
|
||||
|
||||
left_index: int
|
||||
right_index: int
|
||||
cost: float
|
||||
|
||||
|
||||
class Matcher(Protocol):
|
||||
"""匹配器接口（§4：接口注入，默认贪心）。"""
|
||||
|
||||
def match(
|
||||
self,
|
||||
left: Sequence[SamplePoint],
|
||||
right: Sequence[SamplePoint],
|
||||
config: StereoConfig,
|
||||
) -> List[MatchPair]:
|
||||
...
|
||||
|
||||
|
||||
class GreedyMatcher:
|
||||
"""硬约束过滤 + 代价升序贪心：cost < 1 才接受，一一配对。"""
|
||||
|
||||
def match(
|
||||
self,
|
||||
left: Sequence[SamplePoint],
|
||||
right: Sequence[SamplePoint],
|
||||
config: StereoConfig,
|
||||
) -> List[MatchPair]:
|
||||
tau_y = config.tau_y_px
|
||||
tau_h = config.tau_h_ratio
|
||||
candidates = []
|
||||
for i, point_l in enumerate(left):
|
||||
if not point_l.finite:
|
||||
continue
|
||||
for j, point_r in enumerate(right):
|
||||
if not point_r.finite:
|
||||
continue
|
||||
if point_l.box.class_id != point_r.box.class_id:
|
||||
continue
|
||||
dy = abs(point_l.center_y - point_r.center_y)
|
||||
if dy > tau_y:
|
||||
continue
|
||||
height_max = max(point_l.box.height, point_r.box.height)
|
||||
if height_max <= 0.0:
|
||||
continue
|
||||
dh_ratio = abs(point_l.box.height - point_r.box.height) / height_max
|
||||
if dh_ratio > tau_h:
|
||||
continue
|
||||
cost = dy / tau_y + dh_ratio / tau_h
|
||||
if cost >= 1.0:
|
||||
continue
|
||||
candidates.append((cost, i, j))
|
||||
candidates.sort(key=lambda item: (item[0], item[1], item[2]))
|
||||
used_left = set()
|
||||
used_right = set()
|
||||
pairs: List[MatchPair] = []
|
||||
for cost, i, j in candidates:
|
||||
if i in used_left or j in used_right:
|
||||
continue
|
||||
pairs.append(MatchPair(left_index=i, right_index=j, cost=cost))
|
||||
used_left.add(i)
|
||||
used_right.add(j)
|
||||
return pairs
|
||||
@@ -0,0 +1,73 @@
|
||||
"""视差取点器（§6.1）：同侧（左）边缘为主方案，边缘视差越窗时回退框中心。
|
||||
|
||||
纯函数、无状态（§4 构件表）。为什么用同侧边缘：同一物理目标在两目中的
|
||||
检测框宽并不严格相等，框中心视差会混入框宽差的一半，同侧边缘视差可抵消
|
||||
框宽项。
|
||||
|
||||
注：§6.1 的"左右两边缘各算一次视差取中值"精化在数学上恒等于框中心视差
|
||||
（两支的 median = 均值），与"中心视差混入框宽差"的理由矛盾，属原文笔误，
|
||||
v1.0 不实现；规格 §6.1 已删除该项并注明（见包 README「实现决策」）。
|
||||
"""
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
|
||||
from .config import StereoConfig
|
||||
from .types import Detection, DisparitySource
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SamplePoint:
|
||||
"""单目单框取点结果：同侧边缘候选与框中心（§6.1/§6.6）。
|
||||
|
||||
finite=False 表示坐标非有限值：该框不可参与解算，由调用方丢弃并告警。
|
||||
"""
|
||||
|
||||
box: Detection
|
||||
edge_x: float # 同侧（左）边缘 x = box.x1
|
||||
center_x: float
|
||||
center_y: float
|
||||
finite: bool
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Disparity:
|
||||
"""配对后的视差取点结果。"""
|
||||
|
||||
d_px: float
|
||||
source: DisparitySource
|
||||
fallback_used: bool # 边缘视差越窗、回退中心（需告警节流，§6.1）
|
||||
in_domain: bool # d ∈ [D_min, D_max]（含边界，§11.2）
|
||||
positive: bool # d > 0（正视差是物理前提，§6.3）
|
||||
|
||||
|
||||
def sample_point(detection: Detection) -> SamplePoint:
|
||||
"""逐框取点（§5 第 ② 步，每目每框一次）。"""
|
||||
finite = all(math.isfinite(value) for value in (detection.x1, detection.y1, detection.x2, detection.y2))
|
||||
return SamplePoint(
|
||||
box=detection,
|
||||
edge_x=detection.x1,
|
||||
center_x=detection.cx,
|
||||
center_y=detection.cy,
|
||||
finite=finite,
|
||||
)
|
||||
|
||||
|
||||
def select_disparity(left: SamplePoint, right: SamplePoint, config: StereoConfig) -> Disparity:
|
||||
"""在同侧边缘视差与框中心视差之间选择（§6.1 主取点 + 越窗回退）。
|
||||
|
||||
规则：边缘视差在域内取边缘；否则若中心视差在域内则回退中心（告警节流）；
|
||||
两者均越窗时保留边缘值（供诊断与违例告警），由有效性门判无效。
|
||||
"""
|
||||
d_edge = left.edge_x - right.edge_x
|
||||
if _in_domain(d_edge, config):
|
||||
return Disparity(d_edge, DisparitySource.LEFT_EDGE, False, True, d_edge > 0.0)
|
||||
d_center = left.center_x - right.center_x
|
||||
if _in_domain(d_center, config):
|
||||
return Disparity(d_center, DisparitySource.CENTER_FALLBACK, True, True, d_center > 0.0)
|
||||
d = d_edge if math.isfinite(d_edge) else d_center
|
||||
return Disparity(d, DisparitySource.LEFT_EDGE, False, False, d > 0.0)
|
||||
|
||||
|
||||
def _in_domain(d_px: float, config: StereoConfig) -> bool:
|
||||
return config.disparity_min_px <= d_px <= config.disparity_max_px
|
||||
@@ -0,0 +1,227 @@
|
||||
"""StereoRanger —— 双目稀疏目标测距（规格 docs/双目测距技术路线.md）。
|
||||
|
||||
单周期五步（§5）：① 同帧契约检查 → ② 取点 → ③ 左右匹配 →
|
||||
④ 配对目标：测距 + 方位源 + 有效性 + 滤波 → ⑤ 单目退化发布。
|
||||
|
||||
输入为左右目各自、校正后原生坐标系下的检测框；不做目标识别，不做图像
|
||||
采集/立体校正。纯计算单元（§11.3）：单周期处理无阻塞、无 IO；同一实例
|
||||
不得并发调用 update()。
|
||||
"""
|
||||
|
||||
import logging
|
||||
from typing import List, Optional, Sequence
|
||||
|
||||
from .config import StereoConfig
|
||||
from .geometry import confidence_from_cost, evaluate_validity, solve_range, virtual_center_px
|
||||
from .matching import GreedyMatcher, Matcher, MatchPair
|
||||
from .sampling import SamplePoint, sample_point, select_disparity
|
||||
from .target_store import TargetStore
|
||||
from .types import Degradation, Detection, DetectionFrame, Eye, RangeRecord
|
||||
from .warn_throttle import WarnThrottle
|
||||
|
||||
logger = logging.getLogger("stereo_ranger")
|
||||
|
||||
_CONFIDENCE_DEGRADE_FACTOR = 0.5 # ⚠占位：连续跳变 ≥ N 帧的置信度降级系数（§6.4）
|
||||
|
||||
|
||||
class StereoRanger:
|
||||
"""双目稀疏目标测距器：每帧调用一次 update()，传入同帧左右目检测结果。"""
|
||||
|
||||
def __init__(self, config: Optional[StereoConfig] = None, matcher: Optional[Matcher] = None) -> None:
|
||||
self._config = config if config is not None else StereoConfig()
|
||||
self._matcher = matcher if matcher is not None else GreedyMatcher()
|
||||
self._store = TargetStore(self._config)
|
||||
self._warn = WarnThrottle()
|
||||
self._frame_index = 0
|
||||
|
||||
@property
|
||||
def config(self) -> StereoConfig:
|
||||
return self._config
|
||||
|
||||
@property
|
||||
def matcher(self) -> Matcher:
|
||||
return self._matcher
|
||||
|
||||
def reset(self) -> None:
|
||||
"""清空全部运行期状态（滤波 / 一致性 / 隔离 / 节流计数）。"""
|
||||
self._store.clear()
|
||||
self._warn.clear()
|
||||
self._frame_index = 0
|
||||
|
||||
def update(self, left: DetectionFrame, right: DetectionFrame, now_ms: int = 0) -> List[RangeRecord]:
|
||||
"""单周期处理（§5）：返回本帧记录列表。
|
||||
|
||||
顺序：先按左目框顺序给出各自记录（配对→完整记录 / 未配对→仅方位），
|
||||
再给出未配对的右目框的仅方位记录；左目存在时记录数与左目框数一致。
|
||||
"""
|
||||
config = self._config
|
||||
if left.frame_id != right.frame_id:
|
||||
self._warn_emit(
|
||||
"frame_mismatch",
|
||||
"左右帧标识不一致（L=%s, R=%s），拒收本帧",
|
||||
left.frame_id,
|
||||
right.frame_id,
|
||||
)
|
||||
return []
|
||||
|
||||
self._frame_index += 1
|
||||
frame_index = self._frame_index
|
||||
left_points = self._drop_invalid([sample_point(det) for det in left.detections])
|
||||
right_points = self._drop_invalid([sample_point(det) for det in right.detections])
|
||||
|
||||
pairs = self._matcher.match(left_points, right_points, config)
|
||||
pair_of_left = {pair.left_index: pair for pair in pairs}
|
||||
paired_right = {pair.right_index for pair in pairs}
|
||||
|
||||
records: List[RangeRecord] = []
|
||||
for index, point_l in enumerate(left_points):
|
||||
pair = pair_of_left.get(index)
|
||||
if pair is None:
|
||||
records.append(self._bearing_only_record(point_l, left.frame_id, now_ms, Eye.LEFT))
|
||||
continue
|
||||
records.append(
|
||||
self._complete_record(
|
||||
point_l,
|
||||
right_points[pair.right_index],
|
||||
pair,
|
||||
left.frame_id,
|
||||
now_ms,
|
||||
frame_index,
|
||||
)
|
||||
)
|
||||
for index, point_r in enumerate(right_points):
|
||||
if index in paired_right:
|
||||
continue
|
||||
records.append(self._bearing_only_record(point_r, left.frame_id, now_ms, Eye.RIGHT))
|
||||
|
||||
self._store.end_frame(frame_index)
|
||||
return records
|
||||
|
||||
# ---- 内部实现 ----
|
||||
|
||||
def _complete_record(
|
||||
self,
|
||||
point_l: SamplePoint,
|
||||
point_r: SamplePoint,
|
||||
pair: MatchPair,
|
||||
frame_id: int,
|
||||
now_ms: int,
|
||||
frame_index: int,
|
||||
) -> RangeRecord:
|
||||
"""配对成功的记录：测距 + 方位源 + 有效性（门①②）+ 一致性（门③）+ 滤波。"""
|
||||
config = self._config
|
||||
box_l = point_l.box
|
||||
box_r = point_r.box
|
||||
disparity = select_disparity(point_l, point_r, config)
|
||||
center_mid = virtual_center_px(box_l, box_r)
|
||||
|
||||
if disparity.fallback_used:
|
||||
self._warn_emit("center_fallback", "边缘视差越窗，回退框中心（框宽差可能混入视差）")
|
||||
if not disparity.positive:
|
||||
self._warn_emit("disparity_violation", "视差违例 D=%.2f px（须为正），该目标判无效", disparity.d_px)
|
||||
elif not disparity.in_domain:
|
||||
self._warn_emit(
|
||||
"disparity_violation",
|
||||
"视差超界 D=%.2f px（域 [%.2f, %.2f]），该目标判无效",
|
||||
disparity.d_px,
|
||||
config.disparity_min_px,
|
||||
config.disparity_max_px,
|
||||
)
|
||||
|
||||
key = self._store.resolve(
|
||||
box_l.class_id,
|
||||
_shared_track_id(box_l, box_r),
|
||||
center_mid,
|
||||
frame_index,
|
||||
)
|
||||
status = self._store.consistency.advance(key, disparity.d_px)
|
||||
if status.invalid:
|
||||
self._warn_emit(
|
||||
"consistency_invalid",
|
||||
"连续 %d 帧视差跳变超门限（%.1f px），质量判无效",
|
||||
status.streak,
|
||||
config.consistency_jump_px,
|
||||
)
|
||||
|
||||
z_raw = config.focal_baseline_px_m / disparity.d_px if disparity.positive else None
|
||||
solution = solve_range(box_l, box_r, disparity.d_px, config) if z_raw is not None else None
|
||||
decision = evaluate_validity(disparity.d_px, z_raw, config)
|
||||
valid = decision.valid and not status.invalid
|
||||
|
||||
confidence = confidence_from_cost(pair.cost)
|
||||
if status.confidence_degraded:
|
||||
confidence *= _CONFIDENCE_DEGRADE_FACTOR
|
||||
if status.invalid:
|
||||
confidence = 0.0
|
||||
|
||||
if valid:
|
||||
z_out = self._store.distance_filter.advance(key, z_raw)
|
||||
else:
|
||||
self._store.distance_filter.invalidate(key)
|
||||
z_out = None
|
||||
|
||||
# 参考斜距与已发布的前向距离同基准：X/Y/Z 均 ∝ Z，按 z_out/z_raw 缩放即精确换基
|
||||
slant_range_m = None
|
||||
if solution is not None:
|
||||
slant_range_m = solution.r_m if z_out is None else solution.r_m * (z_out / z_raw)
|
||||
|
||||
return RangeRecord(
|
||||
frame_id=frame_id,
|
||||
class_id=box_l.class_id,
|
||||
degradation=Degradation.COMPLETE,
|
||||
valid=valid,
|
||||
track_id=box_l.track_id if box_l.track_id is not None else box_r.track_id,
|
||||
forward_distance_m=z_out,
|
||||
slant_range_m=slant_range_m,
|
||||
disparity_px=disparity.d_px,
|
||||
center_mid_px=center_mid,
|
||||
reference_box=box_l.bbox,
|
||||
reference_eye=Eye.LEFT,
|
||||
confidence=confidence,
|
||||
timestamp_ms=now_ms,
|
||||
)
|
||||
|
||||
def _bearing_only_record(
|
||||
self,
|
||||
point: SamplePoint,
|
||||
frame_id: int,
|
||||
now_ms: int,
|
||||
eye: Eye,
|
||||
) -> RangeRecord:
|
||||
"""单目退化记录（§5 第 ⑤ 步）：仅方位，valid = false，无距离。"""
|
||||
detection = point.box
|
||||
return RangeRecord(
|
||||
frame_id=frame_id,
|
||||
class_id=detection.class_id,
|
||||
degradation=Degradation.BEARING_ONLY,
|
||||
valid=False,
|
||||
track_id=detection.track_id,
|
||||
center_mid_px=(detection.cx, detection.cy),
|
||||
reference_box=detection.bbox,
|
||||
reference_eye=eye,
|
||||
confidence=detection.confidence,
|
||||
timestamp_ms=now_ms,
|
||||
)
|
||||
|
||||
def _drop_invalid(self, points: Sequence[SamplePoint]) -> List[SamplePoint]:
|
||||
"""丢弃坐标非有限值的框（不可参与匹配，也不发布记录），告警节流。"""
|
||||
kept: List[SamplePoint] = []
|
||||
for point in points:
|
||||
if point.finite:
|
||||
kept.append(point)
|
||||
else:
|
||||
self._warn_emit("invalid_box", "检测框坐标非法（非有限值），已丢弃该框")
|
||||
return kept
|
||||
|
||||
def _warn_emit(self, category: str, message: str, *args: object) -> None:
|
||||
"""异常日志（§5 节流规范：第 1 次 + 每满 100 次，带累计计数）。"""
|
||||
if not self._warn.should_emit(category):
|
||||
return
|
||||
logger.warning("[%s] " + message + "（累计 %d 次）", category, *args, self._warn.count(category))
|
||||
|
||||
|
||||
def _shared_track_id(box_l: Detection, box_r: Detection) -> Optional[int]:
|
||||
"""左右目轨道关联一致且有效时返回该 id（目标隔离键优先路径，§4）。"""
|
||||
if box_l.track_id is not None and box_l.track_id > 0 and box_l.track_id == box_r.track_id:
|
||||
return box_l.track_id
|
||||
return None
|
||||
@@ -0,0 +1,119 @@
|
||||
"""目标状态隔离（§4）：track_id 优先 + 框位置最近邻兜底。
|
||||
|
||||
目标隔离键：track_id 可用（左右一致且 > 0）时优先以 (track, 类别, track_id)
|
||||
为键——上游 target_tracker 已在链路中，规格 §4 已按此冻结；否则回退类别 +
|
||||
框位置最近邻（isolation_gate_px 门限，见包 README「实现决策」）。
|
||||
|
||||
连续 filter_reset_frames 帧未再出现的目标状态整体清理（位置 + 距离滤波 +
|
||||
一致性），避免最近邻把新目标接到陈旧状态上。
|
||||
"""
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from typing import Dict, Hashable, Optional, Tuple
|
||||
|
||||
from .config import StereoConfig
|
||||
from .consistency import ConsistencyMonitor
|
||||
from .distance_filter import DistanceFilter
|
||||
|
||||
_KIND_TRACK = "track"
|
||||
_KIND_NEAREST = "nn"
|
||||
|
||||
|
||||
@dataclass
|
||||
class _TargetState:
|
||||
class_id: int
|
||||
track_id: Optional[int]
|
||||
center_px: Tuple[float, float]
|
||||
last_seen_frame: int
|
||||
|
||||
|
||||
class TargetStore:
|
||||
"""每目标状态容器：隔离键解析 + 帧末老化清理（§11.1 状态清单）。"""
|
||||
|
||||
def __init__(self, config: StereoConfig) -> None:
|
||||
self._config = config
|
||||
self._distance_filter = DistanceFilter(
|
||||
config.smooth_alpha, config.jump_reset_m, config.filter_reset_frames
|
||||
)
|
||||
self._consistency = ConsistencyMonitor(
|
||||
config.consistency_jump_px, config.consistency_N_frames, config.consistency_M_frames
|
||||
)
|
||||
self._states: Dict[Hashable, _TargetState] = {}
|
||||
self._next_nearest_id = 0
|
||||
|
||||
@property
|
||||
def distance_filter(self) -> DistanceFilter:
|
||||
return self._distance_filter
|
||||
|
||||
@property
|
||||
def consistency(self) -> ConsistencyMonitor:
|
||||
return self._consistency
|
||||
|
||||
@property
|
||||
def count(self) -> int:
|
||||
"""当前存活的目标状态数。"""
|
||||
return len(self._states)
|
||||
|
||||
def resolve(
|
||||
self,
|
||||
class_id: int,
|
||||
track_id: Optional[int],
|
||||
center_px: Tuple[float, float],
|
||||
frame_index: int,
|
||||
) -> Hashable:
|
||||
"""解析（或新建）目标 key，并刷新该目标最近位置与出现帧。"""
|
||||
if track_id is not None and track_id > 0:
|
||||
key: Hashable = (_KIND_TRACK, class_id, track_id)
|
||||
self._states[key] = _TargetState(class_id, track_id, center_px, frame_index)
|
||||
return key
|
||||
key = self._nearest_key(class_id, center_px, frame_index)
|
||||
if key is None:
|
||||
self._next_nearest_id += 1
|
||||
key = (_KIND_NEAREST, class_id, self._next_nearest_id)
|
||||
self._states[key] = _TargetState(class_id, None, center_px, frame_index)
|
||||
else:
|
||||
state = self._states[key]
|
||||
state.center_px = center_px
|
||||
state.last_seen_frame = frame_index
|
||||
return key
|
||||
|
||||
def end_frame(self, frame_index: int) -> None:
|
||||
"""帧末收尾：未出现的目标记滤波失效，到期整体清理三类状态。"""
|
||||
stale: List[Hashable] = []
|
||||
for key, state in self._states.items():
|
||||
if state.last_seen_frame == frame_index:
|
||||
continue
|
||||
self._distance_filter.invalidate(key)
|
||||
if frame_index - state.last_seen_frame >= self._config.filter_reset_frames:
|
||||
stale.append(key)
|
||||
for key in stale:
|
||||
del self._states[key]
|
||||
self._distance_filter.drop(key)
|
||||
self._consistency.drop(key)
|
||||
|
||||
def clear(self) -> None:
|
||||
self._states.clear()
|
||||
self._distance_filter.clear()
|
||||
self._consistency.clear()
|
||||
self._next_nearest_id = 0
|
||||
|
||||
def _nearest_key(
|
||||
self,
|
||||
class_id: int,
|
||||
center_px: Tuple[float, float],
|
||||
frame_index: int,
|
||||
) -> Optional[Hashable]:
|
||||
"""同类别、isolation_gate_px 内、本帧尚未被占用的最近状态。"""
|
||||
gate = self._config.isolation_gate_px
|
||||
best_key: Optional[Hashable] = None
|
||||
best_distance: Optional[float] = None
|
||||
for key, state in self._states.items():
|
||||
if key[0] != _KIND_NEAREST or state.class_id != class_id:
|
||||
continue
|
||||
if state.last_seen_frame == frame_index:
|
||||
continue # 本帧已被其他框接管，一一归属
|
||||
distance = math.hypot(center_px[0] - state.center_px[0], center_px[1] - state.center_px[1])
|
||||
if distance <= gate and (best_distance is None or distance < best_distance):
|
||||
best_key, best_distance = key, distance
|
||||
return best_key
|
||||
@@ -0,0 +1,107 @@
|
||||
"""stereo_ranger 输入输出数据结构（纯值语义）。
|
||||
|
||||
规格：docs/双目测距技术路线.md —— §2 输入输出接口、§3 符号约定、§6.6 方位源。
|
||||
|
||||
约定：所有像素坐标位于**已立体校正**的图像坐标系，检测框已映射回该基准
|
||||
（§2.1 校正契约 / 坐标基准一致）。本包仅依赖标准库。
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from enum import Enum, auto
|
||||
from typing import Optional, Sequence, Tuple
|
||||
|
||||
BBoxXYXY = Tuple[float, float, float, float]
|
||||
|
||||
|
||||
class Eye(Enum):
|
||||
"""物理目（参考目框来源，§2.2）。"""
|
||||
|
||||
LEFT = auto() # 左目（解算参考系）
|
||||
RIGHT = auto() # 右目
|
||||
|
||||
|
||||
class DisparitySource(Enum):
|
||||
"""视差取点来源（§6.1）。"""
|
||||
|
||||
LEFT_EDGE = auto() # 同侧（左）边缘：v1.0 主方案
|
||||
CENTER_FALLBACK = auto() # 边缘视差越窗后回退框中心
|
||||
|
||||
|
||||
class Degradation(Enum):
|
||||
"""记录退化标记（§2.2）。描述记录的来源形态，质量由 valid 单独表达。"""
|
||||
|
||||
COMPLETE = auto() # 完整：配对成功（距离 + 方位两路都在）
|
||||
BEARING_ONLY = auto() # 仅方位：单目检出，无距离
|
||||
|
||||
|
||||
@dataclass
|
||||
class Detection:
|
||||
"""校正后图像坐标系下的一个检测框（px；x1/y1 左上角，x2/y2 右下角）。"""
|
||||
|
||||
x1: float
|
||||
y1: float
|
||||
x2: float
|
||||
y2: float
|
||||
confidence: float = 1.0
|
||||
class_id: int = 0
|
||||
track_id: Optional[int] = None # 检测侧轨道关联；None = 无（§2.2，v1.0 可无）
|
||||
|
||||
@property
|
||||
def width(self) -> float:
|
||||
return self.x2 - self.x1
|
||||
|
||||
@property
|
||||
def height(self) -> float:
|
||||
return self.y2 - self.y1
|
||||
|
||||
@property
|
||||
def cx(self) -> float:
|
||||
return (self.x1 + self.x2) / 2.0
|
||||
|
||||
@property
|
||||
def cy(self) -> float:
|
||||
return (self.y1 + self.y2) / 2.0
|
||||
|
||||
@property
|
||||
def bbox(self) -> BBoxXYXY:
|
||||
return (self.x1, self.y1, self.x2, self.y2)
|
||||
|
||||
|
||||
@dataclass
|
||||
class DetectionFrame:
|
||||
"""单目一帧：帧标识 + 检测框列表。
|
||||
|
||||
同帧契约（§2.1）：左右两帧的 frame_id 必须一致，否则整帧拒收、输出空。
|
||||
"""
|
||||
|
||||
frame_id: int
|
||||
detections: Sequence[Detection] = field(default_factory=tuple)
|
||||
|
||||
|
||||
@dataclass
|
||||
class RangeRecord:
|
||||
"""目标距离记录（§2.2，字段与文档表格逐项对应）。
|
||||
|
||||
- forward_distance_m（Z）：唯一供距离通道消费的量；valid=False 或单目
|
||||
退化时为 None（不得强行估计，§12.3 红线 3）；
|
||||
- slant_range_m（R）：参考值，不进控制回路；无效但仍可解算时保留原始
|
||||
测量值供诊断；
|
||||
- disparity_px（D）：本次视差（调试与交叉校验用）；
|
||||
- center_mid_px（c_mid）：方位源；配对记录为两目框中心等权平均（虚拟
|
||||
双目中心，§6.6），单目退化为该目框中心；
|
||||
- reference_box / reference_eye：参考目框（配对记录恒为左目）。
|
||||
"""
|
||||
|
||||
frame_id: int
|
||||
class_id: int
|
||||
degradation: Degradation
|
||||
valid: bool
|
||||
track_id: Optional[int] = None
|
||||
forward_distance_m: Optional[float] = None
|
||||
slant_range_m: Optional[float] = None
|
||||
disparity_px: Optional[float] = None
|
||||
center_mid_px: Tuple[float, float] = (0.0, 0.0)
|
||||
reference_box: Optional[BBoxXYXY] = None
|
||||
reference_eye: Eye = Eye.LEFT
|
||||
confidence: float = 0.0
|
||||
timestamp_ms: int = 0
|
||||
@@ -0,0 +1,30 @@
|
||||
"""告警节流器（§4/§5）：每类异常"第 1 次 + 每满 100 次"打印，带累计计数。
|
||||
|
||||
高频热路径（每帧数学）不打日志；所有异常日志走本通道，且不落在数学热路径
|
||||
的决策上（§11.3 单周期处理无阻塞、无 IO）。
|
||||
"""
|
||||
|
||||
from typing import Dict
|
||||
|
||||
|
||||
class WarnThrottle:
|
||||
"""按异常类独立计数与限频。"""
|
||||
|
||||
def __init__(self, every: int = 100) -> None:
|
||||
if every < 1:
|
||||
raise ValueError("every 必须 ≥ 1")
|
||||
self._every = every
|
||||
self._counts: Dict[str, int] = {}
|
||||
|
||||
def should_emit(self, category: str) -> bool:
|
||||
"""推进该类异常计数并返回本次是否应打印（第 1 次 + 每满 every 次）。"""
|
||||
count = self._counts.get(category, 0) + 1
|
||||
self._counts[category] = count
|
||||
return count == 1 or count % self._every == 0
|
||||
|
||||
def count(self, category: str) -> int:
|
||||
"""该类异常的累计次数。"""
|
||||
return self._counts.get(category, 0)
|
||||
|
||||
def clear(self) -> None:
|
||||
self._counts.clear()
|
||||
@@ -0,0 +1,109 @@
|
||||
# stereo_yolo（Python 版）
|
||||
|
||||
双目 YOLO 检测前端：在左右目图像上**独立运行** YOLO 检测器，输出符合
|
||||
`stereo_ranger` 输入契约的检测框（每目一个 `DetectionFrame`、共用 frame_id）。
|
||||
不做测距、左右目关联与立体校正。
|
||||
|
||||
规格：`docs/双目YOLO目标识别需求说明.md`（接口契约以其为准；几何语义权威
|
||||
`docs/双目测距技术路线.md`）。
|
||||
|
||||
⚠ 全部参数为占位值、demo 素材为 AI 合成图：标定与地面真值验证完成前，
|
||||
仅限台架/回放验证，不得接入实飞控制回路（规格 §7 红线同源）。
|
||||
|
||||
## 接口清单
|
||||
|
||||
| 名称 | 说明 |
|
||||
| --- | --- |
|
||||
| `StereoYoloFrontend(config, detector=None)` | 主入口；`detector` 可注入（接口 `detect(img) -> list[Detection]`，单测用假检测器） |
|
||||
| `.detect(left_img, right_img, frame_id) -> (DetectionFrame, DetectionFrame)` | 单帧检测；BGR ndarray（cv2 约定），输出框位于输入图原生坐标系 |
|
||||
| `.close()` | 释放并发线程池 |
|
||||
| `.config` | 只读配置访问 |
|
||||
| `StereoYoloConfig` / `load_from_json(path)` | 7 项配置；缺项/未知字段/类型错误/非法取值一律 `ValueError`（fail-fast） |
|
||||
| `UltralyticsDetector` | ultralytics YOLO `.pt` 后端（双实例池；`ultralytics` 延迟导入） |
|
||||
| `split_sbs(image) -> (left, right)` | side-by-side 单流裁切（宽须为偶数） |
|
||||
| `Detection` / `DetectionFrame` | 直接复用 `stereo_ranger.types`（纯值语义，§2.1 推荐） |
|
||||
| `WarnThrottle` | 告警节流（第 1 次 + 每满 100 次，带累计计数） |
|
||||
|
||||
`logger` 名称为 `stereo_yolo`，走标准 `logging`。
|
||||
|
||||
## 单帧流程（`detect` 四步）
|
||||
|
||||
1. **形状一致性**：左右图形状不一致 → 整帧拒收输出空 + WARN 节流
|
||||
（§2.2 第 2 条：禁止左右各自缩放/填充）；
|
||||
2. **左右推理**：`parallel`（默认，ThreadPoolExecutor(2)，§5 硬性要求）或
|
||||
`sequential`（基准对照）；单目异常被隔离，该目输出空帧列表（§5 失败处理）；
|
||||
3. **出口自检 + 白名单**：丢弃非有限值 / `x2<=x1` / `y2<=y1` 的框
|
||||
（§2.2 第 6 条）；`class_whitelist` 非空时过滤类别；
|
||||
4. **封装**：每目一个 `DetectionFrame(frame_id, detections)`，`frame_id`
|
||||
左右共用（§2.3 同帧契约）。
|
||||
|
||||
## 实现决策（v1.0 冻结）
|
||||
|
||||
1. **ultralytics 直跑 `.pt`**（项目拍板口径）：letterbox、输出解码、NMS、
|
||||
框映射回原生坐标系全部由库内完成；识别端只负责阈值外置、出口自检与
|
||||
契约封装。
|
||||
2. **双实例池**：ultralytics 单实例非线程安全，左右并发各持一份模型
|
||||
（§4 并发策略的"双实例"形态）。
|
||||
3. **无跨帧状态**：前端不持有任何历史（无追踪、无平滑、无 Coast）——
|
||||
结构性满足"只送本帧原生实测框"（§2.5 第 1 条）。
|
||||
4. **形状不一致不 raise**：按"拒收 + 空帧 + 节流 WARN"处理，与 stereo_ranger
|
||||
非同帧拒收同哲学（不阻塞整体）。
|
||||
5. **几何参数单一来源**：`stereo_yolo.json` 不存内参/原分辨率（输出框天然在
|
||||
输入图坐标系），F/B/cx/cy 唯一来源是 `utils/config/stereo_ranger.json`，
|
||||
避免两处漂移（§4 配置一致性）。
|
||||
6. **`track_id` 留 `None`**：外部追踪器注入属链路后段（§2.1；v1.1 可选）。
|
||||
|
||||
## 移植注记（C++ 侧需自行复现的库内行为）
|
||||
|
||||
本次为 ultralytics 直跑口径，以下行为在库内完成、C++ 实现必须逐项复现
|
||||
（不一致会在视差取点上引入系统性偏差）：
|
||||
|
||||
| 库内行为 | C++ 需复现要点 |
|
||||
| --- | --- |
|
||||
| letterbox | 等比缩放到 `imgsz`、灰边填充（ultralytics 预测默认填充值 114）；左右同一套参数（形状一致时天然满足） |
|
||||
| 输出解码 | YOLO26 检测头输出布局与解码（建议导出一份 ONNX 作为参照物；本仓环境：ultralytics 8.4 / torch 2.11 / onnx 1.18） |
|
||||
| NMS | `conf` 先过滤、NMS 用配置 IoU 门限；`max_det` 默认 300 |
|
||||
| 坐标回映 | letterbox 逆变换（scale/pad）到原生分辨率；输出 `x1,y1,x2,y2` 浮点 |
|
||||
| 并发 | 双实例双线程（单实例并发调用不安全） |
|
||||
|
||||
## 配置项（全部外置，占位值）
|
||||
|
||||
| 键 | 占位值 | 说明 |
|
||||
| --- | --- | --- |
|
||||
| `model_path` | `models/yolo26n-drone.pt` | 相对路径按启动工作目录解析（demo 内按仓库根兜底） |
|
||||
| `imgsz` | 640 | 须为 32 的整数倍（YOLO 步长约束） |
|
||||
| `conf_threshold` | 0.25 | 识别端自身过滤口径（测距端不消费检测置信度，§2.4） |
|
||||
| `iou_threshold` | 0.7 | NMS IoU 门限 |
|
||||
| `class_whitelist` | `null` | 可选白名单；同一目标左右目类别必须一致（§2.2 第 5 条） |
|
||||
| `device` | `null` | null = 自动选择（`cpu` / `cuda:0` / …） |
|
||||
| `concurrency` | `parallel` | `parallel` / `sequential` |
|
||||
|
||||
## 测试与演示
|
||||
|
||||
```bash
|
||||
cd utils
|
||||
"D:/program file/conda/envs/pytorch/python.exe" -m pytest tests/stereo_yolo -q # 36 条
|
||||
python utils/demo_stereo_yolo.py # 真模型 + 占位素材
|
||||
```
|
||||
|
||||
单测全部用注入的假检测器驱动（无模型、无素材、无硬件）：frame_id 左右共用、
|
||||
出口自检丢弃退化框、白名单过滤、单目失败隔离、形状不一致拒收、双人屏障
|
||||
断言真并发（串行实现会屏障超时）。
|
||||
|
||||
demo（真模型 + AI 合成素材 `utils/assets/stereo_yolo/drone_sky.png`）：
|
||||
① 单图冒烟 → ② 合成双目对（同一 patch 羽化粘贴、已知视差 12 px，实测
|
||||
|ΔD|≈0.3 px、|Δy|≈0.2 px）→ 真检测框喂 `StereoRanger.update()`，记录值与
|
||||
解析值 `Z=F·B/D` 同量级 → ③ 空场景（正常空帧不告警）→ ④ 形状不一致拒收
|
||||
→ ⑤ 帧周期实测（台架开发机 ≈34 ms/帧，占位）。替换素材文件即可换真实图片。
|
||||
|
||||
## 验收对照（规格 §6）
|
||||
|
||||
| 项 | 状态 |
|
||||
| --- | --- |
|
||||
| 坐标回映误差 <0.5 px | 由库内完成，无法构件级单测；以合成对整体校验（|ΔD|≈0.3 px @1080p，含检测边缘噪声）；C++ 移植后需实测 |
|
||||
| 垂直对齐 P95 ≤1 px | 合成对实测 |Δy|≈0.2 px（合成口径，非真实校正质量）；待真实素材 |
|
||||
| 类别一致性 | 模型单类 `Drone`，合成对天然一致；待真实素材 |
|
||||
| 输出合法性 | ✓ 单测（非有限值/退化框构造用例） |
|
||||
| 输入纯净性 | ✓ 结构性保证（前端无跨帧状态） |
|
||||
| 下游对接 | ✓ demo 场景②；δZ ∝ Z² 趋势由测距端单测覆盖 |
|
||||
| 帧周期 | △ 台架开发机实测 ≈34 ms/帧（30 fps 预算 33.3 ms）；待平台实测（P7） |
|
||||
@@ -0,0 +1,20 @@
|
||||
"""stereo_yolo —— 双目 YOLO 检测前端（规格：docs/双目YOLO目标识别需求说明.md）。
|
||||
|
||||
在左右目图像上独立运行 YOLO 检测器，输出**符合 stereo_ranger 输入契约**的
|
||||
检测框列表（每目一个 DetectionFrame）；不做测距、左右目关联与立体校正。
|
||||
⚠ 全部参数为占位值：标定与地面验证完成前仅限台架/回放验证，不得接入实飞
|
||||
控制回路（需求说明 §7 红线同源）。
|
||||
"""
|
||||
|
||||
from .config import StereoYoloConfig, load_from_json
|
||||
from .detector import UltralyticsDetector
|
||||
from .frontend import StereoYoloFrontend
|
||||
from .sbs import split_sbs
|
||||
|
||||
__all__ = [
|
||||
"StereoYoloConfig",
|
||||
"StereoYoloFrontend",
|
||||
"UltralyticsDetector",
|
||||
"load_from_json",
|
||||
"split_sbs",
|
||||
]
|
||||
@@ -0,0 +1,86 @@
|
||||
"""StereoYoloConfig 与 JSON 配置加载（需求说明 §4 配置项需求）。
|
||||
|
||||
全部配置外置、构造期一次加载并校验：缺项、未知字段、类型错误、非法取值
|
||||
一律拒绝进入运行期（fail-fast）。几何参数（F/B/cx/cy/原分辨率）不在此
|
||||
重复存储——输出框位于**输入图像自身的原生坐标系**，几何参数的唯一来源
|
||||
是 utils/config/stereo_ranger.json（§4 配置一致性要求：同一配置源驱动，
|
||||
杜绝两处漂移）。
|
||||
"""
|
||||
|
||||
import json
|
||||
from dataclasses import dataclass, fields
|
||||
from typing import Any, Dict, Optional, Tuple
|
||||
|
||||
_CONCURRENCY_MODES = ("parallel", "sequential")
|
||||
_INT_FIELDS = frozenset({"imgsz"})
|
||||
_STR_FIELDS = frozenset({"model_path", "concurrency"})
|
||||
|
||||
|
||||
@dataclass
|
||||
class StereoYoloConfig:
|
||||
model_path: str = "models/yolo26n-drone.pt"
|
||||
imgsz: int = 640 # 左右共用（§2.2 第 2 条：同一缩放、同一填充）
|
||||
conf_threshold: float = 0.25 # ⚠占位：识别端自身过滤口径（§2.4）
|
||||
iou_threshold: float = 0.7 # ⚠占位：NMS IoU 门限
|
||||
class_whitelist: Optional[Tuple[int, ...]] = None # None = 不过滤（§4 可选白名单）
|
||||
device: Optional[str] = None # None = 自动选择（cpu / cuda:0 / …）
|
||||
concurrency: str = "parallel" # parallel = 左右并发（§5 硬性要求，串行会吃掉全部预算）
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
_require(bool(self.model_path.strip()), "model_path", "不得为空")
|
||||
_require(
|
||||
self.imgsz >= 32 and self.imgsz % 32 == 0,
|
||||
"imgsz",
|
||||
"须为 32 的整数倍且 ≥ 32（YOLO 步长约束）",
|
||||
)
|
||||
_require(0.0 <= self.conf_threshold <= 1.0, "conf_threshold", "须落在 [0, 1]")
|
||||
_require(0.0 < self.iou_threshold <= 1.0, "iou_threshold", "须落在 (0, 1]")
|
||||
_require(self.concurrency in _CONCURRENCY_MODES, "concurrency", f"须为 {_CONCURRENCY_MODES} 之一")
|
||||
_require(self.device is None or bool(self.device.strip()), "device", "须为非空字符串或 null")
|
||||
if self.class_whitelist is not None:
|
||||
ids = tuple(self.class_whitelist)
|
||||
_require(
|
||||
len(ids) > 0 and all(isinstance(item, int) and not isinstance(item, bool) and item >= 0 for item in ids),
|
||||
"class_whitelist",
|
||||
"须为非空非负整数列表或 null",
|
||||
)
|
||||
self.class_whitelist = ids
|
||||
|
||||
|
||||
def load_from_json(path: str) -> StereoYoloConfig:
|
||||
"""从 JSON 文件构建配置；缺项/未知字段/类型错误/非法取值一律抛 ValueError。"""
|
||||
with open(path, encoding="utf-8") as handle:
|
||||
data: Dict[str, Any] = json.load(handle)
|
||||
if not isinstance(data, dict):
|
||||
raise ValueError("配置非法: 顶层须为 JSON 对象")
|
||||
known = {item.name for item in fields(StereoYoloConfig)}
|
||||
unknown = sorted(set(data) - known)
|
||||
if unknown:
|
||||
raise ValueError(f"未知配置字段: {unknown}")
|
||||
missing = sorted(known - set(data))
|
||||
if missing:
|
||||
raise ValueError(f"缺少配置字段: {missing}")
|
||||
for name, value in data.items():
|
||||
if name == "class_whitelist":
|
||||
if value is not None and not (
|
||||
isinstance(value, list)
|
||||
and all(isinstance(item, int) and not isinstance(item, bool) for item in value)
|
||||
):
|
||||
raise ValueError("配置类型非法: class_whitelist 须为整数数组或 null")
|
||||
elif name == "device":
|
||||
if value is not None and not isinstance(value, str):
|
||||
raise ValueError("配置类型非法: device 须为字符串或 null")
|
||||
elif name in _STR_FIELDS:
|
||||
if not isinstance(value, str):
|
||||
raise ValueError(f"配置类型非法: {name} 须为字符串")
|
||||
elif name in _INT_FIELDS:
|
||||
if not isinstance(value, int) or isinstance(value, bool):
|
||||
raise ValueError(f"配置类型非法: {name} 须为整数")
|
||||
elif isinstance(value, bool) or not isinstance(value, (int, float)):
|
||||
raise ValueError(f"配置类型非法: {name} 须为数值")
|
||||
return StereoYoloConfig(**data)
|
||||
|
||||
|
||||
def _require(condition: bool, field_name: str, message: str) -> None:
|
||||
if not condition:
|
||||
raise ValueError(f"配置非法: {field_name} {message}")
|
||||
@@ -0,0 +1,78 @@
|
||||
"""UltralyticsDetector —— ultralytics YOLO .pt 直载的检测后端。
|
||||
|
||||
⚠ 移植注记（C++ 侧需自行复现，详见 README）：letterbox（缩放 + 填充到
|
||||
imgsz）、输出张量解码、NMS、框映射回输入图原生坐标系，本例由 ultralytics
|
||||
库内完成；移植时必须逐项复现同一几何（步长对齐、填充方式、IoU 门限、
|
||||
原生坐标回映），否则视差取点会引入系统性偏差。
|
||||
"""
|
||||
|
||||
import queue
|
||||
from pathlib import Path
|
||||
from typing import List
|
||||
|
||||
from stereo_ranger.types import Detection
|
||||
|
||||
from .config import StereoYoloConfig
|
||||
|
||||
|
||||
class UltralyticsDetector:
|
||||
"""双实例池：ultralytics 单实例非线程安全，左右并发各持一份（§3/§4）。
|
||||
|
||||
接口约定（可注入替换）：``detect(image) -> list[Detection]``，
|
||||
image 为 BGR ndarray（cv2 约定），返回框位于该图原生坐标系。
|
||||
"""
|
||||
|
||||
def __init__(self, config: StereoYoloConfig) -> None:
|
||||
self._cfg = config
|
||||
self._model_path = _resolve_model_path(config.model_path)
|
||||
self._pool: "queue.SimpleQueue" = queue.SimpleQueue()
|
||||
from ultralytics import YOLO # 延迟导入：仅真实推理付加载成本，单测注入假检测器（§6）
|
||||
|
||||
for _ in range(2):
|
||||
self._pool.put(YOLO(str(self._model_path)))
|
||||
|
||||
def detect(self, image) -> List[Detection]:
|
||||
model = self._pool.get()
|
||||
try:
|
||||
results = model.predict(
|
||||
source=image,
|
||||
imgsz=self._cfg.imgsz,
|
||||
conf=self._cfg.conf_threshold,
|
||||
iou=self._cfg.iou_threshold,
|
||||
device=self._cfg.device,
|
||||
verbose=False,
|
||||
)
|
||||
finally:
|
||||
self._pool.put(model)
|
||||
if not results:
|
||||
return []
|
||||
return _result_to_detections(results[0])
|
||||
|
||||
|
||||
def _result_to_detections(result) -> List[Detection]:
|
||||
boxes = getattr(result, "boxes", None)
|
||||
if boxes is None or len(boxes) == 0:
|
||||
return []
|
||||
xyxy = boxes.xyxy.cpu().numpy()
|
||||
conf = boxes.conf.cpu().numpy()
|
||||
cls = boxes.cls.cpu().numpy()
|
||||
return [
|
||||
Detection(
|
||||
float(box[0]),
|
||||
float(box[1]),
|
||||
float(box[2]),
|
||||
float(box[3]),
|
||||
confidence=float(score),
|
||||
class_id=int(class_id),
|
||||
)
|
||||
for box, score, class_id in zip(xyxy, conf, cls)
|
||||
]
|
||||
|
||||
|
||||
def _resolve_model_path(raw: str) -> Path:
|
||||
path = Path(raw).expanduser()
|
||||
if not path.is_absolute():
|
||||
path = Path.cwd() / path
|
||||
if not path.is_file():
|
||||
raise FileNotFoundError(f"模型权重不存在: {path}（model_path 相对路径按启动工作目录解析）")
|
||||
return path.resolve()
|
||||
@@ -0,0 +1,113 @@
|
||||
"""StereoYoloFrontend —— 双目检测前端主类（需求说明 §2/§3）。
|
||||
|
||||
单帧流程：左右图（已校正、原生分辨率）→ 左右并发检测 → 出口自检 + 类别
|
||||
白名单 → 每目封装 DetectionFrame（共用 frame_id，§2.3 同帧契约）。
|
||||
|
||||
前端**无跨帧状态**：没有历史就没有外推，结构性满足"只送本帧原生实测框"
|
||||
的输入纯净性硬约束（§2.5 第 1 条）。
|
||||
"""
|
||||
|
||||
import logging
|
||||
import math
|
||||
from concurrent.futures import ThreadPoolExecutor
|
||||
from typing import List, Optional, Sequence, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
from stereo_ranger.types import Detection, DetectionFrame
|
||||
|
||||
from .config import StereoYoloConfig
|
||||
from .detector import UltralyticsDetector
|
||||
from .warn_throttle import WarnThrottle
|
||||
|
||||
logger = logging.getLogger("stereo_yolo")
|
||||
|
||||
_EYE_LABELS = ("左目", "右目")
|
||||
|
||||
|
||||
class StereoYoloFrontend:
|
||||
"""图像 → 符合 stereo_ranger 输入契约的检测框（§2 接口契约）。
|
||||
|
||||
``detector`` 可注入替换：接口为 ``detect(img) -> list[Detection]``
|
||||
（单测用假检测器驱动，§6 输出合法性/失败隔离用例无需真实模型）。
|
||||
"""
|
||||
|
||||
def __init__(self, config: StereoYoloConfig, detector: Optional[object] = None) -> None:
|
||||
self._cfg = config
|
||||
self._detector = detector if detector is not None else UltralyticsDetector(config)
|
||||
self._throttle = WarnThrottle()
|
||||
self._executor = (
|
||||
ThreadPoolExecutor(max_workers=2, thread_name_prefix="stereo_yolo")
|
||||
if config.concurrency == "parallel"
|
||||
else None
|
||||
)
|
||||
|
||||
@property
|
||||
def config(self) -> StereoYoloConfig:
|
||||
return self._cfg
|
||||
|
||||
def detect(
|
||||
self,
|
||||
left_img: np.ndarray,
|
||||
right_img: np.ndarray,
|
||||
frame_id: int,
|
||||
) -> Tuple[DetectionFrame, DetectionFrame]:
|
||||
"""左右各输出一个 DetectionFrame；两帧共用同一 frame_id（§2.3）。"""
|
||||
if left_img.shape != right_img.shape:
|
||||
self._warn(
|
||||
"shape_mismatch",
|
||||
f"左右图尺寸不一致（{left_img.shape} vs {right_img.shape}），整帧拒收输出空（§2.2 第 2 条）",
|
||||
)
|
||||
return DetectionFrame(frame_id, ()), DetectionFrame(frame_id, ())
|
||||
|
||||
if self._executor is not None:
|
||||
left_future = self._executor.submit(self._detect_eye, left_img, _EYE_LABELS[0])
|
||||
right_future = self._executor.submit(self._detect_eye, right_img, _EYE_LABELS[1])
|
||||
left, right = left_future.result(), right_future.result()
|
||||
else:
|
||||
left = self._detect_eye(left_img, _EYE_LABELS[0])
|
||||
right = self._detect_eye(right_img, _EYE_LABELS[1])
|
||||
|
||||
return DetectionFrame(frame_id, tuple(left)), DetectionFrame(frame_id, tuple(right))
|
||||
|
||||
def close(self) -> None:
|
||||
"""释放并发线程池（前端不再使用时调用）。"""
|
||||
if self._executor is not None:
|
||||
self._executor.shutdown(wait=True)
|
||||
|
||||
# ---------------- 内部 ----------------
|
||||
|
||||
def _detect_eye(self, image: np.ndarray, eye_label: str) -> List[Detection]:
|
||||
try:
|
||||
raw = self._detector.detect(image)
|
||||
except Exception as exc: # 单目失败隔离：不阻塞整体，该目输出空帧列表（§5 失败处理）
|
||||
self._warn(f"infer_failed:{eye_label}", f"{eye_label}推理失败，该目输出空帧列表：{exc!r}")
|
||||
return []
|
||||
return self._sanitize(raw)
|
||||
|
||||
def _sanitize(self, detections: Sequence[Detection]) -> List[Detection]:
|
||||
"""出口自检（§2.2 第 6 条）：丢弃非有限值 / x2<=x1 / y2<=y1 的框；白名单过滤。"""
|
||||
kept: List[Detection] = []
|
||||
dropped = 0
|
||||
whitelist = self._cfg.class_whitelist
|
||||
for detection in detections:
|
||||
if not _is_valid_box(detection):
|
||||
dropped += 1
|
||||
continue
|
||||
if whitelist is not None and detection.class_id not in whitelist:
|
||||
continue
|
||||
kept.append(detection)
|
||||
if dropped:
|
||||
self._warn("degenerate_box", f"丢弃退化检测框 {dropped} 个（非有限值或 x2<=x1/y2<=y1）")
|
||||
return kept
|
||||
|
||||
def _warn(self, category: str, message: str) -> None:
|
||||
if self._throttle.should_emit(category):
|
||||
logger.warning("%s（累计 %d 次）", message, self._throttle.count(category))
|
||||
|
||||
|
||||
def _is_valid_box(detection: Detection) -> bool:
|
||||
values = (detection.x1, detection.y1, detection.x2, detection.y2)
|
||||
if not all(math.isfinite(value) for value in values):
|
||||
return False
|
||||
return detection.x2 > detection.x1 and detection.y2 > detection.y1
|
||||
@@ -0,0 +1,16 @@
|
||||
"""side-by-side 单流裁切（需求说明 §2.3：同帧拼接形态推荐优先支持）。
|
||||
|
||||
解码一次、裁切为左右图，天然同曝光、天然同帧；宽须为偶数。
|
||||
"""
|
||||
|
||||
from typing import Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def split_sbs(image: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
|
||||
"""左右各半裁切，返回 (左图, 右图)。"""
|
||||
if image.ndim < 2 or image.shape[1] % 2 != 0:
|
||||
raise ValueError(f"side-by-side 图像宽度须为偶数，实际 shape={image.shape}")
|
||||
half = int(image.shape[1] // 2)
|
||||
return image[:, :half].copy(), image[:, half:].copy()
|
||||
@@ -0,0 +1,26 @@
|
||||
"""告警节流器（需求说明 §5 失败处理）：每类异常"第 1 次 + 每满 100 次"打印。
|
||||
|
||||
语义与 stereo_ranger 的节流器一致，但按前端/测距解耦要求（§7）不跨包复用。
|
||||
"""
|
||||
|
||||
from typing import Dict
|
||||
|
||||
|
||||
class WarnThrottle:
|
||||
"""按异常类独立计数与限频。"""
|
||||
|
||||
def __init__(self, every: int = 100) -> None:
|
||||
if every < 1:
|
||||
raise ValueError("every 必须 ≥ 1")
|
||||
self._every = every
|
||||
self._counts: Dict[str, int] = {}
|
||||
|
||||
def should_emit(self, category: str) -> bool:
|
||||
"""推进该类异常计数并返回本次是否应打印（第 1 次 + 每满 every 次）。"""
|
||||
count = self._counts.get(category, 0) + 1
|
||||
self._counts[category] = count
|
||||
return count == 1 or count % self._every == 0
|
||||
|
||||
def count(self, category: str) -> int:
|
||||
"""该类异常的累计次数。"""
|
||||
return self._counts.get(category, 0)
|
||||
@@ -0,0 +1,29 @@
|
||||
# target_tracker（Python 版）
|
||||
|
||||
多目标追踪过滤器：8 维恒速卡尔曼滤波 + 贪婪关联 + 主目标锁定选举。
|
||||
移植自 `reference-c++/target_tracker/`（规格：docs/视觉追踪技术路线.md）。
|
||||
|
||||
## 接口清单
|
||||
|
||||
| 名称 | 说明 |
|
||||
| --- | --- |
|
||||
| `Detection(x1, y1, x2, y2, conf, cls_id=0)` | 单帧检测框输入 |
|
||||
| `FrameShape(height, width)` | 画面尺寸（仅 Coast 兜底选举取画面中心用） |
|
||||
| `TrackResult` | 每帧输出九字段：tracked / primary_id / center / box / confidence / is_predicted / lost_frames / n_active / raw |
|
||||
| `TrackerConfig(max_lost_frames=8, max_association_dist=200.0, min_hits=3, dist_weight=0.5, iou_weight=0.5)` | 追踪器配置 |
|
||||
| `TargetTracker(config=None)` | 追踪器主类 |
|
||||
| `.update(detections, frame_shape=None) -> TrackResult` | 单帧主流程（§7 六步） |
|
||||
| `.last_result() -> TrackResult` | 跨线程读取最近一帧结果（加锁拷贝） |
|
||||
| `.reset()` / `.active_count()` / `.active_tracks()` / `.get_config()` | 生命周期与查询 |
|
||||
| `KalmanBoxFilter` / `BoxCXCYWH` | 单目标滤波器（可独立使用：initiate / predict / update） |
|
||||
| `Tracklet` / `BoxXYXY` / `to_center_size` / `to_xyxy` / `compute_iou` | 轨道与几何工具 |
|
||||
|
||||
## 与 C++ 版的差异
|
||||
|
||||
- **依赖**：矩阵运算用 numpy（C++ 手写定长运算避免 Eigen）；关联/选举路径用纯标量，与 C++ 逐位同序。
|
||||
- **Optional 语义**：C++ `std::optional<T>` → Python `None`；`State()` → `state` 属性（返回拷贝）。
|
||||
- **线程模型**：与 C++ 一致——仅 `last_result()` 加 `threading.Lock`，`update()` 单线程调用。
|
||||
- **字段命名**：成员函数蛇形命名（`IsDead` → `is_dead` 属性，`MarkMiss` → `mark_miss`）。
|
||||
- 行为语义（死亡判定 `lost_count > max_lost` 严格大于、best_cost=1.0 严格小于、
|
||||
ID 严格递增不复用、Q 用状态原值 / R 用 abs×2 的刻意不对称、S 求逆前 1e-9 正则化）
|
||||
与 C++ 完全一致，由 27 条移植测试锁定。
|
||||
@@ -0,0 +1,15 @@
|
||||
"""target_tracker —— 轻量级多目标追踪过滤器（C++ 版 reference-c++/target_tracker 的 Python 移植）。
|
||||
|
||||
纯算法包，零第三方运行依赖之外的约束：仅依赖 numpy，不依赖 visual_track_control。
|
||||
"""
|
||||
|
||||
from .kalman_box_filter import STD_POS, STD_VEL, INI_POS, INI_VEL, R_SCALE, BoxCXCYWH, KalmanBoxFilter
|
||||
from .target_tracker import Detection, FrameShape, TargetTracker, TrackerConfig, TrackResult
|
||||
from .tracklet import BoxXYXY, Tracklet, compute_iou, to_center_size, to_xyxy
|
||||
|
||||
__all__ = [
|
||||
"STD_POS", "STD_VEL", "INI_POS", "INI_VEL", "R_SCALE",
|
||||
"BoxCXCYWH", "BoxXYXY", "KalmanBoxFilter", "Tracklet",
|
||||
"Detection", "FrameShape", "TargetTracker", "TrackerConfig", "TrackResult",
|
||||
"compute_iou", "to_center_size", "to_xyxy",
|
||||
]
|
||||
@@ -0,0 +1,111 @@
|
||||
"""单目标 8 维恒定速度卡尔曼滤波器。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/target_tracker（规格：docs/视觉追踪技术路线.md §4）：
|
||||
状态 x[8] = [cx, cy, w, h, vx, vy, vw, vh]，观测 z[4] = [cx, cy, w, h]，dt = 1 帧。
|
||||
矩阵运算用 numpy 表达（C++ 版为避免 Eigen 手写定长运算，此处等价）。
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
# §4.2 / §8.2 内部常量（经验值，针对 640×480 画面调优）
|
||||
STD_POS = 1.0 / 50.0 # 位置噪声比例系数
|
||||
STD_VEL = 1.0 / 200.0 # 速度噪声比例系数
|
||||
INI_POS = 2.0 # 初始位置协方差
|
||||
INI_VEL = 100.0 # 初始速度协方差
|
||||
R_SCALE = 2.0 # R 额外缩放因子（增强平滑）
|
||||
_S_REG = 1e-9 # S 求逆前的对角正则化（§9.3）
|
||||
|
||||
# 状态转移矩阵 F（§4.1）：I4 + 右上 I4
|
||||
_F = np.eye(8, dtype=float)
|
||||
_F[0:4, 4:8] = np.eye(4)
|
||||
|
||||
# 观测矩阵 H（§4.1）：[I4 | 0]，直接观测位置与尺寸、不观测速度
|
||||
_H_COLS = slice(0, 4)
|
||||
|
||||
|
||||
@dataclass
|
||||
class BoxCXCYWH:
|
||||
"""轴对齐边界框（中心点 + 宽高表示，卡尔曼观测空间）。"""
|
||||
|
||||
cx: float = 0.0
|
||||
cy: float = 0.0
|
||||
w: float = 0.0
|
||||
h: float = 0.0
|
||||
|
||||
|
||||
class KalmanBoxFilter:
|
||||
def __init__(self) -> None:
|
||||
self._x = np.zeros(8, dtype=float)
|
||||
self._p = np.zeros((8, 8), dtype=float)
|
||||
self._initialized = False
|
||||
|
||||
@property
|
||||
def initialized(self) -> bool:
|
||||
return self._initialized
|
||||
|
||||
@property
|
||||
def state(self) -> np.ndarray:
|
||||
"""状态向量 [cx, cy, w, h, vx, vy, vw, vh]（拷贝）。"""
|
||||
return self._x.copy()
|
||||
|
||||
def initiate(self, z: BoxCXCYWH) -> None:
|
||||
# §4.3：速度置零；P = diag(2,2,2,2,100,100,100,100)
|
||||
self._x = np.array([z.cx, z.cy, z.w, z.h, 0.0, 0.0, 0.0, 0.0])
|
||||
self._p = np.diag([INI_POS] * 4 + [INI_VEL] * 4)
|
||||
self._initialized = True
|
||||
|
||||
def predict(self) -> BoxCXCYWH | None:
|
||||
# §4.4：未初始化返回 None
|
||||
if not self._initialized:
|
||||
return None
|
||||
|
||||
# §4.2 动态过程噪声：Q = diag(σ_pos², σ_vel²)，σ 按状态中 w/h 缩放。
|
||||
# Q 直接使用状态值、不做 abs，与 R 的 abs 处理刻意不同（规格 §4.2 注记）。
|
||||
w = self._x[2]
|
||||
h = self._x[3]
|
||||
std_pos = np.array([w * STD_POS, h * STD_POS, w * STD_POS, h * STD_POS])
|
||||
std_vel = np.array([w * STD_VEL, h * STD_VEL, w * STD_VEL, h * STD_VEL])
|
||||
q = np.zeros((8, 8), dtype=float)
|
||||
q[np.arange(4), np.arange(4)] = std_pos * std_pos
|
||||
q[np.arange(4) + 4, np.arange(4) + 4] = std_vel * std_vel
|
||||
|
||||
# §4.4：x ← F·x；P ← F·P·Fᵀ + Q
|
||||
self._x = _F @ self._x
|
||||
self._p = _F @ self._p @ _F.T + q
|
||||
return BoxCXCYWH(float(self._x[0]), float(self._x[1]),
|
||||
float(self._x[2]), float(self._x[3]))
|
||||
|
||||
def update(self, z: BoxCXCYWH) -> None:
|
||||
# §4.5：未初始化直接返回
|
||||
if not self._initialized:
|
||||
return
|
||||
|
||||
# §4.2 动态观测噪声：R 用 abs(w)/abs(h) 缩放并额外乘 R_SCALE
|
||||
w = abs(self._x[2])
|
||||
h = abs(self._x[3])
|
||||
rw = R_SCALE * (w * STD_POS) * (w * STD_POS)
|
||||
rh = R_SCALE * (h * STD_POS) * (h * STD_POS)
|
||||
r = np.diag([rw, rh, rw, rh])
|
||||
|
||||
# §4.5：S = H·P·Hᵀ + R。H = [I4|0]，故 H·P·Hᵀ 即 P 左上 4×4 子块
|
||||
s = self._p[:4, :4] + r
|
||||
try:
|
||||
s_inv = np.linalg.inv(s + _S_REG * np.eye(4))
|
||||
except np.linalg.LinAlgError:
|
||||
return # 求逆失败：防御性跳过本次更新（C++ 版同样跳过）
|
||||
|
||||
# §4.5：K = P·Hᵀ·S⁻¹，P·Hᵀ 即 P 的前 4 列（8×4）
|
||||
k = self._p[:, _H_COLS] @ s_inv
|
||||
|
||||
# 残差 y = z − H·x（H·x 即状态前 4 维）
|
||||
y = np.array([z.cx, z.cy, z.w, z.h]) - self._x[:4]
|
||||
|
||||
# x ← x + K·y
|
||||
self._x = self._x + k @ y
|
||||
|
||||
# P ← (I8 − K·H)·P：K·H 是 K 的前 4 列铺在前 4 列上
|
||||
kh = np.zeros((8, 8), dtype=float)
|
||||
kh[:, :4] = k
|
||||
self._p = (np.eye(8) - kh) @ self._p
|
||||
@@ -0,0 +1,269 @@
|
||||
"""多目标追踪过滤器（TargetTracker）。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/target_tracker/src/target_tracker.cpp
|
||||
（规格：docs/视觉追踪技术路线.md §5 关联 / §6 生命周期与锁定 / §7 六步主流程）。
|
||||
|
||||
线程模型（§9.5）：update() 由单线程（推理主循环）调用；仅对 last_result
|
||||
缓存加锁，last_result() 返回受保护拷贝。
|
||||
"""
|
||||
|
||||
import math
|
||||
import threading
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Optional, Sequence
|
||||
|
||||
from .kalman_box_filter import BoxCXCYWH
|
||||
from .tracklet import BoxXYXY, Tracklet, compute_iou, to_center_size
|
||||
|
||||
|
||||
@dataclass
|
||||
class Detection:
|
||||
"""单个检测框（§2.2 输入元素：[x1,y1,x2,y2,conf,cls_id]）。"""
|
||||
|
||||
x1: float = 0.0
|
||||
y1: float = 0.0
|
||||
x2: float = 0.0
|
||||
y2: float = 0.0
|
||||
conf: float = 0.0
|
||||
cls_id: int = 0
|
||||
|
||||
|
||||
@dataclass
|
||||
class FrameShape:
|
||||
"""画面尺寸（height, width）：仅 Coast 兜底选举时用于取画面中心。"""
|
||||
|
||||
height: int = 0
|
||||
width: int = 0
|
||||
|
||||
|
||||
@dataclass
|
||||
class TrackResult:
|
||||
"""每帧追踪输出（§2.3 九字段）。"""
|
||||
|
||||
tracked: bool = False
|
||||
primary_id: Optional[int] = None
|
||||
center: tuple[float, float] = (0.0, 0.0)
|
||||
box: Optional[BoxXYXY] = None
|
||||
confidence: float = 0.0
|
||||
is_predicted: bool = False
|
||||
lost_frames: int = 0
|
||||
n_active: int = 0
|
||||
raw: Optional[BoxXYXY] = None
|
||||
|
||||
|
||||
@dataclass
|
||||
class TrackerConfig:
|
||||
"""追踪器配置参数（§8.1 默认值取自规格）。"""
|
||||
|
||||
max_lost_frames: int = 8
|
||||
max_association_dist: float = 200.0
|
||||
min_hits: int = 3
|
||||
dist_weight: float = 0.5
|
||||
iou_weight: float = 0.5
|
||||
|
||||
|
||||
@dataclass
|
||||
class _AssociationResult:
|
||||
matches: list[tuple[int, int]] = field(default_factory=list)
|
||||
unmatched_track_ids: list[int] = field(default_factory=list)
|
||||
unmatched_det_indices: list[int] = field(default_factory=list)
|
||||
|
||||
|
||||
class TargetTracker:
|
||||
def __init__(self, config: Optional[TrackerConfig] = None) -> None:
|
||||
self._config = config if config is not None else TrackerConfig()
|
||||
self._next_id = 0
|
||||
self._tracks: dict[int, Tracklet] = {}
|
||||
self._frame_count = 0
|
||||
self._locked_primary_id: Optional[int] = None
|
||||
self._last_result = TrackResult()
|
||||
self._last_result_mutex = threading.Lock()
|
||||
|
||||
@property
|
||||
def config(self) -> TrackerConfig:
|
||||
return self._config
|
||||
|
||||
def get_config(self) -> TrackerConfig:
|
||||
return self._config
|
||||
|
||||
def update(self, detections: Sequence[Detection],
|
||||
frame_shape: Optional[FrameShape] = None) -> TrackResult:
|
||||
# §7 六步主流程
|
||||
self._frame_count += 1
|
||||
|
||||
# 入口防御：过滤坐标或置信度含 NaN/Inf 的检测（非有限值一旦进入
|
||||
# 卡尔曼状态将永久污染轨道并传导到主目标输出）
|
||||
valid_detections = [
|
||||
det for det in detections
|
||||
if all(math.isfinite(v) for v in (det.x1, det.y1, det.x2, det.y2, det.conf))
|
||||
]
|
||||
|
||||
# 步骤 1：所有存活轨道先行预测（推进一帧）
|
||||
for track in self._tracks.values():
|
||||
track.predict()
|
||||
|
||||
# 步骤 2：贪婪关联（无检测时全部轨道进入 Coast 计数）
|
||||
if valid_detections:
|
||||
assoc = self._associate_detections(valid_detections)
|
||||
else:
|
||||
assoc = _AssociationResult(unmatched_track_ids=list(self._tracks.keys()))
|
||||
|
||||
# 步骤 3：用匹配到的检测更新轨道
|
||||
for track_id, det_idx in assoc.matches:
|
||||
det = valid_detections[det_idx]
|
||||
raw_box = BoxXYXY(det.x1, det.y1, det.x2, det.y2)
|
||||
self._tracks[track_id].update(raw_box, det.conf, det.cls_id)
|
||||
|
||||
# 步骤 4：未匹配轨道计丢失，死亡者回收（锁定唯一解除途径之一）
|
||||
for track_id in assoc.unmatched_track_ids:
|
||||
track = self._tracks.get(track_id)
|
||||
if track is None:
|
||||
continue
|
||||
track.mark_miss()
|
||||
if track.is_dead:
|
||||
if self._locked_primary_id == track_id:
|
||||
self._locked_primary_id = None
|
||||
del self._tracks[track_id]
|
||||
|
||||
# 步骤 5：未匹配检测 → 创建新轨道（next_id 从 0 递增永不复用）
|
||||
for det_idx in assoc.unmatched_det_indices:
|
||||
det = valid_detections[det_idx]
|
||||
measurement = to_center_size(BoxXYXY(det.x1, det.y1, det.x2, det.y2))
|
||||
self._tracks[self._next_id] = Tracklet(
|
||||
self._next_id, measurement, det.conf, det.cls_id,
|
||||
self._config.max_lost_frames)
|
||||
self._next_id += 1
|
||||
|
||||
# 步骤 6：选取/延续主目标，组装输出并缓存
|
||||
result = self._select_primary(frame_shape)
|
||||
with self._last_result_mutex:
|
||||
self._last_result = result
|
||||
return result
|
||||
|
||||
def last_result(self) -> TrackResult:
|
||||
with self._last_result_mutex:
|
||||
return self._last_result
|
||||
|
||||
def reset(self) -> None:
|
||||
# §2.1：清空所有轨道、ID 计数器、锁定状态与缓存结果（next_id 归零）
|
||||
self._tracks.clear()
|
||||
self._next_id = 0
|
||||
self._frame_count = 0
|
||||
self._locked_primary_id = None
|
||||
with self._last_result_mutex:
|
||||
self._last_result = TrackResult()
|
||||
|
||||
def active_count(self) -> int:
|
||||
return len(self._tracks)
|
||||
|
||||
def active_tracks(self) -> dict[int, Tracklet]:
|
||||
return dict(self._tracks)
|
||||
|
||||
def _associate_detections(self, detections: Sequence[Detection]) -> _AssociationResult:
|
||||
result = _AssociationResult()
|
||||
|
||||
# §5.3：按轨道 confidence 降序（平局按 ID 升序保证确定性）。
|
||||
# 非有限置信度按 −∞ 处理（排最后），避免 NaN 破坏全序。
|
||||
def sort_key(track_id: int) -> tuple[float, int]:
|
||||
raw = self._tracks[track_id].confidence
|
||||
conf = raw if math.isfinite(raw) else -math.inf
|
||||
return (-conf, track_id)
|
||||
|
||||
sorted_ids = sorted(self._tracks.keys(), key=sort_key)
|
||||
det_assigned = [False] * len(detections)
|
||||
|
||||
for track_id in sorted_ids:
|
||||
track = self._tracks[track_id]
|
||||
# §5.3：最优代价初始 1.0，仅 cost 严格小于才匹配（cost ≥ 1.0 判为不可匹配）
|
||||
best_cost = 1.0
|
||||
best_det = -1
|
||||
for d, det in enumerate(detections):
|
||||
if det_assigned[d]:
|
||||
continue
|
||||
det_cx = (det.x1 + det.x2) / 2.0
|
||||
det_cy = (det.y1 + det.y2) / 2.0
|
||||
dist = math.hypot(track.last_center[0] - det_cx,
|
||||
track.last_center[1] - det_cy)
|
||||
norm_dist = dist / max(self._config.max_association_dist, 1e-3)
|
||||
iou = (compute_iou(track.last_box,
|
||||
BoxXYXY(det.x1, det.y1, det.x2, det.y2))
|
||||
if track.last_box is not None else 0.0)
|
||||
cost = (self._config.dist_weight * norm_dist +
|
||||
self._config.iou_weight * (1.0 - iou))
|
||||
if cost < best_cost:
|
||||
best_cost = cost
|
||||
best_det = d
|
||||
if best_det >= 0:
|
||||
result.matches.append((track_id, best_det))
|
||||
det_assigned[best_det] = True
|
||||
else:
|
||||
result.unmatched_track_ids.append(track_id)
|
||||
|
||||
result.unmatched_det_indices = [
|
||||
d for d, assigned in enumerate(det_assigned) if not assigned
|
||||
]
|
||||
return result
|
||||
|
||||
def _select_primary(self, frame_shape: Optional[FrameShape]) -> TrackResult:
|
||||
result = TrackResult(n_active=len(self._tracks))
|
||||
|
||||
# §6.3：轨道表为空 → 清除锁定，返回 tracked=false
|
||||
if not self._tracks:
|
||||
self._locked_primary_id = None
|
||||
return result
|
||||
|
||||
# §6.3 ① 锁定延续：锁定轨道存活则直接输出（Coast 中照样输出，
|
||||
# 不被高置信度目标抢占）
|
||||
chosen: Optional[Tracklet] = None
|
||||
is_predicted = False
|
||||
if self._locked_primary_id is not None:
|
||||
track = self._tracks.get(self._locked_primary_id)
|
||||
if track is not None:
|
||||
chosen = track
|
||||
is_predicted = track.is_coasting
|
||||
|
||||
# §6.3 ② 重新选举（无锁定 / 锁定目标已死亡被回收）
|
||||
if chosen is None:
|
||||
measured = [t for t in self._tracks.values() if not t.is_coasting]
|
||||
coasting = [t for t in self._tracks.values() if t.is_coasting]
|
||||
|
||||
if measured:
|
||||
# confirmed ← measured 中 hits ≥ min_hits 者；为空则退回 measured
|
||||
# （min_hits 只影响候选池优先级，不阻止输出）
|
||||
pool = [t for t in measured if t.hits >= self._config.min_hits]
|
||||
if not pool:
|
||||
pool = measured
|
||||
chosen = pool[0]
|
||||
for t in pool:
|
||||
if t.confidence > chosen.confidence:
|
||||
chosen = t
|
||||
is_predicted = False
|
||||
elif coasting:
|
||||
# 无实测轨道：选离画面中心最近者（缺省按 640×480 中心 (320,240)）
|
||||
center_x = frame_shape.width / 2.0 if frame_shape and frame_shape.width > 0 else 320.0
|
||||
center_y = frame_shape.height / 2.0 if frame_shape and frame_shape.height > 0 else 240.0
|
||||
best_dist_sq = math.inf
|
||||
for t in coasting:
|
||||
dx = t.last_center[0] - center_x
|
||||
dy = t.last_center[1] - center_y
|
||||
dist_sq = dx * dx + dy * dy
|
||||
if dist_sq < best_dist_sq:
|
||||
best_dist_sq = dist_sq
|
||||
chosen = t
|
||||
is_predicted = True
|
||||
else:
|
||||
# 理论上不可达（轨道非空则 measured/coasting 必居其一）；防御清锁
|
||||
self._locked_primary_id = None
|
||||
return result
|
||||
self._locked_primary_id = chosen.track_id # 选举后锁定
|
||||
|
||||
# 组装输出（§2.3 九字段）；raw 仅实测时有值，Coast 时为空
|
||||
result.tracked = True
|
||||
result.primary_id = chosen.track_id
|
||||
result.center = chosen.last_center
|
||||
result.box = chosen.last_box
|
||||
result.confidence = chosen.confidence
|
||||
result.is_predicted = is_predicted
|
||||
result.lost_frames = chosen.lost_count
|
||||
result.raw = None if is_predicted else chosen.last_raw_box
|
||||
return result
|
||||
@@ -0,0 +1,110 @@
|
||||
"""轨道（Tracklet）与基础几何工具。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/target_tracker/include/target_tracker/tracklet.h
|
||||
（规格：docs/视觉追踪技术路线.md §3.2 / §5.2 / §7）。
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional
|
||||
|
||||
from .kalman_box_filter import BoxCXCYWH, KalmanBoxFilter
|
||||
|
||||
|
||||
@dataclass
|
||||
class BoxXYXY:
|
||||
"""轴对齐边界框（像素坐标，左上/右下角表示）。"""
|
||||
|
||||
x1: float = 0.0
|
||||
y1: float = 0.0
|
||||
x2: float = 0.0
|
||||
y2: float = 0.0
|
||||
|
||||
|
||||
def to_center_size(box: BoxXYXY) -> BoxCXCYWH:
|
||||
# §7 TRACKLET_UPDATE：cx=(x1+x2)/2；cy=(y1+y2)/2；w=x2−x1；h=y2−y1
|
||||
return BoxCXCYWH((box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0,
|
||||
box.x2 - box.x1, box.y2 - box.y1)
|
||||
|
||||
|
||||
def to_xyxy(box: BoxCXCYWH) -> BoxXYXY:
|
||||
# §7 TRACKLET_PREDICT 的 center_size 逆变换
|
||||
return BoxXYXY(box.cx - box.w / 2.0, box.cy - box.h / 2.0,
|
||||
box.cx + box.w / 2.0, box.cy + box.h / 2.0)
|
||||
|
||||
|
||||
def compute_iou(a: BoxXYXY, b: BoxXYXY) -> float:
|
||||
"""标准轴对齐 IoU（§5.2）：任一框为空、交集 ≤ 0 或并集 ≤ 0 时返回 0。"""
|
||||
area_a = (a.x2 - a.x1) * (a.y2 - a.y1)
|
||||
area_b = (b.x2 - b.x1) * (b.y2 - b.y1)
|
||||
if area_a <= 0.0 or area_b <= 0.0:
|
||||
return 0.0
|
||||
|
||||
inter_w = min(a.x2, b.x2) - max(a.x1, b.x1)
|
||||
inter_h = min(a.y2, b.y2) - max(a.y1, b.y1)
|
||||
if inter_w <= 0.0 or inter_h <= 0.0:
|
||||
return 0.0
|
||||
|
||||
inter_area = inter_w * inter_h
|
||||
union_area = area_a + area_b - inter_area
|
||||
if union_area <= 0.0:
|
||||
return 0.0
|
||||
return inter_area / union_area
|
||||
|
||||
|
||||
class Tracklet:
|
||||
"""单条轨道：生命周期计数 + 辅助状态 + 卡尔曼滤波器（§3.2）。
|
||||
|
||||
构造即创建轨道（§7 步骤 5）：用首次观测初始化滤波器，age=1、hits=1、
|
||||
lost_count=0，last_center / last_box 立即由观测重建（创建当帧即可参与输出）。
|
||||
"""
|
||||
|
||||
def __init__(self, track_id: int, measurement: BoxCXCYWH, conf: float,
|
||||
cls_id: int, max_lost_frames: int) -> None:
|
||||
self.track_id = track_id
|
||||
self.kf = KalmanBoxFilter()
|
||||
self.lost_count = 0
|
||||
self.max_lost = max_lost_frames
|
||||
self.confidence = conf
|
||||
self.cls_id = cls_id
|
||||
self.age = 1
|
||||
self.hits = 1
|
||||
self.last_center: tuple[float, float] = (measurement.cx, measurement.cy)
|
||||
self.last_box: Optional[BoxXYXY] = to_xyxy(measurement)
|
||||
# 创建即源于实测检测：同步记录原始检测框（Coast 期间保留不更新）
|
||||
self.last_raw_box: Optional[BoxXYXY] = self.last_box
|
||||
self.kf.initiate(measurement)
|
||||
|
||||
@property
|
||||
def is_dead(self) -> bool:
|
||||
# §6.1 注记 2：lost_count > max_lost（严格大于）
|
||||
return self.lost_count > self.max_lost
|
||||
|
||||
@property
|
||||
def is_coasting(self) -> bool:
|
||||
# §3.2：lost_count > 0 即处于纯预测状态
|
||||
return self.lost_count > 0
|
||||
|
||||
def predict(self) -> None:
|
||||
# §7 TRACKLET_PREDICT：预测成功时更新 last_center / last_box 且 age+1
|
||||
pred = self.kf.predict()
|
||||
if pred is not None:
|
||||
self.last_center = (pred.cx, pred.cy)
|
||||
self.last_box = to_xyxy(pred)
|
||||
self.age += 1
|
||||
|
||||
def update(self, box: BoxXYXY, conf: float, cls_id: int) -> None:
|
||||
# §7 TRACKLET_UPDATE：框 → [cx,cy,w,h] → kf.update
|
||||
self.kf.update(to_center_size(box))
|
||||
self.last_center = ((box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0)
|
||||
self.last_box = box
|
||||
self.last_raw_box = box
|
||||
self.confidence = conf
|
||||
self.cls_id = cls_id
|
||||
self.lost_count = 0
|
||||
self.hits += 1
|
||||
self.age += 1
|
||||
|
||||
def mark_miss(self) -> None:
|
||||
# §7 步骤 4：未匹配计丢失
|
||||
self.lost_count += 1
|
||||
self.age += 1
|
||||
@@ -0,0 +1,94 @@
|
||||
"""StereoConfig 校验与 JSON 加载测试（规格 §9：配置外置、fail-fast）。"""
|
||||
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import StereoConfig, load_from_json
|
||||
|
||||
CONFIG_PATH = Path(__file__).resolve().parents[2] / "config" / "stereo_ranger.json"
|
||||
|
||||
|
||||
def _config_dict() -> dict:
|
||||
return json.loads(CONFIG_PATH.read_text(encoding="utf-8"))
|
||||
|
||||
|
||||
def _write(tmp_path: Path, data: dict) -> str:
|
||||
path = tmp_path / "stereo.json"
|
||||
path.write_text(json.dumps(data, ensure_ascii=False), encoding="utf-8")
|
||||
return str(path)
|
||||
|
||||
|
||||
def test_defaults_are_valid_and_domains_are_consistent():
|
||||
cfg = StereoConfig()
|
||||
assert cfg.focal_baseline_px_m == pytest.approx(cfg.fx_px * cfg.baseline_m)
|
||||
assert cfg.z_min_m == pytest.approx(cfg.focal_baseline_px_m / cfg.disparity_max_px)
|
||||
assert cfg.z_max_m == pytest.approx(cfg.focal_baseline_px_m / cfg.disparity_min_px)
|
||||
assert cfg.z_error_budget_max_m == pytest.approx((cfg.max_delta_z_m * cfg.focal_baseline_px_m) ** 0.5)
|
||||
|
||||
|
||||
def test_load_from_json_matches_defaults():
|
||||
assert load_from_json(str(CONFIG_PATH)) == StereoConfig()
|
||||
|
||||
|
||||
def test_missing_field_rejected(tmp_path):
|
||||
data = _config_dict()
|
||||
data.pop("baseline_m")
|
||||
with pytest.raises(ValueError, match="缺少配置字段"):
|
||||
load_from_json(_write(tmp_path, data))
|
||||
|
||||
|
||||
def test_unknown_field_rejected(tmp_path):
|
||||
data = _config_dict()
|
||||
data["radar_fallback"] = True
|
||||
with pytest.raises(ValueError, match="未知配置字段"):
|
||||
load_from_json(_write(tmp_path, data))
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"field,value",
|
||||
[
|
||||
("fx_px", "960"),
|
||||
("consistency_N_frames", 3.5),
|
||||
("smooth_alpha", True),
|
||||
],
|
||||
)
|
||||
def test_type_error_rejected(tmp_path, field, value):
|
||||
data = _config_dict()
|
||||
data[field] = value
|
||||
with pytest.raises(ValueError, match="配置类型非法"):
|
||||
load_from_json(_write(tmp_path, data))
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"field,value",
|
||||
[
|
||||
("fx_px", 0.0),
|
||||
("fy_px", -1.0),
|
||||
("cx_px", -0.5),
|
||||
("cy_px", -0.5),
|
||||
("baseline_m", 0.0),
|
||||
("tau_y_px", 0.0),
|
||||
("tau_h_ratio", 0.0),
|
||||
("disparity_min_px", 0.0),
|
||||
("disparity_min_px", 60.0),
|
||||
("max_delta_z_m", 0.0),
|
||||
("consistency_N_frames", 0),
|
||||
("consistency_M_frames", 2),
|
||||
("consistency_jump_px", 0.0),
|
||||
("smooth_alpha", 0.0),
|
||||
("smooth_alpha", 1.5),
|
||||
("jump_reset_m", 0.0),
|
||||
("filter_reset_frames", 0),
|
||||
("isolation_gate_px", 0.0),
|
||||
],
|
||||
)
|
||||
def test_invalid_values_rejected(field, value):
|
||||
with pytest.raises(ValueError, match="配置非法"):
|
||||
StereoConfig(**{field: value})
|
||||
|
||||
|
||||
def test_boundary_values_accepted():
|
||||
cfg = StereoConfig(disparity_min_px=3.0, disparity_max_px=3.0 + 1e-9, smooth_alpha=1.0, consistency_M_frames=3)
|
||||
assert cfg.z_max_m > cfg.z_min_m
|
||||
@@ -0,0 +1,80 @@
|
||||
"""一致性监控测试（规格 §6.7/§11.4：N 帧降级、M 帧判 invalid、正常帧清零、每目标隔离）。"""
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import ConsistencyMonitor
|
||||
|
||||
|
||||
def _monitor() -> ConsistencyMonitor:
|
||||
return ConsistencyMonitor(jump_px=3.0, degrade_frames=3, invalid_frames=5)
|
||||
|
||||
|
||||
def test_first_frame_has_no_reference_jump():
|
||||
status = _monitor().advance("t", 10.0)
|
||||
assert status.jump_px is None
|
||||
assert status.abnormal is False
|
||||
assert status.streak == 0
|
||||
|
||||
|
||||
def test_normal_jump_resets_streak():
|
||||
monitor = _monitor()
|
||||
monitor.advance("t", 10.0)
|
||||
abnormal = monitor.advance("t", 16.0)
|
||||
assert abnormal.abnormal and abnormal.streak == 1
|
||||
normal = monitor.advance("t", 17.0)
|
||||
assert not normal.abnormal and normal.streak == 0
|
||||
|
||||
|
||||
def test_jump_at_threshold_is_normal():
|
||||
monitor = _monitor()
|
||||
monitor.advance("t", 10.0)
|
||||
assert not monitor.advance("t", 13.0).abnormal # |ΔD| = 3.0 不超门限
|
||||
|
||||
|
||||
def test_degrades_at_n_frames_and_invalidates_at_m_frames():
|
||||
monitor = _monitor()
|
||||
monitor.advance("t", 10.0)
|
||||
disparities = [14.0, 18.0, 22.0, 26.0, 30.0] # 每帧跳变 4 px > 3 px
|
||||
statuses = [monitor.advance("t", d) for d in disparities]
|
||||
assert [s.streak for s in statuses] == [1, 2, 3, 4, 5]
|
||||
assert [s.confidence_degraded for s in statuses] == [False, False, True, True, True]
|
||||
assert [s.invalid for s in statuses] == [False, False, False, False, True]
|
||||
|
||||
|
||||
def test_targets_are_isolated():
|
||||
monitor = _monitor()
|
||||
monitor.advance("a", 10.0)
|
||||
monitor.advance("b", 20.0)
|
||||
monitor.advance("a", 15.0) # a 跳变异常
|
||||
assert monitor.streak("a") == 1
|
||||
assert monitor.streak("b") == 0
|
||||
|
||||
|
||||
def test_drop_clears_only_target():
|
||||
monitor = _monitor()
|
||||
monitor.advance("a", 10.0)
|
||||
monitor.advance("a", 20.0)
|
||||
monitor.drop("a")
|
||||
assert monitor.streak("a") == 0
|
||||
assert monitor.advance("a", 40.0).jump_px is None # 状态已清空，重新起始
|
||||
|
||||
|
||||
def test_clear_resets_all():
|
||||
monitor = _monitor()
|
||||
monitor.advance("a", 10.0)
|
||||
monitor.advance("a", 20.0)
|
||||
monitor.clear()
|
||||
assert monitor.streak("a") == 0
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"kwargs",
|
||||
[
|
||||
{"jump_px": 0.0, "degrade_frames": 3, "invalid_frames": 5},
|
||||
{"jump_px": 3.0, "degrade_frames": 0, "invalid_frames": 5},
|
||||
{"jump_px": 3.0, "degrade_frames": 5, "invalid_frames": 3},
|
||||
],
|
||||
)
|
||||
def test_invalid_ctor_args(kwargs):
|
||||
with pytest.raises(ValueError):
|
||||
ConsistencyMonitor(**kwargs)
|
||||
@@ -0,0 +1,94 @@
|
||||
"""时间滤波器测试（规格 §6.5/§11.4：低通收敛、跳变重置不拖尾、失效清理、每目标隔离）。"""
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import DistanceFilter
|
||||
|
||||
|
||||
def _filter() -> DistanceFilter:
|
||||
return DistanceFilter(alpha=0.3, jump_reset_m=5.0, reset_frames=5)
|
||||
|
||||
|
||||
def test_first_value_initializes():
|
||||
assert _filter().advance("t", 10.0) == pytest.approx(10.0)
|
||||
|
||||
|
||||
def test_low_pass_step_response():
|
||||
distance_filter = _filter()
|
||||
distance_filter.advance("t", 10.0)
|
||||
assert distance_filter.advance("t", 12.0) == pytest.approx(10.6)
|
||||
|
||||
|
||||
def test_low_pass_converges_to_measurement():
|
||||
distance_filter = _filter()
|
||||
distance_filter.advance("t", 10.0)
|
||||
value = 10.0
|
||||
for _ in range(40):
|
||||
value = distance_filter.advance("t", 12.0)
|
||||
assert value == pytest.approx(12.0, abs=1e-3)
|
||||
|
||||
|
||||
def test_alpha_one_is_identity():
|
||||
distance_filter = DistanceFilter(alpha=1.0, jump_reset_m=5.0, reset_frames=5)
|
||||
distance_filter.advance("t", 10.0)
|
||||
assert distance_filter.advance("t", 12.0) == pytest.approx(12.0)
|
||||
|
||||
|
||||
def test_jump_reset_without_tail():
|
||||
distance_filter = _filter()
|
||||
distance_filter.advance("t", 10.0)
|
||||
assert distance_filter.advance("t", 20.0) == pytest.approx(20.0) # |Δ| > 5 → 直接重置
|
||||
assert distance_filter.value("t") == pytest.approx(20.0)
|
||||
assert distance_filter.advance("t", 20.2) == pytest.approx(20.06) # 之后恢复低通
|
||||
|
||||
|
||||
def test_invalidate_keeps_state_before_threshold_and_clears_after():
|
||||
distance_filter = _filter()
|
||||
distance_filter.advance("t", 10.0)
|
||||
for _ in range(4):
|
||||
distance_filter.invalidate("t")
|
||||
assert distance_filter.value("t") == pytest.approx(10.0)
|
||||
assert distance_filter.invalid_streak("t") == 4
|
||||
distance_filter.invalidate("t")
|
||||
assert distance_filter.value("t") is None
|
||||
assert distance_filter.advance("t", 30.0) == pytest.approx(30.0) # 重新初始化而非平滑
|
||||
|
||||
|
||||
def test_valid_frame_resets_invalid_streak():
|
||||
distance_filter = _filter()
|
||||
distance_filter.advance("t", 10.0)
|
||||
distance_filter.invalidate("t")
|
||||
distance_filter.invalidate("t")
|
||||
distance_filter.advance("t", 10.5)
|
||||
assert distance_filter.invalid_streak("t") == 0
|
||||
|
||||
|
||||
def test_invalidate_without_state_is_noop():
|
||||
distance_filter = _filter()
|
||||
distance_filter.invalidate("t")
|
||||
assert distance_filter.value("t") is None
|
||||
|
||||
|
||||
def test_targets_are_isolated():
|
||||
distance_filter = _filter()
|
||||
distance_filter.advance("a", 10.0)
|
||||
distance_filter.advance("b", 100.0)
|
||||
assert distance_filter.advance("a", 12.0) == pytest.approx(10.6)
|
||||
assert distance_filter.value("b") == pytest.approx(100.0)
|
||||
distance_filter.drop("a")
|
||||
assert distance_filter.value("a") is None
|
||||
assert distance_filter.value("b") == pytest.approx(100.0)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"kwargs",
|
||||
[
|
||||
{"alpha": 0.0, "jump_reset_m": 5.0, "reset_frames": 5},
|
||||
{"alpha": 1.5, "jump_reset_m": 5.0, "reset_frames": 5},
|
||||
{"alpha": 0.3, "jump_reset_m": 0.0, "reset_frames": 5},
|
||||
{"alpha": 0.3, "jump_reset_m": 5.0, "reset_frames": 0},
|
||||
],
|
||||
)
|
||||
def test_invalid_ctor_args(kwargs):
|
||||
with pytest.raises(ValueError):
|
||||
DistanceFilter(**kwargs)
|
||||
@@ -0,0 +1,103 @@
|
||||
"""测距计算与有效性判定测试（规格 §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)
|
||||
@@ -0,0 +1,87 @@
|
||||
"""匹配器测试（规格 §6.2/§11.4：硬约束各一例、贪心排序、一一配对、cost ≥ 1 拒绝）。"""
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import Detection, GreedyMatcher, SamplePoint, StereoConfig, sample_point
|
||||
|
||||
CFG = StereoConfig()
|
||||
MATCHER = GreedyMatcher()
|
||||
|
||||
|
||||
def _point(cx: float, cy: float, *, w: float = 60.0, h: float = 40.0, class_id: int = 0) -> SamplePoint:
|
||||
return sample_point(
|
||||
Detection(cx - w / 2.0, cy - h / 2.0, cx + w / 2.0, cy + h / 2.0, class_id=class_id)
|
||||
)
|
||||
|
||||
|
||||
def test_class_mismatch_rejected():
|
||||
assert MATCHER.match([_point(1000.0, 500.0)], [_point(1000.0, 500.0, class_id=1)], CFG) == []
|
||||
|
||||
|
||||
def test_dy_beyond_tau_rejected():
|
||||
assert MATCHER.match([_point(1000.0, 500.0)], [_point(1000.0, 502.1)], CFG) == []
|
||||
|
||||
|
||||
def test_dy_within_tau_accepted_with_cost():
|
||||
pairs = MATCHER.match([_point(1000.0, 500.0)], [_point(1000.0, 501.9)], CFG)
|
||||
assert len(pairs) == 1
|
||||
assert pairs[0].cost == pytest.approx(1.9 / CFG.tau_y_px)
|
||||
|
||||
|
||||
def test_height_ratio_beyond_tau_rejected():
|
||||
assert MATCHER.match([_point(1000.0, 500.0, h=100.0)], [_point(1000.0, 500.0, h=140.0)], CFG) == []
|
||||
|
||||
|
||||
def test_height_ratio_near_tau_accepted():
|
||||
pairs = MATCHER.match([_point(1000.0, 500.0, h=120.0)], [_point(1000.0, 500.0, h=159.0)], CFG)
|
||||
assert len(pairs) == 1
|
||||
|
||||
|
||||
def test_cost_at_one_rejected():
|
||||
# dy = tau_y ⇒ cost = 1.0，严格 cost < 1 才接受（§6.2）
|
||||
assert MATCHER.match([_point(1000.0, 500.0)], [_point(1000.0, 502.0)], CFG) == []
|
||||
|
||||
|
||||
def test_combined_cost_at_one_rejected():
|
||||
# dy/tau = 0.5 与 Δh/(tau_h·h_max) = 0.5 相加恰为 1.0 → 拒绝
|
||||
assert MATCHER.match([_point(1000.0, 500.0, h=112.0)], [_point(1000.0, 501.0, h=128.0)], CFG) == []
|
||||
|
||||
|
||||
def test_greedy_pairs_cheapest_first_and_one_to_one():
|
||||
left = [_point(1000.0, 500.0), _point(1000.0, 501.0)]
|
||||
right = [_point(1000.0, 500.0), _point(1000.0, 501.0)]
|
||||
pairs = MATCHER.match(left, right, CFG)
|
||||
assert [(p.left_index, p.right_index) for p in pairs] == [(0, 0), (1, 1)]
|
||||
assert pairs[0].cost == pytest.approx(0.0)
|
||||
|
||||
|
||||
def test_one_left_claims_cheapest_right():
|
||||
pairs = MATCHER.match([_point(1000.0, 500.0)], [_point(1000.0, 500.0), _point(1000.0, 500.5)], CFG)
|
||||
assert [(p.left_index, p.right_index) for p in pairs] == [(0, 0)]
|
||||
|
||||
|
||||
def test_two_lefts_cannot_share_one_right():
|
||||
left = [_point(1000.0, 500.0), _point(1000.0, 500.2)]
|
||||
right = [_point(1000.0, 500.0)]
|
||||
pairs = MATCHER.match(left, right, CFG)
|
||||
assert len(pairs) == 1
|
||||
assert pairs[0].left_index == 0
|
||||
|
||||
|
||||
def test_unmatched_right_is_not_paired():
|
||||
pairs = MATCHER.match([_point(1000.0, 500.0)], [_point(1000.0, 500.0), _point(1400.0, 500.0)], CFG)
|
||||
assert len(pairs) == 1
|
||||
assert pairs[0].right_index == 0
|
||||
|
||||
|
||||
def test_non_finite_points_excluded():
|
||||
bad = sample_point(Detection(float("nan"), 500.0, 1060.0, 540.0))
|
||||
good = _point(1000.0, 500.0)
|
||||
assert MATCHER.match([bad], [good], CFG) == []
|
||||
assert MATCHER.match([good], [bad], CFG) == []
|
||||
|
||||
|
||||
def test_zero_height_box_rejected():
|
||||
left = sample_point(Detection(970.0, 500.0, 1030.0, 500.0))
|
||||
right = sample_point(Detection(970.0, 500.0, 1030.0, 500.0))
|
||||
assert MATCHER.match([left], [right], CFG) == []
|
||||
@@ -0,0 +1,79 @@
|
||||
"""视差取点器测试（规格 §6.1/§11.4：同侧边缘、框宽差隔离、越窗回退、边界含等号）。"""
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import Detection, DisparitySource, StereoConfig, sample_point, select_disparity
|
||||
|
||||
CFG = StereoConfig()
|
||||
|
||||
|
||||
def _box(x1: float, x2: float, *, y1: float = 500.0, y2: float = 540.0) -> Detection:
|
||||
return Detection(x1, y1, x2, y2)
|
||||
|
||||
|
||||
def _disparity(left: Detection, right: Detection):
|
||||
return select_disparity(sample_point(left), sample_point(right), CFG)
|
||||
|
||||
|
||||
def test_left_edge_used_as_primary_sampling():
|
||||
d = _disparity(_box(1000.0, 1060.0), _box(990.4, 1050.4))
|
||||
assert d.source is DisparitySource.LEFT_EDGE
|
||||
assert d.d_px == pytest.approx(9.6)
|
||||
assert d.in_domain and d.positive and not d.fallback_used
|
||||
|
||||
|
||||
def test_box_width_difference_does_not_enter_edge_disparity():
|
||||
# §11.4：构造两目不等宽框，断言同侧边缘视差相同（框宽差不进入视差）
|
||||
left = _box(1000.0, 1100.0) # 宽 100
|
||||
right_narrow = _box(994.0, 1024.0) # 宽 30，左边缘差同 6 px
|
||||
right_wide = _box(994.0, 1094.0) # 宽 100
|
||||
d_narrow = _disparity(left, right_narrow)
|
||||
d_wide = _disparity(left, right_wide)
|
||||
assert d_narrow.d_px == pytest.approx(6.0)
|
||||
assert d_wide.d_px == pytest.approx(6.0)
|
||||
|
||||
|
||||
def test_center_disparity_would_be_polluted_by_width_difference():
|
||||
# 对照：中心视差 41 px = 边缘视差 6 px + 框宽差的一半 35 px，证明主取点选择边缘的理由
|
||||
left = _box(1000.0, 1100.0) # 宽 100, center 1050
|
||||
right_narrow = _box(994.0, 1024.0) # 宽 30, center 1009
|
||||
assert (left.cx - right_narrow.cx) == pytest.approx(41.0)
|
||||
|
||||
|
||||
def test_center_fallback_when_edge_out_of_window():
|
||||
# 边缘视差 60 px 越窗（> 57.6），中心视差 40 px 在域内 → 回退中心并标记
|
||||
left = _box(1000.0, 1060.0) # center 1030
|
||||
right = _box(940.0, 1040.0) # center 990
|
||||
d = _disparity(left, right)
|
||||
assert d.fallback_used is True
|
||||
assert d.source is DisparitySource.CENTER_FALLBACK
|
||||
assert d.d_px == pytest.approx(40.0)
|
||||
assert d.in_domain and d.positive
|
||||
|
||||
|
||||
def test_both_out_of_window_keeps_edge_value_for_diagnostics():
|
||||
left = _box(1000.0, 1200.0) # center 1100
|
||||
right = _box(900.0, 1000.0) # center 950
|
||||
d = _disparity(left, right)
|
||||
assert d.d_px == pytest.approx(100.0) # 边缘值保留（诊断 + 违例告警）
|
||||
assert not d.in_domain and not d.fallback_used and d.positive
|
||||
|
||||
|
||||
def test_domain_boundaries_are_inclusive():
|
||||
assert _disparity(_box(1000.0, 1060.0), _box(997.0, 1057.0)).in_domain # D = 3.0 = 下限
|
||||
# 上限用二进制可精确表示的 57.5 构造：十进制字面量 57.6 无法精确表示，
|
||||
# 框坐标相减会落到 57.6 常量之上，边界断言被浮点舍入干扰
|
||||
exact_cfg = StereoConfig(disparity_max_px=57.5)
|
||||
left, right = _box(1000.0, 1060.0), _box(942.5, 1002.5)
|
||||
assert select_disparity(sample_point(left), sample_point(right), exact_cfg).in_domain # D = 57.5 = 上限
|
||||
|
||||
|
||||
def test_negative_disparity_flagged_as_violation():
|
||||
d = _disparity(_box(1000.0, 1060.0), _box(1010.0, 1070.0))
|
||||
assert d.d_px == pytest.approx(-10.0)
|
||||
assert not d.positive and not d.in_domain
|
||||
|
||||
|
||||
def test_non_finite_box_marked_unusable():
|
||||
assert sample_point(Detection(float("nan"), 0.0, 10.0, 10.0)).finite is False
|
||||
assert sample_point(Detection(0.0, 0.0, 10.0, 10.0)).finite is True
|
||||
@@ -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) # 未平滑（状态已清空）
|
||||
@@ -0,0 +1,73 @@
|
||||
"""目标隔离测试（规格 §4：track_id 优先 + 框位置最近邻、帧末老化清理）。"""
|
||||
|
||||
from stereo_ranger import StereoConfig, TargetStore
|
||||
|
||||
CFG = StereoConfig() # isolation_gate_px = 50，filter_reset_frames = 5
|
||||
|
||||
|
||||
def test_track_key_stable_across_position_move():
|
||||
store = TargetStore(CFG)
|
||||
key_1 = store.resolve(0, 7, (100.0, 100.0), 1)
|
||||
key_2 = store.resolve(0, 7, (400.0, 100.0), 2) # 位置大幅移动仍是同一目标
|
||||
assert key_1 == key_2
|
||||
assert store.count == 1
|
||||
|
||||
|
||||
def test_nearest_neighbor_keeps_identity_within_gate():
|
||||
store = TargetStore(CFG)
|
||||
key_1 = store.resolve(0, None, (100.0, 100.0), 1)
|
||||
key_2 = store.resolve(0, None, (130.0, 140.0), 2) # 距离 50 ≤ 门限
|
||||
assert key_1 == key_2
|
||||
|
||||
|
||||
def test_beyond_gate_creates_new_key():
|
||||
store = TargetStore(CFG)
|
||||
key_1 = store.resolve(0, None, (100.0, 100.0), 1)
|
||||
key_2 = store.resolve(0, None, (200.0, 100.0), 2) # 距离 100 > 门限
|
||||
assert key_1 != key_2
|
||||
assert store.count == 2
|
||||
|
||||
|
||||
def test_class_id_separates_states():
|
||||
store = TargetStore(CFG)
|
||||
assert store.resolve(0, None, (100.0, 100.0), 1) != store.resolve(1, None, (100.0, 100.0), 1)
|
||||
|
||||
|
||||
def test_two_boxes_same_frame_claim_distinct_states():
|
||||
store = TargetStore(CFG)
|
||||
key_1 = store.resolve(0, None, (100.0, 100.0), 1)
|
||||
key_2 = store.resolve(0, None, (110.0, 100.0), 1) # 同帧第二框不得接管已占用状态
|
||||
assert key_1 != key_2
|
||||
assert store.count == 2
|
||||
|
||||
|
||||
def test_state_evicted_after_reset_frames_unseen():
|
||||
store = TargetStore(CFG)
|
||||
key = store.resolve(0, None, (100.0, 100.0), 1)
|
||||
store.distance_filter.advance(key, 5.0)
|
||||
store.consistency.advance(key, 9.6)
|
||||
for frame in range(2, 7): # 第 2~6 帧：连续 5 帧缺席
|
||||
store.end_frame(frame)
|
||||
assert store.count == 0
|
||||
assert store.distance_filter.value(key) is None
|
||||
assert store.consistency.streak(key) == 0
|
||||
|
||||
|
||||
def test_present_but_invalid_keeps_position_state():
|
||||
store = TargetStore(CFG)
|
||||
key = store.resolve(0, None, (100.0, 100.0), 1)
|
||||
store.distance_filter.advance(key, 5.0)
|
||||
for frame in range(2, 8):
|
||||
store.resolve(0, None, (100.0, 100.0), frame) # 每帧仍出现
|
||||
store.distance_filter.invalidate(key)
|
||||
assert store.count == 1 # 位置状态保留
|
||||
assert store.distance_filter.value(key) is None # 滤波状态已按门限清空
|
||||
|
||||
|
||||
def test_clear_resets_everything():
|
||||
store = TargetStore(CFG)
|
||||
key = store.resolve(0, 5, (100.0, 100.0), 1)
|
||||
store.distance_filter.advance(key, 5.0)
|
||||
store.clear()
|
||||
assert store.count == 0
|
||||
assert store.distance_filter.value(key) is None
|
||||
@@ -0,0 +1,45 @@
|
||||
"""告警节流测试（规格 §5：第 1 次 + 每满 100 次，带累计计数）。"""
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_ranger import WarnThrottle
|
||||
|
||||
|
||||
def test_first_and_every_hundred_emit():
|
||||
throttle = WarnThrottle() # 默认 every=100
|
||||
assert throttle.should_emit("a") is True
|
||||
for _ in range(98):
|
||||
assert throttle.should_emit("a") is False
|
||||
assert throttle.should_emit("a") is True # 第 100 次
|
||||
for _ in range(99):
|
||||
assert throttle.should_emit("a") is False
|
||||
assert throttle.should_emit("a") is True # 第 200 次
|
||||
assert throttle.count("a") == 200
|
||||
|
||||
|
||||
def test_categories_are_independent():
|
||||
throttle = WarnThrottle()
|
||||
assert throttle.should_emit("a") is True
|
||||
assert throttle.should_emit("b") is True
|
||||
assert throttle.count("a") == 1
|
||||
assert throttle.count("b") == 1
|
||||
assert throttle.count("never_seen") == 0
|
||||
|
||||
|
||||
def test_custom_interval():
|
||||
throttle = WarnThrottle(every=3)
|
||||
# 规格语义"第 1 次 + 计数每满 every" → 第 1/3/6 次打印
|
||||
assert [throttle.should_emit("a") for _ in range(6)] == [True, False, True, False, False, True]
|
||||
|
||||
|
||||
def test_clear_resets_counts():
|
||||
throttle = WarnThrottle()
|
||||
throttle.should_emit("a")
|
||||
throttle.clear()
|
||||
assert throttle.count("a") == 0
|
||||
assert throttle.should_emit("a") is True
|
||||
|
||||
|
||||
def test_invalid_interval():
|
||||
with pytest.raises(ValueError):
|
||||
WarnThrottle(every=0)
|
||||
@@ -0,0 +1,96 @@
|
||||
"""StereoYoloConfig 校验与 JSON 加载测试（需求说明 §4：配置外置、fail-fast）。"""
|
||||
|
||||
import json
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from stereo_yolo import StereoYoloConfig, load_from_json
|
||||
|
||||
CONFIG_PATH = Path(__file__).resolve().parents[2] / "config" / "stereo_yolo.json"
|
||||
|
||||
|
||||
def _config_dict() -> dict:
|
||||
return json.loads(CONFIG_PATH.read_text(encoding="utf-8"))
|
||||
|
||||
|
||||
def _write(tmp_path: Path, data: dict) -> str:
|
||||
path = tmp_path / "stereo_yolo.json"
|
||||
path.write_text(json.dumps(data, ensure_ascii=False), encoding="utf-8")
|
||||
return str(path)
|
||||
|
||||
|
||||
def test_load_from_json_matches_defaults():
|
||||
assert load_from_json(str(CONFIG_PATH)) == StereoYoloConfig()
|
||||
|
||||
|
||||
def test_whitelist_normalized_to_tuple():
|
||||
cfg = StereoYoloConfig(class_whitelist=[0, 2])
|
||||
assert cfg.class_whitelist == (0, 2)
|
||||
|
||||
|
||||
def test_missing_field_rejected(tmp_path):
|
||||
data = _config_dict()
|
||||
data.pop("imgsz")
|
||||
with pytest.raises(ValueError, match="缺少配置字段"):
|
||||
load_from_json(_write(tmp_path, data))
|
||||
|
||||
|
||||
def test_unknown_field_rejected(tmp_path):
|
||||
data = _config_dict()
|
||||
data["letterbox_scale"] = 0.5
|
||||
with pytest.raises(ValueError, match="未知配置字段"):
|
||||
load_from_json(_write(tmp_path, data))
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"field,value",
|
||||
[
|
||||
("model_path", 123),
|
||||
("imgsz", "640"),
|
||||
("imgsz", True),
|
||||
("conf_threshold", "0.25"),
|
||||
("iou_threshold", None),
|
||||
("class_whitelist", 0),
|
||||
("class_whitelist", ["0"]),
|
||||
("device", 5),
|
||||
("concurrency", 1),
|
||||
],
|
||||
)
|
||||
def test_type_error_rejected(tmp_path, field, value):
|
||||
data = _config_dict()
|
||||
data[field] = value
|
||||
with pytest.raises(ValueError, match="配置类型非法"):
|
||||
load_from_json(_write(tmp_path, data))
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"field,value",
|
||||
[
|
||||
("model_path", ""),
|
||||
("imgsz", 0),
|
||||
("imgsz", 600),
|
||||
("conf_threshold", -0.1),
|
||||
("conf_threshold", 1.5),
|
||||
("iou_threshold", 0.0),
|
||||
("class_whitelist", []),
|
||||
("class_whitelist", [-1]),
|
||||
("device", " "),
|
||||
("concurrency", "batch"),
|
||||
],
|
||||
)
|
||||
def test_invalid_values_rejected(field, value):
|
||||
with pytest.raises(ValueError, match="配置非法"):
|
||||
StereoYoloConfig(**{field: value})
|
||||
|
||||
|
||||
def test_boundary_values_accepted():
|
||||
cfg = StereoYoloConfig(
|
||||
imgsz=32,
|
||||
conf_threshold=0.0,
|
||||
iou_threshold=1.0,
|
||||
class_whitelist=(0,),
|
||||
device="cpu",
|
||||
concurrency="sequential",
|
||||
)
|
||||
assert cfg.imgsz == 32
|
||||
@@ -0,0 +1,155 @@
|
||||
"""StereoYoloFrontend 出口契约测试（需求说明 §2.2/§2.3/§2.5/§5）。
|
||||
|
||||
全部用注入的假检测器驱动：不需要模型权重、不需要图像素材、不需要硬件；
|
||||
真并行用双人屏障断言（串行实现会屏障超时）。
|
||||
"""
|
||||
|
||||
import logging
|
||||
import threading
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from stereo_ranger.types import Detection
|
||||
from stereo_yolo import StereoYoloConfig, StereoYoloFrontend
|
||||
|
||||
|
||||
class ScriptedDetector:
|
||||
"""按调用顺序返回脚本项：list[Detection] 正常返回；Exception 抛出。"""
|
||||
|
||||
def __init__(self, script):
|
||||
self._script = list(script)
|
||||
self.calls = 0
|
||||
|
||||
def detect(self, image):
|
||||
self.calls += 1
|
||||
if not self._script:
|
||||
raise AssertionError("假检测器脚本已耗尽（调用次数多于预期）")
|
||||
item = self._script.pop(0)
|
||||
if isinstance(item, Exception):
|
||||
raise item
|
||||
return list(item)
|
||||
|
||||
|
||||
class BarrierDetector:
|
||||
"""两次调用必须同时在途（屏障超时即证明串行或单线程执行）。"""
|
||||
|
||||
def __init__(self, barrier: threading.Barrier, detections):
|
||||
self._barrier = barrier
|
||||
self._detections = list(detections)
|
||||
|
||||
def detect(self, image):
|
||||
self._barrier.wait(timeout=2.0)
|
||||
return list(self._detections)
|
||||
|
||||
|
||||
def _img(fill=0, shape=(8, 8, 3)) -> np.ndarray:
|
||||
return np.full(shape, fill, dtype=np.uint8)
|
||||
|
||||
|
||||
def _det(x1=1.0, y1=2.0, x2=11.0, y2=12.0, class_id=0, confidence=0.9, track_id=None) -> Detection:
|
||||
return Detection(x1, y1, x2, y2, confidence=confidence, class_id=class_id, track_id=track_id)
|
||||
|
||||
|
||||
def _frontend(script, **cfg_kwargs) -> StereoYoloFrontend:
|
||||
kwargs = {"concurrency": "sequential"}
|
||||
kwargs.update(cfg_kwargs)
|
||||
return StereoYoloFrontend(StereoYoloConfig(**kwargs), detector=ScriptedDetector(script))
|
||||
|
||||
|
||||
def test_frames_share_frame_id_and_keep_boxes():
|
||||
left_box, right_box = _det(1.0, 2.0, 11.0, 12.0, confidence=0.77), _det(3.0, 4.0, 13.0, 14.0)
|
||||
frontend = _frontend([[left_box], [right_box]])
|
||||
left_frame, right_frame = frontend.detect(_img(), _img(), frame_id=7)
|
||||
|
||||
assert left_frame.frame_id == right_frame.frame_id == 7
|
||||
assert left_frame.detections == (left_box,)
|
||||
assert right_frame.detections == (right_box,)
|
||||
assert left_frame.detections[0].confidence == pytest.approx(0.77)
|
||||
assert left_frame.detections[0].track_id is None # 无追踪器时留空（§2.1）
|
||||
assert frontend._detector.calls == 2
|
||||
|
||||
|
||||
def test_whitelist_filters_by_class():
|
||||
frontend = _frontend([[_det(class_id=3)], [_det(class_id=0)]], class_whitelist=(0,))
|
||||
left_frame, right_frame = frontend.detect(_img(), _img(), frame_id=1)
|
||||
|
||||
assert left_frame.detections == ()
|
||||
assert len(right_frame.detections) == 1 and right_frame.detections[0].class_id == 0
|
||||
|
||||
|
||||
def test_no_whitelist_keeps_all_classes():
|
||||
frontend = _frontend([[_det(class_id=3)], [_det(class_id=5)]])
|
||||
left_frame, right_frame = frontend.detect(_img(), _img(), frame_id=1)
|
||||
|
||||
assert len(left_frame.detections) == 1 and len(right_frame.detections) == 1
|
||||
|
||||
|
||||
def test_degenerate_boxes_dropped(caplog):
|
||||
nan_box = _det(x1=float("nan"))
|
||||
zero_width = _det(x1=5.0, x2=5.0)
|
||||
inverted = _det(x1=20.0, x2=10.0)
|
||||
good = _det()
|
||||
frontend = _frontend([[nan_box, zero_width, inverted, good], []])
|
||||
|
||||
with caplog.at_level(logging.WARNING, logger="stereo_yolo"):
|
||||
left_frame, right_frame = frontend.detect(_img(), _img(), frame_id=2)
|
||||
|
||||
assert left_frame.detections == (good,)
|
||||
assert right_frame.detections == ()
|
||||
assert "丢弃退化检测框" in caplog.text
|
||||
assert "累计 1 次" in caplog.text
|
||||
|
||||
|
||||
def test_boundary_touching_box_kept():
|
||||
hairline = _det(x1=10.0, x2=10.0 + 1e-9)
|
||||
frontend = _frontend([[hairline], []])
|
||||
left_frame, _ = frontend.detect(_img(), _img(), frame_id=3)
|
||||
assert left_frame.detections == (hairline,)
|
||||
|
||||
|
||||
def test_one_eye_failure_isolated(caplog):
|
||||
good = _det()
|
||||
frontend = _frontend([[good], RuntimeError("boom")])
|
||||
|
||||
with caplog.at_level(logging.WARNING, logger="stereo_yolo"):
|
||||
left_frame, right_frame = frontend.detect(_img(), _img(), frame_id=4)
|
||||
|
||||
assert left_frame.detections == (good,)
|
||||
assert right_frame.detections == ()
|
||||
assert left_frame.frame_id == right_frame.frame_id == 4
|
||||
assert "右目推理失败" in caplog.text
|
||||
|
||||
|
||||
def test_shape_mismatch_rejected(caplog):
|
||||
frontend = _frontend([]) # 脚本为空：若检测器被调用会抛断言
|
||||
|
||||
with caplog.at_level(logging.WARNING, logger="stereo_yolo"):
|
||||
left_frame, right_frame = frontend.detect(_img(shape=(8, 8, 3)), _img(shape=(8, 6, 3)), frame_id=5)
|
||||
|
||||
assert left_frame.detections == () and right_frame.detections == ()
|
||||
assert left_frame.frame_id == right_frame.frame_id == 5
|
||||
assert frontend._detector.calls == 0
|
||||
assert "左右图尺寸不一致" in caplog.text
|
||||
|
||||
|
||||
def test_parallel_eyes_run_concurrently():
|
||||
barrier = threading.Barrier(2)
|
||||
box = _det()
|
||||
frontend = StereoYoloFrontend(
|
||||
StereoYoloConfig(concurrency="parallel"),
|
||||
detector=BarrierDetector(barrier, [box]),
|
||||
)
|
||||
try:
|
||||
left_frame, right_frame = frontend.detect(_img(), _img(), frame_id=6)
|
||||
finally:
|
||||
frontend.close()
|
||||
|
||||
assert left_frame.detections == (box,)
|
||||
assert right_frame.detections == (box,)
|
||||
|
||||
|
||||
def test_close_is_idempotent():
|
||||
frontend = StereoYoloFrontend(StereoYoloConfig(concurrency="parallel"), detector=ScriptedDetector([]))
|
||||
frontend.close()
|
||||
frontend.close()
|
||||
@@ -0,0 +1,27 @@
|
||||
"""split_sbs 裁切测试（需求说明 §2.3：sbs 单流形态）。"""
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from stereo_yolo import split_sbs
|
||||
|
||||
|
||||
def test_split_returns_left_and_right_halves():
|
||||
image = np.zeros((4, 10, 3), dtype=np.uint8)
|
||||
image[:, :5] = 1
|
||||
image[:, 5:] = 2
|
||||
|
||||
left, right = split_sbs(image)
|
||||
|
||||
assert left.shape == (4, 5, 3) and right.shape == (4, 5, 3)
|
||||
assert (left == 1).all() and (right == 2).all()
|
||||
|
||||
|
||||
def test_odd_width_rejected():
|
||||
with pytest.raises(ValueError, match="宽度须为偶数"):
|
||||
split_sbs(np.zeros((4, 9, 3), dtype=np.uint8))
|
||||
|
||||
|
||||
def test_non_image_rejected():
|
||||
with pytest.raises(ValueError, match="宽度须为偶数"):
|
||||
split_sbs(np.zeros((9,), dtype=np.uint8))
|
||||
@@ -0,0 +1,107 @@
|
||||
"""kalman_box_filter_test.py —— KalmanBoxFilter 单元测试。
|
||||
|
||||
移植自 reference-c++/target_tracker/tests/kalman_box_filter_test.cpp。
|
||||
覆盖（视觉追踪技术路线 §4）：初始化状态、静止目标收敛、匀速目标速度收敛
|
||||
且预测超前一个 dt、未初始化 predict 返回 None。
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from target_tracker.kalman_box_filter import BoxCXCYWH, KalmanBoxFilter
|
||||
|
||||
|
||||
def test_uninitialized_predict_returns_none():
|
||||
kf = KalmanBoxFilter()
|
||||
assert not kf.initialized
|
||||
assert kf.predict() is None
|
||||
|
||||
# 未初始化时 update 无副作用
|
||||
kf.update(BoxCXCYWH(100.0, 100.0, 20.0, 20.0))
|
||||
assert not kf.initialized
|
||||
assert kf.predict() is None
|
||||
for v in kf.state:
|
||||
assert v == 0.0
|
||||
|
||||
|
||||
def test_initiate_sets_state_correctly():
|
||||
kf = KalmanBoxFilter()
|
||||
kf.initiate(BoxCXCYWH(320.0, 240.0, 40.0, 30.0))
|
||||
assert kf.initialized
|
||||
|
||||
expected = [320.0, 240.0, 40.0, 30.0, 0.0, 0.0, 0.0, 0.0]
|
||||
state = kf.state
|
||||
for i, want in enumerate(expected):
|
||||
assert state[i] == want, f"index={i}"
|
||||
|
||||
# 初始化后 predict 返回状态前 4 维（速度为零 → 位置不变，§4.4）
|
||||
pred = kf.predict()
|
||||
assert pred is not None
|
||||
assert pred.cx == pytest.approx(320.0, abs=1e-9)
|
||||
assert pred.cy == pytest.approx(240.0, abs=1e-9)
|
||||
assert pred.w == pytest.approx(40.0, abs=1e-9)
|
||||
assert pred.h == pytest.approx(30.0, abs=1e-9)
|
||||
|
||||
|
||||
def test_static_target_converges_to_observation():
|
||||
kf = KalmanBoxFilter()
|
||||
z = BoxCXCYWH(100.0, 150.0, 40.0, 60.0)
|
||||
kf.initiate(BoxCXCYWH(104.0, 156.0, 42.0, 62.0)) # 带初始偏差，验证收敛能力
|
||||
|
||||
for _ in range(50):
|
||||
kf.predict()
|
||||
kf.update(z)
|
||||
|
||||
state = kf.state
|
||||
assert state[0] == pytest.approx(z.cx, abs=0.5) # cx
|
||||
assert state[1] == pytest.approx(z.cy, abs=0.5) # cy
|
||||
assert state[2] == pytest.approx(z.w, abs=0.5) # w
|
||||
assert state[3] == pytest.approx(z.h, abs=0.5) # h
|
||||
# 静止目标速度应收敛到约 0
|
||||
assert state[4] == pytest.approx(0.0, abs=0.1)
|
||||
assert state[5] == pytest.approx(0.0, abs=0.1)
|
||||
|
||||
|
||||
def test_constant_velocity_converges_and_predict_leads_one_dt():
|
||||
kf = KalmanBoxFilter()
|
||||
vx = 3.0 # 像素/帧
|
||||
vy = 2.0 # 像素/帧
|
||||
|
||||
# 目标从 (100, 100) 出发匀速运动，每帧观测一次（predict → update）
|
||||
z = BoxCXCYWH(100.0, 100.0, 30.0, 30.0)
|
||||
kf.initiate(z)
|
||||
for _ in range(60):
|
||||
kf.predict()
|
||||
z.cx += vx
|
||||
z.cy += vy
|
||||
kf.update(z)
|
||||
|
||||
state = kf.state
|
||||
# 速度收敛到真实速度（像素/帧）
|
||||
assert state[4] == pytest.approx(vx, abs=0.3), "vx 未收敛"
|
||||
assert state[5] == pytest.approx(vy, abs=0.3), "vy 未收敛"
|
||||
# 位置贴合当前观测
|
||||
assert state[0] == pytest.approx(z.cx, abs=1.5)
|
||||
assert state[1] == pytest.approx(z.cy, abs=1.5)
|
||||
|
||||
# 再预测一帧：应超前当前观测一个 dt（≈ 速度 × 1 帧）
|
||||
pred = kf.predict()
|
||||
assert pred is not None
|
||||
assert pred.cx == pytest.approx(z.cx + vx, abs=1.0), "预测未超前一个 dt"
|
||||
assert pred.cy == pytest.approx(z.cy + vy, abs=1.0), "预测未超前一个 dt"
|
||||
|
||||
|
||||
def test_small_target_noise_scaling_stable():
|
||||
# 动态噪声不退化：小尺寸目标多帧观测仍稳定收敛（§4.2 噪声与尺寸成比例）
|
||||
kf = KalmanBoxFilter()
|
||||
z = BoxCXCYWH(50.0, 60.0, 4.0, 4.0) # 小目标（4×4 像素）
|
||||
kf.initiate(z)
|
||||
for _ in range(30):
|
||||
kf.predict()
|
||||
kf.update(z)
|
||||
state = kf.state
|
||||
assert state[0] == pytest.approx(z.cx, abs=0.5)
|
||||
assert state[1] == pytest.approx(z.cy, abs=0.5)
|
||||
assert math.isfinite(state[2])
|
||||
assert math.isfinite(state[3])
|
||||
@@ -0,0 +1,364 @@
|
||||
"""target_tracker_test.py —— TargetTracker 单元测试。
|
||||
|
||||
移植自 reference-c++/target_tracker/tests/target_tracker_test.cpp。
|
||||
逐条覆盖 docs/视觉追踪技术路线.md §9.4 行为一致性验收用例（硬验收标准）：
|
||||
1. 空检测帧序列：Coast 持续 max_lost_frames 帧，第 max_lost_frames+1 帧判丢
|
||||
2. ID 严格递增不复用（含 Reset 后归零）
|
||||
3. 锁定不被抢占；锁定目标死亡后切换到新目标
|
||||
4. min_hits 回退：无已确认实测轨道时新轨道也可当选主目标
|
||||
5. cost=1.0 恰好不构成匹配（严格小于）
|
||||
6. 置信度优先：两轨道竞争同一检测，置信度高者获胜
|
||||
另含 NaN/Inf 防御与 §2.3/§6.3/§9.5 行为测试。
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from target_tracker.target_tracker import (Detection, FrameShape, TargetTracker,
|
||||
TrackerConfig)
|
||||
|
||||
|
||||
def make_det(x1, y1, x2, y2, conf, cls=0):
|
||||
return Detection(x1, y1, x2, y2, conf, cls)
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 验收用例 1（§9.4-1）：空检测帧序列
|
||||
# ============================================================================
|
||||
def test_empty_frames_coast_then_lost():
|
||||
tracker = TargetTracker() # 默认配置 max_lost_frames = 8
|
||||
assert tracker.get_config().max_lost_frames == 8
|
||||
|
||||
# 第 1 帧：给出一个检测，建立轨道并锁定为主目标
|
||||
r = tracker.update([make_det(100.0, 100.0, 140.0, 140.0, 0.9)])
|
||||
assert r.tracked
|
||||
assert r.primary_id is not None
|
||||
assert not r.is_predicted # 当帧有实测
|
||||
assert r.lost_frames == 0
|
||||
assert r.n_active == 1
|
||||
assert r.box is not None
|
||||
assert r.raw is not None # 当帧关联到原始检测框
|
||||
|
||||
# 后续空检测帧：应持续 Coast 共 max_lost_frames 帧（is_predicted=True）
|
||||
for i in range(1, 9):
|
||||
r = tracker.update([])
|
||||
assert r.tracked, f"第 {i} 帧 Coast 期间不应判丢"
|
||||
assert r.is_predicted, f"第 {i} 帧应为纯预测输出"
|
||||
assert r.lost_frames == i
|
||||
assert r.primary_id is not None
|
||||
assert r.box is not None # Coast 期间继续输出预测框
|
||||
assert r.raw is None # Coast 时 raw 为空（§2.3）
|
||||
assert r.confidence == pytest.approx(0.9) # 继承最近实测置信度（§2.3）
|
||||
|
||||
# 第 max_lost_frames + 1 帧：轨道死亡回收 → tracked=False
|
||||
r = tracker.update([])
|
||||
assert not r.tracked
|
||||
assert r.primary_id is None
|
||||
assert r.box is None
|
||||
assert r.lost_frames == 0 # 丢失时 lost_frames=0（§2.3）
|
||||
assert r.n_active == 0
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 验收用例 2（§9.4-2）：ID 单调性
|
||||
# ============================================================================
|
||||
def test_id_strictly_increasing_no_reuse():
|
||||
cfg = TrackerConfig()
|
||||
cfg.max_lost_frames = 1 # 缩短 Coast 窗口，加速轨道死亡
|
||||
tracker = TargetTracker(cfg)
|
||||
|
||||
# 目标 A 出现 → id 0
|
||||
r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.9)])
|
||||
assert r.primary_id is not None
|
||||
id_a = r.primary_id
|
||||
assert id_a == 0
|
||||
|
||||
# 目标 A 消失至死亡（max_lost=1：第 2 帧 Coast、第 3 帧死亡回收）
|
||||
tracker.update([])
|
||||
r = tracker.update([])
|
||||
assert not r.tracked
|
||||
assert tracker.active_count() == 0
|
||||
|
||||
# 目标 B 出现（同一位置）：ID 不复用，严格递增 → id 1
|
||||
r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.9)])
|
||||
assert r.primary_id is not None
|
||||
id_b = r.primary_id
|
||||
assert id_b == 1
|
||||
assert id_b > id_a
|
||||
|
||||
# 目标 C 同帧新建 → id 2（同帧多目标亦严格递增）
|
||||
r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.9),
|
||||
make_det(400.0, 300.0, 430.0, 330.0, 0.8)])
|
||||
assert tracker.active_count() == 2
|
||||
max_id = max(tracker.active_tracks().keys())
|
||||
assert max_id == 2
|
||||
|
||||
# Reset 后计数器归零（§6.2）：新轨道重新从 id 0 开始
|
||||
tracker.reset()
|
||||
assert tracker.active_count() == 0
|
||||
assert not tracker.last_result().tracked
|
||||
r = tracker.update([make_det(50.0, 50.0, 70.0, 70.0, 0.9)])
|
||||
assert r.primary_id is not None
|
||||
assert r.primary_id == 0
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 验收用例 3（§9.4-3）：锁定不被抢占
|
||||
# ============================================================================
|
||||
def test_lock_not_stolen_and_switch_after_death():
|
||||
cfg = TrackerConfig()
|
||||
cfg.max_lost_frames = 3
|
||||
tracker = TargetTracker(cfg)
|
||||
|
||||
# 低置信度目标 A 建立并锁定
|
||||
r = tracker.update([make_det(100.0, 100.0, 140.0, 140.0, 0.4)])
|
||||
assert r.primary_id is not None
|
||||
id_a = r.primary_id
|
||||
|
||||
# A 消失进入 Coast；期间出现更高置信度新目标 B（远离 A，不会被误关联）
|
||||
r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)])
|
||||
assert r.primary_id is not None
|
||||
assert r.primary_id == id_a, "Coast 期间锁定不应被高置信度目标抢占"
|
||||
assert r.is_predicted
|
||||
assert tracker.active_count() == 2 # A（Coast）与 B 并存
|
||||
|
||||
# 继续 Coast，锁定依旧延续
|
||||
r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)])
|
||||
assert r.primary_id == id_a
|
||||
assert r.is_predicted
|
||||
|
||||
# A 继续丢失直至死亡（max_lost=3：lost 达 4 帧时回收）
|
||||
for _ in range(2):
|
||||
r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)])
|
||||
# A 已死亡删除 → 锁定解除，重新选举切换到 B（此时 B 已确认且实测）
|
||||
assert r.tracked
|
||||
assert r.primary_id != id_a, "锁定目标死亡后应切换到新目标"
|
||||
assert not r.is_predicted
|
||||
|
||||
# 后续 B 持续作为主目标
|
||||
r = tracker.update([make_det(500.0, 300.0, 540.0, 340.0, 0.95)])
|
||||
assert r.primary_id is not None
|
||||
assert r.primary_id != id_a
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 验收用例 4（§9.4-4）：min_hits 回退
|
||||
# ============================================================================
|
||||
def test_min_hits_fallback_allows_new_track():
|
||||
cfg = TrackerConfig()
|
||||
cfg.min_hits = 5 # 抬高确认门槛，保证测试期间轨道始终未确认
|
||||
tracker = TargetTracker(cfg)
|
||||
|
||||
# 首帧新建轨道 hits=1 < min_hits：候选池 confirmed 为空 → 退回 measured
|
||||
r = tracker.update([make_det(200.0, 150.0, 240.0, 190.0, 0.6)])
|
||||
assert r.tracked, "无已确认轨道时新轨道应能当选主目标"
|
||||
assert r.primary_id is not None
|
||||
assert r.primary_id == 0
|
||||
assert not r.is_predicted
|
||||
|
||||
# 再跟几帧（hits 增长但仍未达 min_hits=5），依旧当选
|
||||
for _ in range(3):
|
||||
r = tracker.update([make_det(200.0, 150.0, 240.0, 190.0, 0.6)])
|
||||
assert r.tracked
|
||||
assert r.primary_id == 0
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 验收用例 5（§9.4-5）：cost 边界（cost = 1.0 恰好不匹配）
|
||||
# ============================================================================
|
||||
def test_cost_exactly_one_does_not_match():
|
||||
cfg = TrackerConfig()
|
||||
cfg.max_lost_frames = 1
|
||||
cfg.max_association_dist = 200.0
|
||||
cfg.dist_weight = 1.0 # 纯距离项，IoU 项权重为 0 且两框零交集
|
||||
cfg.iou_weight = 0.0
|
||||
tracker = TargetTracker(cfg)
|
||||
|
||||
# 建立轨道：中心 (100,100)
|
||||
r = tracker.update([make_det(90.0, 90.0, 110.0, 110.0, 0.9)])
|
||||
assert r.primary_id is not None
|
||||
id_a = r.primary_id
|
||||
|
||||
# 下一帧检测中心 (100,300)：距离 = 200 = Dmax → cost = 1.0，恰好不匹配
|
||||
r = tracker.update([make_det(90.0, 290.0, 110.0, 310.0, 0.9)])
|
||||
# 未匹配 → 原轨道 Coast、检测新建轨道；两个目标并存
|
||||
assert tracker.active_count() == 2, "cost=1.0 不应匹配，应新建轨道"
|
||||
# 原轨道（锁定）进入 Coast：lost_frames=1
|
||||
assert r.tracked
|
||||
assert r.is_predicted
|
||||
assert r.primary_id == id_a
|
||||
assert r.lost_frames == 1
|
||||
|
||||
# 对照：距离略小于阈值（199px → cost=0.995 < 1.0）应匹配成功
|
||||
tracker2 = TargetTracker(cfg)
|
||||
r = tracker2.update([make_det(90.0, 90.0, 110.0, 110.0, 0.9)])
|
||||
id_b = r.primary_id
|
||||
r = tracker2.update([make_det(90.0, 289.0, 110.0, 309.0, 0.9)])
|
||||
assert tracker2.active_count() == 1, "cost<1.0 应匹配成功，不应新建轨道"
|
||||
assert r.primary_id is not None
|
||||
assert r.primary_id == id_b
|
||||
assert not r.is_predicted # 实测命中
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 验收用例 6（§9.4-6）：置信度优先
|
||||
# ============================================================================
|
||||
def test_higher_confidence_track_wins_detection():
|
||||
tracker = TargetTracker()
|
||||
|
||||
# 同帧建立两条轨道：A 置信度 0.5、B 置信度 0.9，相距 80px（不互相重叠）
|
||||
r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.5),
|
||||
make_det(180.0, 100.0, 200.0, 120.0, 0.9)])
|
||||
assert tracker.active_count() == 2
|
||||
|
||||
# 找出两条轨道 ID（按置信度区分）
|
||||
id_low = None
|
||||
id_high = None
|
||||
for track_id, track in tracker.active_tracks().items():
|
||||
if track.confidence > 0.8:
|
||||
id_high = track_id
|
||||
else:
|
||||
id_low = track_id
|
||||
assert id_low is not None
|
||||
assert id_high is not None
|
||||
|
||||
# 下一帧只给一个检测，放在两轨道预测中心之间（各距 40px，对称）：
|
||||
# 距离项对两者相同，贪婪按置信度降序 → 高置信度轨道 B 优先匹配
|
||||
r = tracker.update([make_det(140.0, 100.0, 160.0, 120.0, 0.7)])
|
||||
|
||||
tracks = tracker.active_tracks()
|
||||
assert len(tracks) == 2
|
||||
# 获胜者：命中（lost=0、hits=2、置信度更新）
|
||||
assert tracks[id_high].lost_count == 0, "高置信度轨道应获胜"
|
||||
assert tracks[id_high].hits == 2
|
||||
assert tracks[id_high].confidence == pytest.approx(0.7)
|
||||
# 失败者：未匹配进入 Coast（lost=1、hits 不变）
|
||||
assert tracks[id_low].lost_count == 1, "低置信度轨道应落选"
|
||||
assert tracks[id_low].hits == 1
|
||||
assert tracks[id_low].confidence == pytest.approx(0.5)
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# NaN/Inf 防御（评审修复项）
|
||||
# ============================================================================
|
||||
def test_non_finite_detections_are_discarded():
|
||||
tracker = TargetTracker()
|
||||
nan = math.nan
|
||||
inf = math.inf
|
||||
|
||||
# 第 1 帧：建立正常轨道
|
||||
r = tracker.update([make_det(100.0, 100.0, 140.0, 140.0, 0.9)])
|
||||
assert r.tracked
|
||||
|
||||
# 第 2 帧：混入 NaN 坐标 / Inf 坐标 / NaN 置信度检测与一个正常检测：
|
||||
# 非有限检测全部被丢弃，仅正常检测参与关联，不新建轨道
|
||||
r = tracker.update([make_det(nan, 100.0, 140.0, 140.0, 0.9),
|
||||
make_det(100.0, -inf, 140.0, 140.0, 0.9),
|
||||
make_det(100.0, 100.0, nan, 140.0, 0.9),
|
||||
make_det(102.0, 102.0, 142.0, 142.0, nan),
|
||||
make_det(101.0, 101.0, 141.0, 141.0, 0.9)])
|
||||
assert tracker.active_count() == 1, "非有限检测不应新建轨道"
|
||||
assert r.tracked
|
||||
assert not r.is_predicted # 正常检测命中，实测输出
|
||||
# 主目标输出未被 NaN 污染
|
||||
assert math.isfinite(r.center[0])
|
||||
assert math.isfinite(r.center[1])
|
||||
assert r.box is not None
|
||||
assert math.isfinite(r.box.x1) and math.isfinite(r.box.y1)
|
||||
assert math.isfinite(r.box.x2) and math.isfinite(r.box.y2)
|
||||
assert math.isfinite(r.confidence)
|
||||
assert r.raw is not None # 实测帧 raw 有值
|
||||
|
||||
# 第 3 帧：仅含 NaN 检测 → 等价空帧，轨道 Coast 不崩溃
|
||||
r = tracker.update([make_det(nan, nan, nan, nan, nan)])
|
||||
assert r.tracked
|
||||
assert r.is_predicted
|
||||
assert r.lost_frames == 1
|
||||
assert r.raw is None # Coast 时 raw 为空（§2.3）
|
||||
assert math.isfinite(r.center[0])
|
||||
|
||||
# 持续 NaN 检测直至轨道死亡回收，全程不崩溃
|
||||
for _ in range(10):
|
||||
r = tracker.update([make_det(nan, 0.0, 10.0, 10.0, 0.9)])
|
||||
assert not r.tracked
|
||||
assert tracker.active_count() == 0
|
||||
|
||||
|
||||
# ============================================================================
|
||||
# 补充行为测试（§2.3 / §6.3 / §9.5）
|
||||
# ============================================================================
|
||||
|
||||
def test_lost_result_fields_are_empty():
|
||||
tracker = TargetTracker()
|
||||
r = tracker.update([]) # 空帧序列起点：无任何轨道
|
||||
assert not r.tracked
|
||||
assert r.primary_id is None
|
||||
assert r.box is None
|
||||
assert r.raw is None
|
||||
assert r.lost_frames == 0
|
||||
assert r.n_active == 0
|
||||
assert r.confidence == 0.0
|
||||
|
||||
# LastResult 返回同一快照（§9.5 跨线程读取入口）
|
||||
cached = tracker.last_result()
|
||||
assert not cached.tracked
|
||||
|
||||
|
||||
def test_coasting_fallback_chooses_nearest_to_frame_center():
|
||||
# Coast 兜底选举：无实测轨道时选离画面中心最近的 Coast 轨道（§6.3）。
|
||||
# 锁定延续优先：需先让锁定轨道死亡解除锁定，才会进入重新选举分支。
|
||||
cfg = TrackerConfig()
|
||||
cfg.max_lost_frames = 2
|
||||
tracker = TargetTracker(cfg)
|
||||
|
||||
# F1：角落目标（高置信度）先建立并被锁定
|
||||
tracker.update([make_det(10.0, 10.0, 30.0, 30.0, 0.95)])
|
||||
# F2：画面中心附近目标建立；角落目标丢失(1)
|
||||
tracker.update([make_det(300.0, 220.0, 340.0, 260.0, 0.9)])
|
||||
# F3：远处目标建立；角落丢失(2)、中心目标丢失(1)
|
||||
tracker.update([make_det(600.0, 450.0, 620.0, 470.0, 0.85)])
|
||||
# F4：全部丢失：角落丢失(3) > max_lost 死亡解除锁定；
|
||||
# 其余两条 Coast 存活 → 重新选举选离画面中心最近者
|
||||
r = tracker.update([], FrameShape(480, 640))
|
||||
assert r.tracked
|
||||
assert r.is_predicted
|
||||
assert r.center[0] == pytest.approx(320.0, abs=25.0) # 靠近画面中心 (320,240)
|
||||
assert r.center[1] == pytest.approx(240.0, abs=25.0)
|
||||
assert r.n_active == 2 # 角落轨道已回收
|
||||
|
||||
|
||||
def test_locked_primary_kept_while_measured():
|
||||
# 锁定延续优先于置信度选举（§6.3 ①）
|
||||
tracker = TargetTracker()
|
||||
r = tracker.update([make_det(100.0, 100.0, 120.0, 120.0, 0.4)])
|
||||
assert r.primary_id is not None
|
||||
locked_id = r.primary_id
|
||||
|
||||
# 高置信度新目标出现并持续，锁定目标也持续实测 → primary_id 不变
|
||||
for i in range(5):
|
||||
r = tracker.update([make_det(100.0 + i, 100.0, 120.0 + i, 120.0, 0.4),
|
||||
make_det(400.0, 300.0, 430.0, 330.0, 0.95)])
|
||||
assert r.primary_id is not None
|
||||
assert r.primary_id == locked_id
|
||||
assert not r.is_predicted
|
||||
|
||||
|
||||
def test_default_frame_center_is_320x240():
|
||||
# frame_shape 缺省按 640×480 画面中心 (320,240) 计算（§8.2 / §2.2）
|
||||
cfg = TrackerConfig()
|
||||
cfg.max_lost_frames = 2
|
||||
tracker = TargetTracker(cfg)
|
||||
|
||||
# F1：角落目标先建立并被锁定（高置信度）
|
||||
tracker.update([make_det(0.0, 0.0, 20.0, 20.0, 0.95)])
|
||||
# F2：左侧目标（中心 (280,240)，距默认中心 40px）建立
|
||||
tracker.update([make_det(270.0, 230.0, 290.0, 250.0, 0.9)])
|
||||
# F3：右侧目标（中心 (500,240)，距默认中心 180px）建立
|
||||
tracker.update([make_det(490.0, 230.0, 510.0, 250.0, 0.85)])
|
||||
# F4：全部丢失：锁定轨道死亡解除锁定，两条 Coast 轨道兜底选举
|
||||
r = tracker.update([])
|
||||
assert r.tracked
|
||||
assert r.is_predicted
|
||||
assert r.center[0] == pytest.approx(280.0, abs=30.0) # 左侧更近，当选
|
||||
assert r.center[1] == pytest.approx(240.0, abs=30.0)
|
||||
@@ -0,0 +1,159 @@
|
||||
"""tracklet_test.py —— Tracklet 与几何工具单元测试。
|
||||
|
||||
移植自 reference-c++/target_tracker/tests/tracklet_test.cpp。
|
||||
覆盖（视觉追踪技术路线 §3.2/§5.2/§6.1/§7）：框互转、IoU 边界、
|
||||
IsDead/IsCoasting 判定（严格大于）、Predict/Update/MarkMiss 计数语义。
|
||||
"""
|
||||
|
||||
import pytest
|
||||
|
||||
from target_tracker.tracklet import (BoxXYXY, BoxCXCYWH, Tracklet, compute_iou,
|
||||
to_center_size, to_xyxy)
|
||||
|
||||
|
||||
# ---- 框表示互转（§7） ----
|
||||
|
||||
def test_round_trip_preserves_values():
|
||||
box = BoxXYXY(10.0, 20.0, 50.0, 80.0)
|
||||
c = to_center_size(box)
|
||||
assert c.cx == 30.0 # (10+50)/2
|
||||
assert c.cy == 50.0 # (20+80)/2
|
||||
assert c.w == 40.0 # 50−10
|
||||
assert c.h == 60.0 # 80−20
|
||||
|
||||
back = to_xyxy(c)
|
||||
assert back.x1 == box.x1
|
||||
assert back.y1 == box.y1
|
||||
assert back.x2 == box.x2
|
||||
assert back.y2 == box.y2
|
||||
|
||||
|
||||
# ---- IoU 边界（§5.2） ----
|
||||
|
||||
def test_identical_boxes_return_one():
|
||||
a = BoxXYXY(10.0, 10.0, 30.0, 30.0)
|
||||
assert compute_iou(a, a) == pytest.approx(1.0, abs=1e-12) # 完全重合 = 1
|
||||
|
||||
|
||||
def test_disjoint_boxes_return_zero():
|
||||
a = BoxXYXY(0.0, 0.0, 10.0, 10.0)
|
||||
b = BoxXYXY(20.0, 20.0, 30.0, 30.0)
|
||||
assert compute_iou(a, b) == 0.0 # 零交集
|
||||
|
||||
|
||||
def test_touching_edges_return_zero():
|
||||
# 仅边/角相接：交集宽高为 0 → 交集面积 ≤ 0 返回 0（§5.2）
|
||||
a = BoxXYXY(0.0, 0.0, 10.0, 10.0)
|
||||
b = BoxXYXY(10.0, 0.0, 20.0, 10.0) # 右边缘相接
|
||||
assert compute_iou(a, b) == 0.0
|
||||
c = BoxXYXY(10.0, 10.0, 20.0, 20.0) # 角点相接
|
||||
assert compute_iou(a, c) == 0.0
|
||||
|
||||
|
||||
def test_empty_box_returns_zero():
|
||||
normal = BoxXYXY(0.0, 0.0, 10.0, 10.0)
|
||||
zero_area = BoxXYXY(5.0, 5.0, 5.0, 5.0) # 零面积
|
||||
inverted = BoxXYXY(10.0, 10.0, 0.0, 0.0) # 反向（负宽高）
|
||||
assert compute_iou(normal, zero_area) == 0.0
|
||||
assert compute_iou(zero_area, normal) == 0.0
|
||||
assert compute_iou(normal, inverted) == 0.0
|
||||
assert compute_iou(zero_area, zero_area) == 0.0
|
||||
|
||||
|
||||
def test_partial_overlap_is_correct():
|
||||
# A = [0,0,10,10]（面积 100），B = [5,5,15,15]（面积 100），交集 25
|
||||
a = BoxXYXY(0.0, 0.0, 10.0, 10.0)
|
||||
b = BoxXYXY(5.0, 5.0, 15.0, 15.0)
|
||||
assert compute_iou(a, b) == pytest.approx(25.0 / 175.0, abs=1e-12)
|
||||
assert compute_iou(b, a) == pytest.approx(25.0 / 175.0, abs=1e-12) # 对称性
|
||||
|
||||
|
||||
# ---- 构造与派生状态（§3.2 / §6.1） ----
|
||||
|
||||
def test_construction_initial_fields():
|
||||
t = Tracklet(7, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 1, 8)
|
||||
|
||||
assert t.track_id == 7
|
||||
assert t.lost_count == 0
|
||||
assert t.max_lost == 8
|
||||
assert t.confidence == 0.9
|
||||
assert t.cls_id == 1
|
||||
assert t.age == 1 # 创建时为 1（§3.2）
|
||||
assert t.hits == 1 # 创建时为 1（§3.2）
|
||||
assert t.last_center == (100.0, 100.0)
|
||||
assert t.last_box is not None # 创建当帧即赋 last_box（§7 步骤 5）
|
||||
assert t.last_box.x1 == 90.0
|
||||
assert t.last_box.y1 == 90.0
|
||||
assert t.last_box.x2 == 110.0
|
||||
assert t.last_box.y2 == 110.0
|
||||
assert not t.is_dead
|
||||
assert not t.is_coasting
|
||||
|
||||
|
||||
def test_is_dead_uses_strictly_greater():
|
||||
# IsDead 使用严格大于：lost_count == max_lost 仍存活（§6.1 注记 2）
|
||||
t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
|
||||
for _ in range(8):
|
||||
t.mark_miss()
|
||||
assert not t.is_dead, f"lost_count={t.lost_count} 未超过 max_lost 不应判死"
|
||||
assert t.lost_count == 8
|
||||
t.mark_miss() # lost_count = 9 > 8
|
||||
assert t.is_dead
|
||||
|
||||
|
||||
def test_is_coasting_when_lost_positive():
|
||||
t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
|
||||
assert not t.is_coasting
|
||||
t.mark_miss()
|
||||
assert t.is_coasting # lost_count > 0 即 Coast（§3.2）
|
||||
|
||||
|
||||
# ---- 生命周期方法计数语义（§7） ----
|
||||
|
||||
def test_predict_advances_age_and_updates_last_state():
|
||||
t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
|
||||
age_before = t.age
|
||||
t.predict() # 速度为零：位置不变，但 age+1、last_box 更新为预测框
|
||||
assert t.age == age_before + 1
|
||||
assert t.last_box is not None
|
||||
assert t.last_center[0] == pytest.approx(100.0, abs=1e-9)
|
||||
assert t.last_center[1] == pytest.approx(100.0, abs=1e-9)
|
||||
# hits / lost_count / confidence 不受预测影响
|
||||
assert t.hits == 1
|
||||
assert t.lost_count == 0
|
||||
assert t.confidence == 0.9
|
||||
|
||||
|
||||
def test_update_resets_lost_and_increments_hits():
|
||||
t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
|
||||
t.mark_miss()
|
||||
t.mark_miss()
|
||||
assert t.lost_count == 2
|
||||
assert t.is_coasting
|
||||
|
||||
age_before = t.age
|
||||
box = BoxXYXY(150.0, 160.0, 170.0, 180.0)
|
||||
t.update(box, 0.75, 2)
|
||||
|
||||
assert t.lost_count == 0 # 匹配成功清零（§7）
|
||||
assert t.hits == 2 # hits+1
|
||||
assert t.age == age_before + 1 # age+1
|
||||
assert t.confidence == 0.75
|
||||
assert t.cls_id == 2
|
||||
assert t.last_center == (160.0, 170.0) # ((150+170)/2, (160+180)/2)
|
||||
assert t.last_box is not None
|
||||
assert t.last_box.x1 == box.x1
|
||||
assert t.last_raw_box is not None # 实测框同步记录
|
||||
assert t.last_raw_box.x2 == box.x2
|
||||
assert not t.is_coasting
|
||||
|
||||
|
||||
def test_mark_miss_increments_lost_and_age():
|
||||
t = Tracklet(0, BoxCXCYWH(100.0, 100.0, 20.0, 20.0), 0.9, 0, 8)
|
||||
age_before = t.age
|
||||
t.mark_miss()
|
||||
assert t.lost_count == 1
|
||||
assert t.age == age_before + 1
|
||||
# MarkMiss 不影响 hits / confidence
|
||||
assert t.hits == 1
|
||||
assert t.confidence == 0.9
|
||||
Reference in New Issue
Block a user