精简仓库结构并增强任务链路：归档双目可选后端、新增LOS判据与自定义返航
归档 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。
This commit is contained in:
keita
@@ -0,0 +1,269 @@
|
||||
"""demo_stereo_ranger.py —— 双目稀疏目标测距演示（规格 docs/双目测距技术路线.md）。
|
||||
|
||||
全部场景用**解析构造**的左右目检测框驱动：不需要相机、不需要图像、不需要
|
||||
飞控，几何是闭式可验证的（规格 §11.4）。覆盖：
|
||||
|
||||
① 逼近：三条有效性门依次生效（域外 → 预算门 → 有效）+ 时间滤波平滑
|
||||
② 远场域外：valid = false 但 c_mid（虚拟双目中心方位）照常发布
|
||||
③ 单目退化：仅一目检出 → 仅方位记录
|
||||
④ 非同帧拒收：左右帧标识不一致 → 整帧拒收 + 告警节流
|
||||
⑤ 一致性监控：持续视差跳变 → N 帧降置信度 → M 帧判无效
|
||||
⑥ 多目标：贪心一一配对，各自独立状态；未匹配右目退化为仅方位
|
||||
|
||||
用法：
|
||||
python utils/demo_stereo_ranger.py # 默认配置
|
||||
python utils/demo_stereo_ranger.py --config <path> # 指定配置
|
||||
|
||||
⚠ 所有参数均为占位值（焦距/标定待厂商数据）：标定与地面真值验证完成前，
|
||||
本模块输出仅限静态标靶/回放/台架验证，不得作为飞行控制决策依据
|
||||
（规格 §12.3 红线）。
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import logging
|
||||
from pathlib import Path
|
||||
from typing import List, Optional, Tuple
|
||||
|
||||
from stereo_ranger import (
|
||||
Detection,
|
||||
DetectionFrame,
|
||||
RangeRecord,
|
||||
StereoConfig,
|
||||
StereoRanger,
|
||||
load_from_json,
|
||||
)
|
||||
|
||||
logger = logging.getLogger("demo_stereo_ranger")
|
||||
|
||||
_DEMO_DIR = Path(__file__).resolve().parent
|
||||
_DEFAULT_CONFIG = _DEMO_DIR / "config" / "stereo_ranger.json"
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 合成框对
|
||||
|
||||
|
||||
def boxes_for(
|
||||
cfg: StereoConfig,
|
||||
x_center_m: float,
|
||||
y_m: float,
|
||||
z_m: float,
|
||||
*,
|
||||
w_m: float = 0.5,
|
||||
h_m: float = 0.4,
|
||||
class_id: int = 0,
|
||||
track_id: Optional[int] = None,
|
||||
confidence: float = 0.95,
|
||||
) -> Tuple[Detection, Detection]:
|
||||
"""模组中心系 (X_C, Y, Z) 的理想目标 → (左目框, 右目框)。
|
||||
|
||||
两目框同宽，因此边缘视差 = 中心视差 = F·B/Z（§3.2/§6.6 的精确投影）。
|
||||
"""
|
||||
x_l = cfg.cx_px + cfg.fx_px * (x_center_m + cfg.baseline_m / 2.0) / z_m
|
||||
x_r = cfg.cx_px + cfg.fx_px * (x_center_m - cfg.baseline_m / 2.0) / z_m
|
||||
y = cfg.cy_px + cfg.fy_px * y_m / z_m
|
||||
w_px = cfg.fx_px * w_m / z_m
|
||||
h_px = cfg.fy_px * h_m / z_m
|
||||
|
||||
def _box(cx_px: float) -> Detection:
|
||||
return Detection(
|
||||
cx_px - w_px / 2.0,
|
||||
y - h_px / 2.0,
|
||||
cx_px + w_px / 2.0,
|
||||
y + h_px / 2.0,
|
||||
confidence=confidence,
|
||||
class_id=class_id,
|
||||
track_id=track_id,
|
||||
)
|
||||
|
||||
return _box(x_l), _box(x_r)
|
||||
|
||||
|
||||
def pair_with_disparity(d_px: float, *, y_center: float = 500.0) -> Tuple[Detection, Detection]:
|
||||
"""构造视差恰为 d_px 的等宽框对（d 取 0.5 的整数倍时浮点精确）。"""
|
||||
width, height = 60.0, 40.0
|
||||
left = Detection(1000.0 - width / 2, y_center - height / 2, 1000.0 + width / 2, y_center + height / 2)
|
||||
right = Detection(
|
||||
1000.0 - d_px - width / 2,
|
||||
y_center - height / 2,
|
||||
1000.0 - d_px + width / 2,
|
||||
y_center + height / 2,
|
||||
)
|
||||
return left, right
|
||||
|
||||
|
||||
def frame(frame_id: int, detections: List[Detection]) -> DetectionFrame:
|
||||
return DetectionFrame(frame_id=frame_id, detections=detections)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 打印
|
||||
|
||||
|
||||
def describe(record: RangeRecord) -> str:
|
||||
z_text = f"{record.forward_distance_m:7.3f}" if record.forward_distance_m is not None else " — "
|
||||
r_text = f"{record.slant_range_m:7.3f}" if record.slant_range_m is not None else " — "
|
||||
d_text = f"{record.disparity_px:7.2f}" if record.disparity_px is not None else " — "
|
||||
return (
|
||||
f"[F{record.frame_id:02d}] {record.degradation.name:<12} valid={int(record.valid)} "
|
||||
f"Z={z_text} m R={r_text} m D={d_text} px "
|
||||
f"c_mid=({record.center_mid_px[0]:7.1f},{record.center_mid_px[1]:6.1f}) "
|
||||
f"conf={record.confidence:.2f}"
|
||||
)
|
||||
|
||||
|
||||
def log_scene(title: str) -> None:
|
||||
logger.info("")
|
||||
logger.info("── %s", title)
|
||||
|
||||
|
||||
def log_config_summary(cfg: StereoConfig) -> None:
|
||||
logger.info("配置：B=%.3f m F=%.1f px (F·B=%.2f px·m)", cfg.baseline_m, cfg.fx_px, cfg.focal_baseline_px_m)
|
||||
logger.info(
|
||||
" 视差域 [%.1f, %.1f] px ⇔ 距离域 [%.2f, %.2f] m（视差域权威）",
|
||||
cfg.disparity_min_px,
|
||||
cfg.disparity_max_px,
|
||||
cfg.z_min_m,
|
||||
cfg.z_max_m,
|
||||
)
|
||||
logger.info(
|
||||
" 误差预算 δZ(δD=1px) ≤ %.1f m ⇒ 有效远界 %.2f m；一致性 N=%d / M=%d（跳变门限 %.1f px）",
|
||||
cfg.max_delta_z_m,
|
||||
cfg.z_error_budget_max_m,
|
||||
cfg.consistency_N_frames,
|
||||
cfg.consistency_M_frames,
|
||||
cfg.consistency_jump_px,
|
||||
)
|
||||
logger.info(
|
||||
" 滤波 α=%.2f（≈%.0f 帧群延迟）跳变重置 %.1f m；目标隔离门限 %.0f px",
|
||||
cfg.smooth_alpha,
|
||||
1.0 / cfg.smooth_alpha,
|
||||
cfg.jump_reset_m,
|
||||
cfg.isolation_gate_px,
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 场景
|
||||
|
||||
|
||||
def scene_approach(ranger: StereoRanger, cfg: StereoConfig) -> None:
|
||||
"""① 逼近：Z 20 → 6 m，三条有效性门依次生效，有效帧起低通平滑。"""
|
||||
ranger.reset() # 场景独立：清空跨场景的滤波/一致性/隔离状态
|
||||
log_scene("① 逼近场景：目标沿光轴 20 → 6 m（1 m/帧），观察三道门与滤波")
|
||||
logger.info(" %-7s %-9s %-6s %-7s %s", "真值Z", "D(px)", "valid", "输出Z", "说明")
|
||||
user_ms = 1000
|
||||
for index, z_true in enumerate([20.0, 18.0, 16.0, 14.0, 12.0, 11.0, 10.0, 9.0, 8.0, 7.0, 6.0], start=1):
|
||||
left, right = boxes_for(cfg, 0.0, 0.0, z_true)
|
||||
record = ranger.update(frame(index, [left]), frame(index, [right]), now_ms=user_ms)[0]
|
||||
user_ms += 50
|
||||
if record.disparity_px is not None and record.disparity_px < cfg.disparity_min_px:
|
||||
note = "门① 视差域外 → invalid"
|
||||
elif not record.valid and record.disparity_px is not None:
|
||||
note = "门② 误差预算不满足 → invalid"
|
||||
elif record.forward_distance_m is not None and abs(record.forward_distance_m - z_true) > 1e-9:
|
||||
note = f"有效（滤波后，真值 {z_true:.2f}）"
|
||||
else:
|
||||
note = "有效（滤波器初始化）"
|
||||
logger.info(
|
||||
" %-7.2f %-9.2f %-6d %-7s %s",
|
||||
z_true,
|
||||
record.disparity_px if record.disparity_px is not None else float("nan"),
|
||||
int(record.valid),
|
||||
f"{record.forward_distance_m:.3f}" if record.forward_distance_m is not None else "—",
|
||||
note,
|
||||
)
|
||||
logger.info(
|
||||
" → 远场：域外或 δZ 超预算时诚实输出 invalid（不强行估计）；"
|
||||
"有效帧之间才做一阶低通，输出滞后于真值属预期（消费侧 PID 与限幅吸收）"
|
||||
)
|
||||
|
||||
|
||||
def scene_far_field_bearing(ranger: StereoRanger, cfg: StereoConfig) -> None:
|
||||
"""② 远场域外：无距离但方位照常发布（远场定速接近策略的输入基础）。"""
|
||||
ranger.reset()
|
||||
log_scene("② 远场域外：Z=25 m（D < 下限），valid=false 但 c_mid 仍发布")
|
||||
x_center_m = 0.6
|
||||
left, right = boxes_for(cfg, x_center_m, 0.0, 25.0)
|
||||
record = ranger.update(frame(20, [left]), frame(20, [right]), now_ms=2000)[0]
|
||||
logger.info(" %s", describe(record))
|
||||
logger.info(
|
||||
" c_mid.x=%.1f = cx + F·X_C/Z = %.1f（虚拟双目中心，无近场视差偏差）",
|
||||
record.center_mid_px[0],
|
||||
cfg.cx_px + cfg.fx_px * x_center_m / 25.0,
|
||||
)
|
||||
logger.info(" → 消费侧据此走\"方位闭环 + 定速接近\"（D4），而不是使用不可靠的远场估计")
|
||||
|
||||
|
||||
def scene_monocular_degradation(ranger: StereoRanger, cfg: StereoConfig) -> None:
|
||||
"""③ 单目退化：仅一目检出 → 仅方位记录（v1.0 接受近场视差偏差）。"""
|
||||
ranger.reset()
|
||||
log_scene("③ 单目退化：仅左目检出")
|
||||
left, _ = boxes_for(cfg, 0.3, 0.0, 4.0, confidence=0.88)
|
||||
records = ranger.update(frame(21, [left]), frame(21, []), now_ms=3000)
|
||||
logger.info(" %s", describe(records[0]))
|
||||
logger.info(" → 退化 = 仅方位：报该目框中心作方位，valid=false 无距离（§2.2 记录两态）")
|
||||
|
||||
|
||||
def scene_frame_mismatch(ranger: StereoRanger) -> None:
|
||||
"""④ 非同帧拒收：左右帧标识不一致 → 整帧拒收（WARN 节流）。"""
|
||||
ranger.reset()
|
||||
log_scene("④ 同帧契约：左右帧标识不一致（F30 vs F31）")
|
||||
left_pair = pair_with_disparity(9.6)
|
||||
for _ in range(2):
|
||||
records = ranger.update(frame(30, [left_pair[0]]), frame(31, [left_pair[1]]))
|
||||
logger.info(" 两次违约共输出记录 %d 条（应为 0）；告警按节流规范只在第 1 次打印", len(records))
|
||||
logger.info(" → 不同曝光时刻的框配对解算没有物理意义，宁可拒收不可给值（§2.1）")
|
||||
|
||||
|
||||
def scene_consistency(ranger: StereoRanger) -> None:
|
||||
"""⑤ 一致性监控：持续视差跳变 → N 帧降置信度 → M 帧判无效。"""
|
||||
ranger.reset()
|
||||
log_scene("⑤ 一致性监控：视差逐帧跳变 4 px（门限 3 px）")
|
||||
for index, d_px in enumerate([9.5, 13.5, 17.5, 21.5, 25.5, 29.5], start=40):
|
||||
left, right = pair_with_disparity(d_px)
|
||||
record = ranger.update(frame(index, [left]), frame(index, [right]), now_ms=4000)[0]
|
||||
logger.info(" %s", describe(record))
|
||||
logger.info(" → 只能降级、不能纠偏：连续异常帧数达 N 降置信度、达 M 判无效（§6.7）")
|
||||
|
||||
|
||||
def scene_multi_target(ranger: StereoRanger, cfg: StereoConfig) -> None:
|
||||
"""⑥ 多目标：贪心一一配对；未配对的右目框退化为仅方位记录。"""
|
||||
ranger.reset()
|
||||
log_scene("⑥ 多目标：两个同类别目标 + 一个孤立右目框")
|
||||
left_a, right_a = boxes_for(cfg, -1.0, 0.0, 8.0)
|
||||
left_b, right_b = boxes_for(cfg, 1.2, 0.0, 9.0)
|
||||
extra_right = Detection(360.0, 480.0, 420.0, 540.0)
|
||||
records = ranger.update(frame(50, [left_a, left_b]), frame(50, [right_a, right_b, extra_right]), now_ms=5000)
|
||||
for record in records:
|
||||
logger.info(" %s", describe(record))
|
||||
logger.info(" → 目标数极少（1~2）时 O(n²) 贪心与全局最优几乎必然同解；匹配器接口可替换（§10.3）")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 入口
|
||||
|
||||
|
||||
def run_demo(config_path: str) -> None:
|
||||
cfg = load_from_json(config_path)
|
||||
logger.info("配置来源：%s", config_path)
|
||||
log_config_summary(cfg)
|
||||
logger.warning("⚠ 占位参数未标定：仅限静态标靶/回放/台架验证，不得作为飞行控制决策依据（§12.3）")
|
||||
|
||||
ranger = StereoRanger(cfg) # 演示共用一个实例，状态跨场景延续（逼近 → 后续场景）
|
||||
scene_approach(ranger, cfg)
|
||||
scene_far_field_bearing(ranger, cfg)
|
||||
scene_monocular_degradation(ranger, cfg)
|
||||
scene_frame_mismatch(ranger)
|
||||
scene_consistency(ranger)
|
||||
scene_multi_target(ranger, cfg)
|
||||
logger.info("")
|
||||
logger.info("演示结束：以上全部为合成框对的解析结果，无真实图像与硬件参与")
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser(description="双目稀疏目标测距演示（合成框对，离线）")
|
||||
parser.add_argument("--config", default=str(_DEFAULT_CONFIG), help="配置 JSON 路径")
|
||||
args = parser.parse_args()
|
||||
logging.basicConfig(level=logging.INFO, format="%(levelname)s %(message)s")
|
||||
run_demo(args.config)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user