双目测距方案重构与其他小修改

utils/stereo_ranger/stereo_ranger.py

@@ -1,215 +1,227 @@
-"""StereoRanger —— 双目稀疏目标测距（纯算法，规格 docs/双目测距方案.md）。
+"""StereoRanger —— 双目稀疏目标测距（规格 docs/双目测距技术路线.md）。
 
-输入为左右目各自、校正后原生坐标系下的检测框列表；不做目标识别，不做
-图像采集/立体校正。单帧主流程：关联（§6）→ 视差取点（§5.1）→
-Z=F·B/D 与 (X, Y, R)（§4.2）→ 有效性（§5.4）→ 可选时间滤波（§7）。
-依赖仅为标准库。
+单周期五步（§5）：① 同帧契约检查 → ② 取点 → ③ 左右匹配 →
+④ 配对目标：测距 + 方位源 + 有效性 + 滤波 → ⑤ 单目退化发布。
+
+输入为左右目各自、校正后原生坐标系下的检测框；不做目标识别，不做图像
+采集/立体校正。纯计算单元（§11.3）：单周期处理无阻塞、无 IO；同一实例
+不得并发调用 update()。
 """
 
 import logging
-import math
-import time
-from typing import Dict, List, Optional, Sequence, Tuple
+from typing import List, Optional, Sequence
 
 from .config import StereoConfig
-from .types import BBoxXYXY, Detection, RangeReport
+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")
 
-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)
+_CONFIDENCE_DEGRADE_FACTOR = 0.5  # ⚠占位：连续跳变 ≥ N 帧的置信度降级系数（§6.4）
 
 
 class StereoRanger:
-    """双目稀疏目标测距器。
+    """双目稀疏目标测距器：每帧调用一次 update()，传入同帧左右目检测结果。"""
 
-    用法：每帧调用一次 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] = {}
+    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._cfg
+        return self._config
+
+    @property
+    def matcher(self) -> Matcher:
+        return self._matcher
 
     def reset(self) -> None:
-        self._z_state.clear()
-        self._warn_at.clear()
+        """清空全部运行期状态（滤波 / 一致性 / 隔离 / 节流计数）。"""
+        self._store.clear()
+        self._warn.clear()
+        self._frame_index = 0
 
-    def update(
-        self,
-        left: Sequence[Detection],
-        right: Sequence[Detection],
-        frame_sequence: int = 0,
-        timestamp_s: Optional[float] = None,
-    ) -> List[RangeReport]:
-        """单帧主流程：返回报告列表（每个左目检测一条，保持左目输入顺序）。
+    def update(self, left: DetectionFrame, right: DetectionFrame, now_ms: int = 0) -> List[RangeRecord]:
+        """单周期处理（§5）：返回本帧记录列表。
 
-        配对成功且测距有效的报告 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))
+        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
-            report = self._build_report(det_l, right[r_idx], frame_sequence, timestamp_s)
-            reports.append(report)
-        return reports
+            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 _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 _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)
 
-    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),
+        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,
         )
-        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)
+        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:
-            report.error_est_m = err
-        self._fill_xyz(report, det_l.cx)
-        return report
+            self._store.distance_filter.invalidate(key)
+            z_out = None
 
-    def _fill_xyz(self, report: RangeReport, x_l_px: float) -> None:
-        """由 Z 与左目（参考目）框中心像素回算左目系 X, Y, R（§4.2）。
+        # 参考斜距与已发布的前向距离同基准：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)
 
-        注意：方位源 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)
+        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 _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,
+    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,
-            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),
+            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 _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:
+    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
-        self._warn_at[category] = now
-        logger.warning("[%s] %s", category, message)
+        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