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

utils/stereo_ranger/consistency.py

@@ -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()