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。
120 lines
4.4 KiB
Python
120 lines
4.4 KiB
Python
"""目标状态隔离（§4）：track_id 优先 + 框位置最近邻兜底。
|
|
|
|
目标隔离键：track_id 可用（左右一致且 > 0）时优先以 (track, 类别, track_id)
|
|
为键——上游 target_tracker 已在链路中，规格 §4 已按此冻结；否则回退类别 +
|
|
框位置最近邻（isolation_gate_px 门限，见包 README「实现决策」）。
|
|
|
|
连续 filter_reset_frames 帧未再出现的目标状态整体清理（位置 + 距离滤波 +
|
|
一致性），避免最近邻把新目标接到陈旧状态上。
|
|
"""
|
|
|
|
import math
|
|
from dataclasses import dataclass
|
|
from typing import Dict, Hashable, Optional, Tuple
|
|
|
|
from .config import StereoConfig
|
|
from .consistency import ConsistencyMonitor
|
|
from .distance_filter import DistanceFilter
|
|
|
|
_KIND_TRACK = "track"
|
|
_KIND_NEAREST = "nn"
|
|
|
|
|
|
@dataclass
|
|
class _TargetState:
|
|
class_id: int
|
|
track_id: Optional[int]
|
|
center_px: Tuple[float, float]
|
|
last_seen_frame: int
|
|
|
|
|
|
class TargetStore:
|
|
"""每目标状态容器：隔离键解析 + 帧末老化清理（§11.1 状态清单）。"""
|
|
|
|
def __init__(self, config: StereoConfig) -> None:
|
|
self._config = config
|
|
self._distance_filter = DistanceFilter(
|
|
config.smooth_alpha, config.jump_reset_m, config.filter_reset_frames
|
|
)
|
|
self._consistency = ConsistencyMonitor(
|
|
config.consistency_jump_px, config.consistency_N_frames, config.consistency_M_frames
|
|
)
|
|
self._states: Dict[Hashable, _TargetState] = {}
|
|
self._next_nearest_id = 0
|
|
|
|
@property
|
|
def distance_filter(self) -> DistanceFilter:
|
|
return self._distance_filter
|
|
|
|
@property
|
|
def consistency(self) -> ConsistencyMonitor:
|
|
return self._consistency
|
|
|
|
@property
|
|
def count(self) -> int:
|
|
"""当前存活的目标状态数。"""
|
|
return len(self._states)
|
|
|
|
def resolve(
|
|
self,
|
|
class_id: int,
|
|
track_id: Optional[int],
|
|
center_px: Tuple[float, float],
|
|
frame_index: int,
|
|
) -> Hashable:
|
|
"""解析（或新建）目标 key，并刷新该目标最近位置与出现帧。"""
|
|
if track_id is not None and track_id > 0:
|
|
key: Hashable = (_KIND_TRACK, class_id, track_id)
|
|
self._states[key] = _TargetState(class_id, track_id, center_px, frame_index)
|
|
return key
|
|
key = self._nearest_key(class_id, center_px, frame_index)
|
|
if key is None:
|
|
self._next_nearest_id += 1
|
|
key = (_KIND_NEAREST, class_id, self._next_nearest_id)
|
|
self._states[key] = _TargetState(class_id, None, center_px, frame_index)
|
|
else:
|
|
state = self._states[key]
|
|
state.center_px = center_px
|
|
state.last_seen_frame = frame_index
|
|
return key
|
|
|
|
def end_frame(self, frame_index: int) -> None:
|
|
"""帧末收尾：未出现的目标记滤波失效，到期整体清理三类状态。"""
|
|
stale: List[Hashable] = []
|
|
for key, state in self._states.items():
|
|
if state.last_seen_frame == frame_index:
|
|
continue
|
|
self._distance_filter.invalidate(key)
|
|
if frame_index - state.last_seen_frame >= self._config.filter_reset_frames:
|
|
stale.append(key)
|
|
for key in stale:
|
|
del self._states[key]
|
|
self._distance_filter.drop(key)
|
|
self._consistency.drop(key)
|
|
|
|
def clear(self) -> None:
|
|
self._states.clear()
|
|
self._distance_filter.clear()
|
|
self._consistency.clear()
|
|
self._next_nearest_id = 0
|
|
|
|
def _nearest_key(
|
|
self,
|
|
class_id: int,
|
|
center_px: Tuple[float, float],
|
|
frame_index: int,
|
|
) -> Optional[Hashable]:
|
|
"""同类别、isolation_gate_px 内、本帧尚未被占用的最近状态。"""
|
|
gate = self._config.isolation_gate_px
|
|
best_key: Optional[Hashable] = None
|
|
best_distance: Optional[float] = None
|
|
for key, state in self._states.items():
|
|
if key[0] != _KIND_NEAREST or state.class_id != class_id:
|
|
continue
|
|
if state.last_seen_frame == frame_index:
|
|
continue # 本帧已被其他框接管，一一归属
|
|
distance = math.hypot(center_px[0] - state.center_px[0], center_px[1] - state.center_px[1])
|
|
if distance <= gate and (best_distance is None or distance < best_distance):
|
|
best_key, best_distance = key, distance
|
|
return best_key
|