"""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