相机内参测试工具

camera_undistorted_tool.py · 4.5 KiB · Python 原始檔案

import cv2 import threading import sys import os import argparse import numpy as np import time running = True capture_flag = False # 内参 (K) 与畸变系数 (D) K = np.array([[ 3.9875655282072148e+02, 0., 6.3788553583130647e+02], [0.,3.7214138046449295e+02, 3.4915903287538981e+02], [ 0., 0., 1. ]]) D = np.array([ 8.6662165805452579e-02, -2.8155159006680735e-02, -3.5111914522638211e-02, 1.4621518110074484e-02 ]) W, H = 1280, 720 def input_thread(): global running, capture_flag print("\n终端输入's'截图,'q'退出程序。") while running: try: cmd = input().strip().lower() if cmd == 's': capture_flag = True elif cmd == 'q': print("退出命令触发") running = False break except EOFError: break except Exception: pass def main(): global running, capture_flag parser = argparse.ArgumentParser(description="Multi-Camera Hardware Reference Tool") parser.add_argument("--i", type=str, required=True, choices=["front", "back", "left", "right"], help="摄像头方位 (front, back, left, right)") args = parser.parse_args() camera_mapping = {"front": 0, "left": 1, "back": 2, "right": 3} which_camera = camera_mapping[args.i] cam_name = args.i.upper() print(f"无法打开摄像头节点: {cam_name} (Node: /dev/video{which_camera})") save_dir = os.path.join(os.getcwd(), "images") if not os.path.exists(save_dir): os.makedirs(save_dir) cmd_listener = threading.Thread(target=input_thread, daemon=True) cmd_listener.start() cap = None try: cap = cv2.VideoCapture(which_camera, cv2.CAP_V4L2) cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"YUYV")) cap.set(cv2.CAP_PROP_FRAME_WIDTH, W) cap.set(cv2.CAP_PROP_FRAME_HEIGHT, H) cap.set(cv2.CAP_PROP_BUFFERSIZE, 3) if not cap.isOpened(): print(f"[ERROR] Cannot open camera /dev/video{which_camera}", file=sys.stderr) running = False return map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, (W, H), cv2.CV_16SC2) # 预计算鱼眼镜头的抗畸变映射矩阵 cv2.namedWindow('PowLu camera tools', cv2.WND_PROP_FULLSCREEN) cv2.setWindowProperty('Video old vs new', cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN) while running: ret, f = cap.read() if not ret or f is None: time.sleep(0.01) continue raw_frame = f.copy() if capture_flag: filename = os.path.join(save_dir, f"{args.i.lower()}.png") cv2.imwrite(filename, raw_frame) print(f"[SUCCESS] Image saved accurately: {filename}") capture_flag = False undistorted = cv2.remap(f, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) comparison = np.hstack((f, undistorted)) 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 = np.ascontiguousarray(comparison, dtype=np.uint8) text_info = f"Camera: {cam_name} | Press 'q'='Quit', 's'='Screenshot'" cv2.putText(comparison, text_info, (20, 50), cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 255, 255), 3, cv2.LINE_AA) w_half = comparison.shape[1] // 2 h_bottom = comparison.shape[0] - 30 cv2.putText(comparison, "FISHEYE", (20, h_bottom), cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 255, 0), 3) cv2.putText(comparison, "UNDISTORTED", (w_half + 20, h_bottom), cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 255, 0), 3) cv2.imshow('PowLu camera tools', comparison) key = cv2.waitKey(1) & 0xFF if key == ord('q'): running = False break elif key == ord('s'): capture_flag = True finally: running = False if cap is not None and cap.isOpened(): cap.release() cv2.destroyAllWindows() if __name__ == "__main__": try: main() except KeyboardInterrupt: running = False

1	import cv2
2	import threading
3	import sys
4	import os
5	import argparse
6	import numpy as np
7	import time
8
9
10	running = True
11	capture_flag = False
12
13	# 内参 (K) 与畸变系数 (D)
14	K = np.array([[ 3.9875655282072148e+02, 0., 6.3788553583130647e+02],
15	[0.,3.7214138046449295e+02, 3.4915903287538981e+02],
16	[ 0., 0., 1. ]])
17	D = np.array([ 8.6662165805452579e-02, -2.8155159006680735e-02,
18	-3.5111914522638211e-02, 1.4621518110074484e-02 ])
19
20	W, H = 1280, 720
21
22
23	def input_thread():
24
25	global running, capture_flag
26	print("\n终端输入's'截图,'q'退出程序。")
27	while running:
28	try:
29	cmd = input().strip().lower()
30	if cmd == 's':
31	capture_flag = True
32	elif cmd == 'q':
33	print("退出命令触发")
34	running = False
35	break
36	except EOFError:
37	break
38	except Exception:
39	pass
40
41
42	def main():
43	global running, capture_flag
44
45
46	parser = argparse.ArgumentParser(description="Multi-Camera Hardware Reference Tool")
47	parser.add_argument("--i", type=str, required=True,
48	choices=["front", "back", "left", "right"],
49	help="摄像头方位 (front, back, left, right)")
50	args = parser.parse_args()
51
52	camera_mapping = {"front": 0, "left": 1, "back": 2, "right": 3}
53	which_camera = camera_mapping[args.i]
54	cam_name = args.i.upper()
55	print(f"无法打开摄像头节点: {cam_name} (Node: /dev/video{which_camera})")
56
57
58	save_dir = os.path.join(os.getcwd(), "images")
59	if not os.path.exists(save_dir):
60	os.makedirs(save_dir)
61
62
63	cmd_listener = threading.Thread(target=input_thread, daemon=True)
64	cmd_listener.start()
65
66
67	cap = None
68	try:
69	cap = cv2.VideoCapture(which_camera, cv2.CAP_V4L2)
70	cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"YUYV"))
71	cap.set(cv2.CAP_PROP_FRAME_WIDTH, W)
72	cap.set(cv2.CAP_PROP_FRAME_HEIGHT, H)
73	cap.set(cv2.CAP_PROP_BUFFERSIZE, 3)
74
75	if not cap.isOpened():
76	print(f"[ERROR] Cannot open camera /dev/video{which_camera}", file=sys.stderr)
77	running = False
78	return
79
80
81	map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, (W, H), cv2.CV_16SC2) # 预计算鱼眼镜头的抗畸变映射矩阵
82
83	cv2.namedWindow('PowLu camera tools', cv2.WND_PROP_FULLSCREEN)
84	cv2.setWindowProperty('Video old vs new', cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN)
85
86	while running:
87	ret, f = cap.read()
88	if not ret or f is None:
89	time.sleep(0.01)
90	continue
91
92	raw_frame = f.copy()
93
94	if capture_flag:
95	filename = os.path.join(save_dir, f"{args.i.lower()}.png")
96	cv2.imwrite(filename, raw_frame)
97	print(f"[SUCCESS] Image saved accurately: {filename}")
98	capture_flag = False
99
100	undistorted = cv2.remap(f, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
101	comparison = np.hstack((f, undistorted))
102
103	scale = min(1920 / comparison.shape[1], 1080 / comparison.shape[0])
104	if scale < 1:
105	comparison = cv2.resize(comparison, (int(comparison.shape[1] * scale), int(comparison.shape[0] * scale)))
106
107	comparison = np.ascontiguousarray(comparison, dtype=np.uint8)
108
109	text_info = f"Camera: {cam_name} \| Press 'q'='Quit', 's'='Screenshot'"
110	cv2.putText(comparison, text_info, (20, 50),
111	cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 255, 255), 3, cv2.LINE_AA)
112
113	w_half = comparison.shape[1] // 2
114	h_bottom = comparison.shape[0] - 30
115	cv2.putText(comparison, "FISHEYE", (20, h_bottom),
116	cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 255, 0), 3)
117	cv2.putText(comparison, "UNDISTORTED", (w_half + 20, h_bottom),
118	cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 255, 0), 3)
119
120	cv2.imshow('PowLu camera tools', comparison)
121
122	key = cv2.waitKey(1) & 0xFF
123	if key == ord('q'):
124	running = False
125	break
126	elif key == ord('s'):
127	capture_flag = True
128
129	finally:
130	running = False
131	if cap is not None and cap.isOpened():
132	cap.release()
133	cv2.destroyAllWindows()
134
135	if __name__ == "__main__":
136	try:
137	main()
138	except KeyboardInterrupt:
139	running = False
140