176 lines
5.8 KiB
Python
176 lines
5.8 KiB
Python
# 相机参考工具
|
||
# @路怀帅 2025.12.19 10:32am
|
||
# PS : 新增 加载相机畸变KD 视频对比显示功能
|
||
|
||
import cv2
|
||
import threading
|
||
import sys
|
||
from datetime import datetime
|
||
import os
|
||
import argparse
|
||
import numpy as np
|
||
|
||
# 全局变量
|
||
frame = None
|
||
running = True
|
||
which_camera = 0
|
||
W, H = 1920, 1080 # 相机分辨率
|
||
|
||
# 加载鱼眼相机参数
|
||
|
||
# Front 参数
|
||
# K = np.array([[ 5.3382445956203196e+02, 0., 9.7253025945442369e+02],
|
||
# [0., 5.3393792084343488e+02, 5.6249605531924215e+02],
|
||
# [ 0., 0., 1. ]])
|
||
|
||
# D = np.array([ -1.5749135021037808e-02, 2.9390620422222835e-03,
|
||
# -4.3176357910129585e-03, 1.3296605027646462e-03 ])
|
||
|
||
# Right 参数
|
||
K = np.array([[5.3402108990030604e+02, 0., 9.2598444295282172e+02],
|
||
[0., 5.3455325152709827e+02, 5.7771767919091610e+02],
|
||
[0., 0., 1.]])
|
||
D = np.array([-1.8724887233075402e-02, 6.4408558584901701e-03,
|
||
-5.2069636709412993e-03, 8.4815411645490968e-04])
|
||
# 加载透视变换参数
|
||
|
||
# Right 参数
|
||
front_proj_matrix = np.array([
|
||
[-4.5788125154634862e-01, -2.4847281172116515e+00, 1.1901356339453334e+03],
|
||
[7.0190114936088524e-02, -2.2880693827869965e+00, 1.0012772462548877e+03],
|
||
[1.4549566908718078e-04, -3.4392525895003334e-03, 1.0]
|
||
])
|
||
|
||
# 这些参数在 surround_view 中用于 set_scale_and_shift
|
||
scale_xy = np.array([6.99999988e-01, 8.00000012e-01]) # x, y 缩放比例
|
||
shift_xy = np.array([-80., -200.]) # x, y 偏移量 (像素)
|
||
|
||
|
||
def video_thread():
|
||
global frame, running
|
||
cap = cv2.VideoCapture(which_camera, cv2.CAP_ANY)
|
||
cap.set(cv2.CAP_PROP_FOURCC,cv2.VideoWriter_fourcc(*"YUYV"))
|
||
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
|
||
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080)
|
||
|
||
if not cap.isOpened():
|
||
print("[ERROR] Cannot open camera", file=sys.stderr)
|
||
running = False
|
||
return
|
||
|
||
modified_camera_matrix = K.copy()
|
||
modified_camera_matrix[0, 0] *= scale_xy[0] # fx *= scale_x
|
||
modified_camera_matrix[1, 1] *= scale_xy[1] # fy *= scale_y
|
||
modified_camera_matrix[0, 2] += shift_xy[0] # cx += shift_x
|
||
modified_camera_matrix[1, 2] += shift_xy[1] # cy += shift_y
|
||
|
||
# 鱼眼相机去畸变 合并缩放系数
|
||
map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), modified_camera_matrix, (W, H), cv2.CV_16SC2)
|
||
|
||
# 鱼眼相机仅畸变
|
||
map3, map4 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, (W, H), cv2.CV_16SC2)
|
||
|
||
|
||
while running:
|
||
ret, f = cap.read()
|
||
if not ret:
|
||
break
|
||
|
||
# 图像去畸变
|
||
undistorted = cv2.remap(f, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
|
||
undistorted2 = cv2.remap(f, map3, map4, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
|
||
|
||
|
||
proj_image = cv2.warpPerspective(
|
||
undistorted,
|
||
front_proj_matrix,
|
||
(W, H), # 输出大小 (与原始分辨率一致)
|
||
borderMode=cv2.BORDER_CONSTANT,
|
||
borderValue=(0, 0, 0)
|
||
)
|
||
|
||
|
||
frame = f.copy()
|
||
birdseye_small = cv2.resize(f, (W//2, H//2))
|
||
undistorted2_small = cv2.resize(undistorted2, (W//2, H//2))
|
||
|
||
# 拼接原视频和抗畸变后的视频 （左右显示）
|
||
comparison = np.hstack((birdseye_small, undistorted2_small))
|
||
show_video = np.vstack((comparison, proj_image))
|
||
|
||
scale = min(1920 / comparison.shape[1], 1080 / comparison.shape[0])
|
||
if scale < 1:
|
||
comparison = cv2.resize(comparison, (int(comparison.shape[1] * scale), int(comparison.shape[0] * scale)))
|
||
comparison = comparison.astype(np.uint8)
|
||
|
||
# 设置视频流全屏显示
|
||
text_info = f"Camera: {args.i.upper()} | Press 'q' to quit, 's' to screenshot"
|
||
cv2.putText(comparison, text_info, (10, 30),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2, cv2.LINE_AA)
|
||
|
||
cv2.namedWindow('Video old vs new', cv2.WND_PROP_FULLSCREEN)
|
||
cv2.setWindowProperty('Video old vs new', cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN)
|
||
cv2.imshow('Video old vs new', show_video)
|
||
|
||
if cv2.waitKey(1) & 0xFF == ord('q'):
|
||
running = False
|
||
break
|
||
|
||
cap.release()
|
||
cv2.destroyAllWindows()
|
||
|
||
def input_thread():
|
||
global running
|
||
print("SSH命令: 's' = 截图, 'q' = 退出")
|
||
while running:
|
||
try:
|
||
cmd = input().strip().lower()
|
||
if cmd == 's':
|
||
if frame is not None:
|
||
filename = f"./images/{args.i.lower()}.png"
|
||
cv2.imwrite(filename, frame)
|
||
print(f"[SSH] Saved: {os.path.abspath(filename)}")
|
||
else:
|
||
print("[SSH] No frame available yet.")
|
||
elif cmd == 'q':
|
||
running = False
|
||
break
|
||
else:
|
||
print("[SSH] Unknown command. Use 's' or 'q'.")
|
||
except EOFError:
|
||
break
|
||
|
||
if __name__ == "__main__":
|
||
|
||
# 获取用户参数启动
|
||
parser = argparse.ArgumentParser(description="Example script with argparse")
|
||
|
||
# 获取用户输入的摄像头方位 （front back left right）
|
||
parser.add_argument("--i", type=str, required=True, help="摄像头方位")
|
||
|
||
args = parser.parse_args()
|
||
|
||
print("相机方位:", args.i)
|
||
|
||
if args.i == "front":
|
||
which_camera = 0
|
||
elif args.i == "back":
|
||
which_camera = 2
|
||
elif args.i == "left":
|
||
which_camera = 1
|
||
elif args.i == "right":
|
||
which_camera = 3
|
||
else:
|
||
print("[ERROR] Invalid camera direction. Use 'front', 'back', 'left', or 'right'.", file=sys.stderr)
|
||
running = False
|
||
exit(1)
|
||
|
||
# 启动视频线程
|
||
vt = threading.Thread(target=video_thread, daemon=True)
|
||
vt.start()
|
||
|
||
# 主线程监听 SSH 输入
|
||
input_thread()
|
||
|
||
print("[INFO] Exiting...")
|