code-test-drone/utils/stereo_ranger/stereo_ranger.py

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