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。
83 lines
2.7 KiB
Python
83 lines
2.7 KiB
Python
"""左右匹配器（§6.2）：硬约束 + 代价升序贪心一一配对；接口注入。
|
|
|
|
默认 GreedyMatcher 为纯函数（§4 构件表）；调用方可通过
|
|
StereoRanger(matcher=...) 注入匈牙利等替换实现而不动调用方（§10.3）。
|
|
|
|
硬约束（任一不满足直接拒绝配对，不进代价排序）：类别一致、|Δy| ≤ τ_y、
|
|
框高相对差 ≤ τ_h。视差域 [D_min, D_max] 不参与配对：越界属有效性门
|
|
（§6.2 已回填，与 §5 失败路径表/§6.3/§6.4/§2.3 一致）。
|
|
"""
|
|
|
|
from dataclasses import dataclass
|
|
from typing import List, Protocol, Sequence
|
|
|
|
from .config import StereoConfig
|
|
from .sampling import SamplePoint
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class MatchPair:
|
|
"""一一配对结果：左右下标 + 匹配代价（§6.2）。"""
|
|
|
|
left_index: int
|
|
right_index: int
|
|
cost: float
|
|
|
|
|
|
class Matcher(Protocol):
|
|
"""匹配器接口（§4：接口注入，默认贪心）。"""
|
|
|
|
def match(
|
|
self,
|
|
left: Sequence[SamplePoint],
|
|
right: Sequence[SamplePoint],
|
|
config: StereoConfig,
|
|
) -> List[MatchPair]:
|
|
...
|
|
|
|
|
|
class GreedyMatcher:
|
|
"""硬约束过滤 + 代价升序贪心：cost < 1 才接受，一一配对。"""
|
|
|
|
def match(
|
|
self,
|
|
left: Sequence[SamplePoint],
|
|
right: Sequence[SamplePoint],
|
|
config: StereoConfig,
|
|
) -> List[MatchPair]:
|
|
tau_y = config.tau_y_px
|
|
tau_h = config.tau_h_ratio
|
|
candidates = []
|
|
for i, point_l in enumerate(left):
|
|
if not point_l.finite:
|
|
continue
|
|
for j, point_r in enumerate(right):
|
|
if not point_r.finite:
|
|
continue
|
|
if point_l.box.class_id != point_r.box.class_id:
|
|
continue
|
|
dy = abs(point_l.center_y - point_r.center_y)
|
|
if dy > tau_y:
|
|
continue
|
|
height_max = max(point_l.box.height, point_r.box.height)
|
|
if height_max <= 0.0:
|
|
continue
|
|
dh_ratio = abs(point_l.box.height - point_r.box.height) / height_max
|
|
if dh_ratio > tau_h:
|
|
continue
|
|
cost = dy / tau_y + dh_ratio / tau_h
|
|
if cost >= 1.0:
|
|
continue
|
|
candidates.append((cost, i, j))
|
|
candidates.sort(key=lambda item: (item[0], item[1], item[2]))
|
|
used_left = set()
|
|
used_right = set()
|
|
pairs: List[MatchPair] = []
|
|
for cost, i, j in candidates:
|
|
if i in used_left or j in used_right:
|
|
continue
|
|
pairs.append(MatchPair(left_index=i, right_index=j, cost=cost))
|
|
used_left.add(i)
|
|
used_right.add(j)
|
|
return pairs
|