"""StereoRanger —— 双目稀疏目标测距（纯算法，规格 docs/双目测距方案.md）。 输入为左右目各自、校正后原生坐标系下的检测框列表；不做目标识别，不做 图像采集/立体校正。单帧主流程：关联（§6）→ 视差取点（§5.1）→ Z=F·B/D 与 (X, Y, R)（§4.2）→ 有效性（§5.4）→ 可选时间滤波（§7）。 依赖仅为标准库。 """ import logging import math import time from typing import Dict, List, Optional, Sequence, Tuple from .config import StereoConfig from .types import BBoxXYXY, Detection, RangeReport logger = logging.getLogger("stereo_ranger") DISP_SOURCE_EDGE = "left_edge" DISP_SOURCE_CENTER = "center" def focal_from_hfov(image_width_px: float, hfov_deg: float) -> float: """由水平视场与图像宽近似像素焦距：F = (W/2) / tan(HFOV/2)（§5.3）。""" return (image_width_px / 2.0) / math.tan(math.radians(hfov_deg) / 2.0) def range_error_abs(z_m: float, fx_px: float, baseline_m: float, delta_d_px: float = 1.0) -> float: """绝对距离误差估计 δZ ≈ Z^2·δD/(F·B)（§5.2）。""" return z_m * z_m * delta_d_px / (fx_px * baseline_m) def range_error_rel(z_m: float, fx_px: float, baseline_m: float, delta_d_px: float = 1.0) -> float: """相对距离误差 δZ/Z ≈ Z·δD/(F·B)（§5.2）。""" return z_m * delta_d_px / (fx_px * baseline_m) class StereoRanger: """双目稀疏目标测距器。 用法：每帧调用一次 update()，传入同一逻辑帧的左右目检测结果。 """ def __init__(self, config: Optional[StereoConfig] = None): self._cfg = config if config is not None else StereoConfig() self._z_state: Dict[Tuple[int, int], float] = {} # (cls_id, track_id) -> 滤波 Z self._warn_at: Dict[str, float] = {} @property def config(self) -> StereoConfig: return self._cfg def reset(self) -> None: self._z_state.clear() self._warn_at.clear() def update( self, left: Sequence[Detection], right: Sequence[Detection], frame_sequence: int = 0, timestamp_s: Optional[float] = None, ) -> List[RangeReport]: """单帧主流程：返回报告列表（每个左目检测一条，保持左目输入顺序）。 配对成功且测距有效的报告 valid=True；配对成功但超出有效工作域的 valid=False（距离仍给出，§5.4）；未配对的左目检测 matched=False， 仅发布单目方位（§6 失败处理）。 """ pairs = self._match(left, right) matched_r: Dict[int, int] = {l_idx: r_idx for l_idx, r_idx, _ in pairs} reports: List[RangeReport] = [] for l_idx, det_l in enumerate(left): r_idx = matched_r.get(l_idx) if r_idx is None: reports.append(self._unmatched_report(det_l, frame_sequence, timestamp_s)) continue report = self._build_report(det_l, right[r_idx], frame_sequence, timestamp_s) reports.append(report) return reports # ---- 内部实现 ---- def _match(self, left: Sequence[Detection], right: Sequence[Detection]) -> List[Tuple[int, int, Detection]]: """硬约束过滤 + 代价升序贪心一一配对（§6）。返回 (左下标, 右下标, 左框)。""" cfg = self._cfg d_lo, d_hi = cfg.disparity_bounds_px() candidates: List[Tuple[int, int, Detection, float]] = [] for i, det_l in enumerate(left): for j, det_r in enumerate(right): if det_l.cls_id != det_r.cls_id: continue dy = abs(det_l.cy - det_r.cy) if dy > cfg.vertical_tau_px: continue h_max = max(det_l.height, det_r.height) if h_max <= 0.0: continue dh_ratio = abs(det_l.height - det_r.height) / h_max if dh_ratio > cfg.height_ratio_tau: continue cost = dy / cfg.vertical_tau_px + dh_ratio / cfg.height_ratio_tau if cost >= 1.0: continue d_edge = det_l.x1 - det_r.x1 if not (d_lo <= d_edge <= d_hi): d_center = det_l.cx - det_r.cx if cfg.use_center_fallback and (d_lo <= d_center <= d_hi): self._warn("center_fallback", "左边缘视差越出窗口，回退中心视差（宽度差可能混入）") else: continue same_track = det_l.track_id > 0 and det_l.track_id == det_r.track_id candidates.append((0 if same_track else 1, cost, i, j, det_l)) candidates.sort(key=lambda c: (c[0], c[1])) matched_l: set = set() used_r: set = set() pairs: List[Tuple[int, int, Detection]] = [] for _, _, i, j, det_l in candidates: if i in matched_l or j in used_r: continue pairs.append((i, j, det_l)) matched_l.add(i) used_r.add(j) return pairs def _build_report(self, det_l: Detection, det_r: Detection, seq: int, ts: Optional[float]) -> RangeReport: cfg = self._cfg d_edge = det_l.x1 - det_r.x1 d_lo, d_hi = cfg.disparity_bounds_px() if d_lo <= d_edge <= d_hi: d = d_edge source = DISP_SOURCE_EDGE else: d = det_l.cx - det_r.cx source = DISP_SOURCE_CENTER z = cfg.fx_px * cfg.baseline_m / d report = RangeReport( frame_sequence=seq, timestamp_s=ts, class_id=det_l.cls_id, track_id=det_l.track_id if det_l.track_id != 0 else det_r.track_id, matched=True, valid=False, # §5.4 判定见下 forward_distance_m=z, disparity_px=d, disparity_source=source, confidence=min(det_l.conf, det_r.conf), centre_pixel_x_mid=(det_l.cx + det_r.cx) / 2.0, centre_pixel_y_mid=det_l.cy, ref_bbox=(det_l.x1, det_l.y1, det_l.x2, det_l.y2), ) err = range_error_abs(z, cfg.fx_px, cfg.baseline_m, cfg.disp_noise_px) if d >= cfg.disparity_min_px and err <= cfg.range_tol_m: report.valid = True z_out = self._filter_z(report.class_id, report.track_id, z) report.forward_distance_m = z_out report.error_est_m = range_error_abs(z_out, cfg.fx_px, cfg.baseline_m, cfg.disp_noise_px) else: report.error_est_m = err self._fill_xyz(report, det_l.cx) return report def _fill_xyz(self, report: RangeReport, x_l_px: float) -> None: """由 Z 与左目（参考目）框中心像素回算左目系 X, Y, R（§4.2）。 注意：方位源 centre_pixel_x_mid 属于基线中点虚拟相机（§11.3）， 而 X 定义在左目坐标系，二者相差 B/2，不得混用。 """ cfg = self._cfg z = report.forward_distance_m x = (x_l_px - cfg.cx_px) * z / cfg.fx_px y = (report.centre_pixel_y_mid - cfg.cy_px) * z / cfg.fy_px report.lateral_m = x report.vertical_m = y report.slant_range_m = math.sqrt(x * x + y * y + z * z) def _filter_z(self, cls_id: int, track_id: int, z: float) -> float: """可选一阶低通 + 跳变重置（§7）。track_id 为 0（无轨迹）时不过滤。""" cfg = self._cfg if not cfg.filter_enabled or track_id == 0: return z key = (cls_id, track_id) prev = self._z_state.get(key) if prev is None: self._z_state[key] = z return z if abs(z - prev) / prev > cfg.filter_jump_ratio: self._z_state[key] = z # 跳变：重置 return z z_out = (1.0 - cfg.filter_alpha) * prev + cfg.filter_alpha * z self._z_state[key] = z_out return z_out def _unmatched_report(self, det: Detection, seq: int, ts: Optional[float]) -> RangeReport: return RangeReport( frame_sequence=seq, timestamp_s=ts, class_id=det.cls_id, track_id=det.track_id, matched=False, valid=False, confidence=det.conf, centre_pixel_x_mid=det.cx, centre_pixel_y_mid=det.cy, ref_bbox=(det.x1, det.y1, det.x2, det.y2), ) def _warn(self, category: str, message: str) -> None: """节流诊断（§7：失败日志须节流）。""" now = time.monotonic() last = self._warn_at.get(category) if last is not None and now - last < self._cfg.warn_interval_s: return self._warn_at[category] = now logger.warning("[%s] %s", category, message)