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。
228 lines
8.9 KiB
Python
228 lines
8.9 KiB
Python
"""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
|