keita
df91b1110e
归档 legacy/：stereo_yolo/stereo_ranger/target_tracker 三包 + 175 条测试 + demo/配置/权重/三份规格；运行链路不再依赖，默认 pytest 446 条，归档测试按需运行。 目标有效性第二判据改为 LOS 运动学一致性（pod_bridge/los_consistency.py），删除 require_vision；pod_locked() 叠加该判据，状态机随之后退重搜。 自定义返航（task_runner/return_home.py）：保持转场高度回起飞点 → 恢复初始航向 → 垂直降落，不再爬 RTL_RETURN_ALT；未起飞/无起飞点/超时/异常仍退回 PX4 原生 RTL。 任务链安全兜底：主循环异常→紧急返航、stop 幂等、landing_stop 保留遥测、in_air 流复用+超时、解锁/起飞与吊舱识别仅执行一次、rtl_timeout 上限。 入口共享件拆到 utils/sitl_support.py；pod_sitl_test.py→l1_sitl_test.py；删除旧架构 test.py；相机脚本移入 tools/。 新增 .gitignore 并剔除历史误跟踪的 59 个 .pyc；requirements 按实际依赖重写；文档全面对齐，新增 legacy/README.md 与 tools/README.md。
75 lines
2.9 KiB
Python
75 lines
2.9 KiB
Python
"""一致性监控（§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()
|