精简仓库结构并增强任务链路：归档双目可选后端、新增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,23 @@
|
||||
# tools/ —— 临时/台架小工具（不参与运行链路）
|
||||
|
||||
这里放的是**一次性或台架用**的脚本，不被 `pod_bridge` / `task_runner` 引用，
|
||||
也不在 pytest 收集范围内。移动进来是为了让仓库根目录只保留现行入口。
|
||||
|
||||
| 文件 | 用途 | 依赖 | 输出 |
|
||||
|---|---|---|---|
|
||||
| `camera.py` | 双目相机棋盘格采集 + 标定 | `opencv-python`、`numpy` | `stereo_calib_images/`（图像）、`stereo_calib_params.npz`（内参/外参） |
|
||||
| `camera_test.py` | 单相机取流冒烟测试（索引/分辨率/帧率） | `opencv-python` | 无（窗口预览） |
|
||||
| `video_test.py` | HDMI 全屏预览（第二屏） | `opencv-python`、`numpy` | 无（窗口预览） |
|
||||
|
||||
## 注意
|
||||
|
||||
- 这些脚本会在**当前工作目录**下产出文件（`stereo_calib_images/`、`stereo_calib_params.npz`）。
|
||||
建议在 `tools/` 内运行，避免污染仓库根：
|
||||
```bash
|
||||
cd tools
|
||||
"D:/program file/conda/envs/pytorch/python.exe" camera.py
|
||||
```
|
||||
- 标定结果现归档在 `legacy/stereo_calib_params.npz`（对应已归档的双目链路）。
|
||||
当前运行链路**不需要**这些参数（吊舱不提供距离、双目未接入）。
|
||||
- 仓库根原有的 `camera.py` / `camera_test.py` / `video_test.py` 是同一批脚本，
|
||||
2026-10-20 移入本目录，内容未改。
|
||||
+182
@@ -0,0 +1,182 @@
|
||||
import cv2
|
||||
import numpy as np
|
||||
import glob
|
||||
import os
|
||||
from pathlib import Path
|
||||
|
||||
# ==================== 配置参数 ====================
|
||||
CHESSBOARD_SIZE = (8, 11) # 内角点数量 (列, 行)
|
||||
SQUARE_SIZE = 20.0 # 每个方格实际边长（毫米），请改成你的真实尺寸
|
||||
|
||||
CAMERA_INDEX = 1
|
||||
SAVE_DIR = "stereo_calib_images" # 保存标定图片的文件夹
|
||||
os.makedirs(SAVE_DIR, exist_ok=True)
|
||||
|
||||
# 标定用分辨率（每个单目）
|
||||
CALIB_WIDTH = 1280
|
||||
CALIB_HEIGHT = 720
|
||||
|
||||
|
||||
# =================================================
|
||||
|
||||
def capture_images():
|
||||
"""采集标定图片（左右目同步保存）"""
|
||||
cap = cv2.VideoCapture(CAMERA_INDEX, cv2.CAP_DSHOW)
|
||||
if not cap.isOpened():
|
||||
cap = cv2.VideoCapture(CAMERA_INDEX, cv2.CAP_MSMF)
|
||||
|
||||
cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'MJPG'))
|
||||
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 2560)
|
||||
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
|
||||
|
||||
print("按空格键保存当前帧，按 q 退出采集")
|
||||
count = 0
|
||||
|
||||
while True:
|
||||
ret, frame = cap.read()
|
||||
if not ret:
|
||||
break
|
||||
|
||||
left = frame[:, :1280].copy()
|
||||
right = frame[:, 1280:].copy()
|
||||
|
||||
gray_l = cv2.cvtColor(left, cv2.COLOR_BGR2GRAY)
|
||||
gray_r = cv2.cvtColor(right, cv2.COLOR_BGR2GRAY)
|
||||
|
||||
ret_l, corners_l = cv2.findChessboardCorners(gray_l, CHESSBOARD_SIZE, None)
|
||||
ret_r, corners_r = cv2.findChessboardCorners(gray_r, CHESSBOARD_SIZE, None)
|
||||
|
||||
# 用于显示的图像（带角点）
|
||||
display_left = left.copy()
|
||||
display_right = right.copy()
|
||||
|
||||
if ret_l:
|
||||
cv2.drawChessboardCorners(display_left, CHESSBOARD_SIZE, corners_l, ret_l)
|
||||
if ret_r:
|
||||
cv2.drawChessboardCorners(display_right, CHESSBOARD_SIZE, corners_r, ret_r)
|
||||
|
||||
display = np.hstack([display_left, display_right])
|
||||
cv2.putText(display, f"Saved: {count} | Space: Save Q: Quit", (20, 40),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
|
||||
cv2.imshow("Stereo Capture (Left | Right)", display)
|
||||
|
||||
key = cv2.waitKey(1) & 0xFF
|
||||
if key == ord(' '): # 空格保存
|
||||
if ret_l and ret_r:
|
||||
# 1. 保存干净的原图（用于正式标定）
|
||||
cv2.imwrite(f"{SAVE_DIR}/left_{count:02d}.png", left)
|
||||
cv2.imwrite(f"{SAVE_DIR}/right_{count:02d}.png", right)
|
||||
|
||||
# 2. 同时保存带检测结果的可视化图（方便检查）
|
||||
cv2.imwrite(f"{SAVE_DIR}/left_{count:02d}_detect.png", display_left)
|
||||
cv2.imwrite(f"{SAVE_DIR}/right_{count:02d}_detect.png", display_right)
|
||||
|
||||
print(f"已保存第 {count} 组（含检测结果图）")
|
||||
count += 1
|
||||
else:
|
||||
print("未同时检测到棋盘格，未保存")
|
||||
elif key == ord('q'):
|
||||
break
|
||||
|
||||
cap.release()
|
||||
cv2.destroyAllWindows()
|
||||
print(f"共采集 {count} 组图片")
|
||||
|
||||
|
||||
def calibrate_stereo():
|
||||
"""执行双目标定"""
|
||||
# 准备物体点（世界坐标）
|
||||
objp = np.zeros((CHESSBOARD_SIZE[0] * CHESSBOARD_SIZE[1], 3), np.float32)
|
||||
objp[:, :2] = np.mgrid[0:CHESSBOARD_SIZE[0], 0:CHESSBOARD_SIZE[1]].T.reshape(-1, 2)
|
||||
objp *= SQUARE_SIZE
|
||||
|
||||
objpoints = [] # 3D点
|
||||
imgpoints_l = [] # 左目2D点
|
||||
imgpoints_r = [] # 右目2D点
|
||||
|
||||
left_images = sorted(glob.glob(f"{SAVE_DIR}/left_*.png"))
|
||||
right_images = sorted(glob.glob(f"{SAVE_DIR}/right_*.png"))
|
||||
|
||||
if len(left_images) < 10:
|
||||
print("标定图片太少（建议 ≥15 组），请重新采集")
|
||||
return
|
||||
|
||||
print(f"开始处理 {len(left_images)} 组图片...")
|
||||
|
||||
for left_path, right_path in zip(left_images, right_images):
|
||||
img_l = cv2.imread(left_path)
|
||||
img_r = cv2.imread(right_path)
|
||||
|
||||
gray_l = cv2.cvtColor(img_l, cv2.COLOR_BGR2GRAY)
|
||||
gray_r = cv2.cvtColor(img_r, cv2.COLOR_BGR2GRAY)
|
||||
|
||||
ret_l, corners_l = cv2.findChessboardCorners(gray_l, CHESSBOARD_SIZE, None)
|
||||
ret_r, corners_r = cv2.findChessboardCorners(gray_r, CHESSBOARD_SIZE, None)
|
||||
|
||||
if ret_l and ret_r:
|
||||
# 亚像素精化
|
||||
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
|
||||
corners_l = cv2.cornerSubPix(gray_l, corners_l, (11, 11), (-1, -1), criteria)
|
||||
corners_r = cv2.cornerSubPix(gray_r, corners_r, (11, 11), (-1, -1), criteria)
|
||||
|
||||
objpoints.append(objp)
|
||||
imgpoints_l.append(corners_l)
|
||||
imgpoints_r.append(corners_r)
|
||||
print(f"✓ {Path(left_path).name}")
|
||||
else:
|
||||
print(f"✗ 跳过 {Path(left_path).name}")
|
||||
|
||||
if len(objpoints) < 10:
|
||||
print("有效图片不足，标定失败")
|
||||
return
|
||||
|
||||
# 单目标定（可选，用来给 stereoCalibrate 提供初始值）
|
||||
ret_l, mtx_l, dist_l, rvecs_l, tvecs_l = cv2.calibrateCamera(
|
||||
objpoints, imgpoints_l, (CALIB_WIDTH, CALIB_HEIGHT), None, None)
|
||||
ret_r, mtx_r, dist_r, rvecs_r, tvecs_r = cv2.calibrateCamera(
|
||||
objpoints, imgpoints_r, (CALIB_WIDTH, CALIB_HEIGHT), None, None)
|
||||
|
||||
print("\n左目重投影误差:", ret_l)
|
||||
print("右目重投影误差:", ret_r)
|
||||
|
||||
# 双目标定
|
||||
flags = cv2.CALIB_FIX_INTRINSIC # 使用上面得到的内参
|
||||
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 1e-5)
|
||||
|
||||
ret, mtx_l, dist_l, mtx_r, dist_r, R, T, E, F = cv2.stereoCalibrate(
|
||||
objpoints, imgpoints_l, imgpoints_r,
|
||||
mtx_l, dist_l, mtx_r, dist_r,
|
||||
(CALIB_WIDTH, CALIB_HEIGHT),
|
||||
criteria=criteria,
|
||||
flags=flags
|
||||
)
|
||||
|
||||
print("\n双目标定完成！")
|
||||
print("重投影误差:", ret)
|
||||
print("旋转矩阵 R:\n", R)
|
||||
print("平移向量 T:\n", T)
|
||||
|
||||
# 计算立体校正参数
|
||||
R1, R2, P1, P2, Q, roi_l, roi_r = cv2.stereoRectify(
|
||||
mtx_l, dist_l, mtx_r, dist_r,
|
||||
(CALIB_WIDTH, CALIB_HEIGHT), R, T,
|
||||
alpha=0 # 0=只保留有效区域，1=保留所有像素
|
||||
)
|
||||
|
||||
# 保存所有结果
|
||||
np.savez("stereo_calib_params.npz",
|
||||
mtx_l=mtx_l, dist_l=dist_l,
|
||||
mtx_r=mtx_r, dist_r=dist_r,
|
||||
R=R, T=T, E=E, F=F,
|
||||
R1=R1, R2=R2, P1=P1, P2=P2, Q=Q)
|
||||
|
||||
print("\n标定参数已保存到 stereo_calib_params.npz")
|
||||
return mtx_l, dist_l, mtx_r, dist_r, R, T, Q
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
print("1. 先采集标定图片")
|
||||
capture_images()
|
||||
|
||||
print("\n2. 开始标定")
|
||||
calibrate_stereo()
|
||||
@@ -0,0 +1,59 @@
|
||||
import cv2
|
||||
|
||||
# ==================== 配置区 ====================
|
||||
CAMERA_INDEX = 1 # 相机索引，如果不对请用前面的探测代码找
|
||||
FRAME_WIDTH = 2560
|
||||
FRAME_HEIGHT = 720
|
||||
TARGET_FPS = 30
|
||||
# =================================================
|
||||
|
||||
def decode_fourcc(val):
|
||||
"""把 get(CAP_PROP_FOURCC) 返回的浮点数转成可读的四字符代码"""
|
||||
val = int(val)
|
||||
return "".join([chr((val >> 8 * i) & 0xFF) for i in range(4)])
|
||||
|
||||
# 1. 打开相机，Windows 上指定 DirectShow 后端
|
||||
cap = cv2.VideoCapture(CAMERA_INDEX, cv2.CAP_DSHOW)
|
||||
|
||||
if not cap.isOpened():
|
||||
print(f"无法打开相机，索引 {CAMERA_INDEX}")
|
||||
exit()
|
||||
|
||||
# 2. 设置分辨率（在设置 MJPG 之前或之后都可以，但建议先设格式）
|
||||
cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'))
|
||||
cap.set(cv2.CAP_PROP_FRAME_WIDTH, FRAME_WIDTH)
|
||||
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, FRAME_HEIGHT)
|
||||
cap.set(cv2.CAP_PROP_FPS, TARGET_FPS)
|
||||
|
||||
# 3. 读取一次，确认参数是否真的生效
|
||||
ret, frame = cap.read()
|
||||
if not ret:
|
||||
print("相机已打开，但无法读取帧（可能分辨率不支持）")
|
||||
cap.release()
|
||||
exit()
|
||||
|
||||
# 4. 打印实际生效的参数和帧格式
|
||||
print("===== 相机实际参数 =====")
|
||||
print(f"分辨率 : {int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))} x {int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))}")
|
||||
print(f"帧率 : {cap.get(cv2.CAP_PROP_FPS):.1f}")
|
||||
print(f"FOURCC : {decode_fourcc(cap.get(cv2.CAP_PROP_FOURCC))}")
|
||||
print(f"帧 dtype : {frame.dtype}")
|
||||
print(f"帧 shape : {frame.shape} # 应为 (H, W, 3)")
|
||||
print("========================")
|
||||
|
||||
# 5. 实时显示循环
|
||||
while True:
|
||||
ret, frame = cap.read()
|
||||
if not ret:
|
||||
print("读取帧失败，退出")
|
||||
break
|
||||
|
||||
cv2.imshow("USB Camera", frame)
|
||||
|
||||
# 按 'q' 退出
|
||||
if cv2.waitKey(1) & 0xFF == ord('q'):
|
||||
break
|
||||
|
||||
# 6. 释放资源
|
||||
cap.release()
|
||||
cv2.destroyAllWindows()
|
||||
@@ -0,0 +1,42 @@
|
||||
import cv2
|
||||
import numpy as np
|
||||
|
||||
# 摄像头
|
||||
cap = cv2.VideoCapture(0)
|
||||
if not cap.isOpened():
|
||||
print("无法打开摄像头")
|
||||
exit()
|
||||
|
||||
# 目标输出分辨率（匹配你的小屏+转换器）
|
||||
OUT_WIDTH = 720
|
||||
OUT_HEIGHT = 480
|
||||
|
||||
# 创建窗口并设为可调整
|
||||
cv2.namedWindow("HDMI_Output", cv2.WINDOW_NORMAL)
|
||||
|
||||
# 关键：把窗口移到第二块屏幕（HDMI）的位置
|
||||
# 假设主屏幕是 1920 宽，第二块屏幕从 x=1920 开始
|
||||
# 请根据你实际的主屏幕宽度修改这个值！
|
||||
SECOND_SCREEN_X = 1920 # ← 改成你主屏幕的宽度
|
||||
SECOND_SCREEN_Y = 0
|
||||
|
||||
cv2.moveWindow("HDMI_Output", SECOND_SCREEN_X, SECOND_SCREEN_Y)
|
||||
cv2.setWindowProperty("HDMI_Output", cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN)
|
||||
|
||||
while True:
|
||||
ret, frame = cap.read()
|
||||
if not ret:
|
||||
break
|
||||
|
||||
# 缩放到目标分辨率
|
||||
frame = cv2.resize(frame, (OUT_WIDTH, OUT_HEIGHT))
|
||||
|
||||
# 显示
|
||||
cv2.imshow("HDMI_Output", frame)
|
||||
|
||||
# 按 q 退出
|
||||
if cv2.waitKey(1) & 0xFF == ord('q'):
|
||||
break
|
||||
|
||||
cap.release()
|
||||
cv2.destroyAllWindows()
|
||||
Reference in New Issue
Block a user