实时订阅相机图像，检测棋盘格角点，按 s 键保存 20 张合格图片，你可以自己修改，后自动调用 OpenCV 计算相机内参，把结果写成 ROS/LiDAR 联合标定可直接使用的 camera_calibration.yaml，全程 1 行命令启动，拍够即完事。
#!/usr/bin/env python3
import os
import cv2
import rospy
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import threading
# === 配置 ===
PATTERN = (8, 11) # 棋盘格行列
SQUARE_SIZE = 0.05 # 棋盘格方格边长（米）
OUT_DIR = "calib_result" # 输出文件夹
TARGET_IMAGES = 20 # 需要的有效角点图片数
# === 全局变量 ===
bridge = CvBridge()
objpoints, imgpoints, saved = [], [], []
objp = np.zeros((PATTERN[1]*PATTERN[0], 3), np.float32)
objp[:, :2] = np.indices(PATTERN).T.reshape(-1, 2) * SQUARE_SIZE
frame_size = None
calibration_triggered = False
current_frame = None
current_gray = None
current_found = False
current_corners = None
image_received = False
def setup_directories():
"""创建必要的文件夹"""
if not os.path.exists(OUT_DIR):
os.makedirs(OUT_DIR)
print(f"创建输出目录: {OUT_DIR}")
# 创建子文件夹
subdirs = ["images", "corners"]
for subdir in subdirs:
path = os.path.join(OUT_DIR, subdir)
if not os.path.exists(path):
os.makedirs(path)
def calculate_single_reprojection_error(objp, imgp, K, D):
"""计算单张图像的重投影误差"""
try:
ret, rvec, tvec = cv2.solvePnP(objp, imgp, K, D)
if ret:
imgpoints2, _ = cv2.projectPoints(objp, rvec, tvec, K, D)
error = cv2.norm(imgp, imgpoints2, cv2.NORM_L2) / len(imgpoints2)
return error
except Exception as e:
print(f"误差计算失败: {e}")
return float('inf')
def save_image_with_info(frame, corners, image_id):
"""保存图像和相关信息"""
try:
# 保存原图 - 修正参数顺序
img_path = os.path.join(OUT_DIR, "images", f"image_{image_id:02d}.png")
cv2.imwrite(img_path, frame) # 正确的参数顺序：路径, 图像
# 保存带角点的图像 - 修正参数顺序和变量名
corner_img = frame.copy()
cv2.drawChessboardCorners(corner_img, PATTERN, corners, True)
corner_path = os.path.join(OUT_DIR, "corners", f"corner_{image_id:02d}.png")
cv2.imwrite(corner_path, corner_img) # 正确的参数顺序：路径, 图像
return img_path, corner_path
except Exception as e:
print(f"保存图像失败: {e}")
return None, None
def image_callback(msg):
global current_frame, current_gray, current_found, current_corners, frame_size, image_received
if len(saved) >= TARGET_IMAGES or calibration_triggered:
return
try:
# 转换图像
current_frame = bridge.imgmsg_to_cv2(msg, "bgr8")
current_gray = cv2.cvtColor(current_frame, cv2.COLOR_BGR2GRAY)
frame_size = (current_frame.shape[1], current_frame.shape[0])
image_received = True
# 检测角点
current_found, corners = cv2.findChessboardCorners(current_gray, PATTERN)
if current_found:
current_corners = cv2.cornerSubPix(
current_gray, corners, (11, 11), (-1, -1),
(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
)
else:
current_corners = None
except Exception as e:
print(f"图像处理错误: {e}")
def save_current_image():
"""保存当前图像并计算误差"""
global objpoints, imgpoints, saved
if not current_found or current_corners is None:
print("当前图像没有检测到角点！")
return
if current_frame is None:
print("当前没有图像数据！")
return
try:
# 保存数据
objpoints.append(objp.copy())
imgpoints.append(current_corners.copy())
# 保存图像文件
result = save_image_with_info(current_frame, current_corners, len(saved))
if result[0] is not None and result[1] is not None:
img_path, corner_path = result
saved.append(img_path)
print(f"\n=== 保存第 {len(saved)} 张图像 ===")
print(f"原图保存至: {img_path}")
print(f"角点图保存至: {corner_path}")
# 如果有足够的图像，计算当前的标定参数和误差
if len(objpoints) >= 3:
try:
# 临时标定计算误差
ret, K, D, rvecs, tvecs = cv2.calibrateCamera(
objpoints, imgpoints, frame_size, None, None
)
# 计算当前图像的重投影误差
current_error = calculate_single_reprojection_error(objp, current_corners, K, D)
print(f"当前图像重投影误差: {current_error:.4f} 像素")
print(f"临时标定RMS误差: {ret:.4f}")
except Exception as e:
print(f"临时标定计算失败: {e}")
print(f"进度: {len(saved)}/{TARGET_IMAGES}")
# 如果达到目标数量，自动开始标定
if len(saved) >= TARGET_IMAGES:
print("\n已达到目标图像数量，开始最终标定...")
start_calibration()
else:
# 如果保存失败，回滚数据
objpoints.pop()
imgpoints.pop()
print("图像保存失败，请重试")
except Exception as e:
print(f"保存图像过程中出错: {e}")
# 回滚数据
if len(objpoints) > len(saved):
objpoints.pop()
if len(imgpoints) > len(saved):
imgpoints.pop()
def start_calibration():
"""开始最终标定计算"""
global calibration_triggered
calibration_triggered = True
def save_calibration_results(K, D, rvecs, tvecs, rms_error):
"""保存标定结果为自定义 YAML 格式"""
try:
yaml_path = os.path.join(OUT_DIR, "camera_calibration.yaml")
with open(yaml_path, 'w') as f:
f.write("%YAML:1.0\n")
f.write("---\n\n")
# ROS话题
f.write(f'image_topic: "/usb_cam/image_raw"\n')
f.write(f'pointcloud_topic: "/velodyne_points"\n\n')
# 相机类型与激光雷达
f.write("distortion_model: 0\n")
f.write("scan_line: 16\n\n")
# 棋盘格参数
f.write("chessboard:\n")
f.write(f" length: {PATTERN[1]}\n") # 注意行列对调
f.write(f" width: {PATTERN[0]}\n")
f.write(f" grid_size: {int(SQUARE_SIZE*1000)}\n\n") # 转为毫米
# 标定板尺寸
board_length = PATTERN[1] * SQUARE_SIZE * 1000
board_width = PATTERN[0] * SQUARE_SIZE * 1000
f.write("board_dimension:\n")
f.write(f" length: {board_length:.0f}\n")
f.write(f" width: {board_width:.0f}\n\n")
# 棋盘中心偏移
f.write("translation_error:\n")
f.write(" length: 0\n")
f.write(" width: 0\n\n")
# 相机内参
f.write("camera_matrix: !!opencv-matrix\n")
f.write(" rows: 3\n")
f.write(" cols: 3\n")
f.write(" dt: d\n")
f.write(f" data: {K.flatten().tolist()}\n\n")
# 畸变系数
f.write("distortion_coefficients: !!opencv-matrix\n")
f.write(" rows: 5\n")
f.write(" cols: 1\n")
f.write(" dt: d\n")
f.write(f" data: {D.flatten().tolist()}\n\n")
# 图像分辨率
f.write("image_pixel:\n")
f.write(f" width: {frame_size[0]}\n")
f.write(f" height: {frame_size[1]}\n")
print(f"自定义YAML文件已保存: {yaml_path}")
except Exception as e:
print(f"保存YAML文件失败: {e}")
def display_thread():
"""图像显示线程"""
global current_frame, current_found, current_corners, calibration_triggered
while not rospy.is_shutdown() and not calibration_triggered:
try:
if current_frame is not None:
# 创建显示图像
display_frame = current_frame.copy()
# 绘制角点（如果找到）
if current_found and current_corners is not None:
cv2.drawChessboardCorners(display_frame, PATTERN, current_corners, current_found)
# 显示绿色状态
cv2.putText(display_frame, "READY TO SAVE", (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
else:
# 显示红色状态
cv2.putText(display_frame, "NO CORNERS DETECTED", (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2)
# 显示当前保存数量和进度
progress_text = f"SAVED: {len(saved)}/{TARGET_IMAGES}"
cv2.putText(display_frame, progress_text, (10, 70),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
# 显示操作提示
cv2.putText(display_frame, "s:Save a:Skip d:Calibrate q:Quit", (10, display_frame.shape[0]-20),
cv2.FONT_HERSHEY_SIMPLEX, 0.3, (255, 255, 255), 2)
cv2.imshow("Camera Calibration", display_frame)
# 处理按键
key = cv2.waitKey(30) & 0xFF
if key == ord('s'):
if current_found and current_corners is not None:
print("正在保存图像...")
save_current_image()
else:
print("无法保存：当前图像未检测到棋盘格角点")
elif key == ord('a'):
print(f"跳过当前图像 (已保存: {len(saved)}/{TARGET_IMAGES})")
elif key == ord('d'):
if len(saved) > 0:
print("手动开始标定计算...")
start_calibration()
break
else:
print("没有保存的图像，无法进行标定！")
elif key == ord('q'):
print("用户退出")
calibration_triggered = True
rospy.signal_shutdown("User quit")
break
else:
cv2.waitKey(30)
except Exception as e:
print(f"显示线程错误: {e}")
cv2.waitKey(30)
def check_topic():
"""检查ROS话题是否存在"""
try:
topics = rospy.get_published_topics()
topic_names = [topic[0] for topic in topics]
print("当前可用的图像话题:")
image_topics = [t for t in topic_names if 'image' in t.lower()]
if not image_topics:
print(" 未找到图像话题！")
return None
for topic in image_topics:
print(f" - {topic}")
# 常见的相机话题
common_topics = ["/usb_cam/image_raw", "/camera/image_raw", "/image_raw", "/camera/color/image_raw"]
for topic in common_topics:
if topic in topic_names:
return topic
# 如果没找到常见话题，返回第一个图像话题
return image_topics[0] if image_topics else None
except Exception as e:
print(f"检查话题时出错: {e}")
return "/usb_cam/image_raw" # 返回默认话题
def main():
global calibration_triggered, image_received
try:
rospy.init_node("camera_calibration_online")
# 设置目录结构
setup_directories()
# 检查并选择图像话题
print("正在检查ROS话题...")
topic = check_topic()
if topic is None:
print("错误: 未找到图像话题！")
print("请确保相机节点正在运行，例如:")
print(" roslaunch usb_cam usb_cam-test.launch")
return
print(f"使用图像话题: {topic}")
# 订阅图像话题
rospy.Subscriber(topic, Image, image_callback)
print("\n=== 相机在线标定程序 ===")
print("操作说明:")
print(" s - 保存当前图像(需检测到角点)")
print(" a - 跳过当前图像")
print(" d - 开始标定计算")
print(" q - 退出程序")
print(f"目标: 保存 {TARGET_IMAGES} 张有效图像")
print("-" * 50)
# 等待第一帧图像
print("等待图像数据...")
timeout_count = 0
while not image_received and not rospy.is_shutdown():
rospy.sleep(0.1)
timeout_count += 1
if timeout_count > 50: # 5秒超时
print("警告: 5秒内未接收到图像数据")
print("请检查:")
print("1. 相机节点是否运行")
print("2. 话题名称是否正确")
break
if not image_received:
print("未接收到图像数据，程序退出")
return
print("开始接收图像，请操作OpenCV窗口...")
# 启动显示线程
display_thread_handle = threading.Thread(target=display_thread)
display_thread_handle.daemon = True
display_thread_handle.start()
# 主循环
rate = rospy.Rate(10)
while not rospy.is_shutdown() and not calibration_triggered:
if len(saved) >= TARGET_IMAGES:
print("\n已达到目标图像数量，开始最终标定...")
start_calibration()
break
rate.sleep()
# 执行最终标定
if len(objpoints) >= 3:
print("\n开始最终标定计算...")
try:
ret, K, D, rvecs, tvecs = cv2.calibrateCamera(
objpoints, imgpoints, frame_size, None, None
)
print(f"\n=== 标定完成 ===")
print(f"使用图像数量: {len(objpoints)}")
print(f"RMS重投影误差: {ret:.6f} 像素")
# 保存结果
save_calibration_results(K, D, rvecs, tvecs, ret)
print("\n标定成功完成！所有文件已保存到:", OUT_DIR)
except Exception as e:
print(f"标定计算失败: {e}")
else:
print("图像数量不足，无法进行标定")
except Exception as e:
print(f"程序运行出错: {e}")
finally:
cv2.destroyAllWindows()
if __name__ == "__main__":
main()标定的结果以及配置文件
%YAML:1.0
image_topic: "/usb_cam/image_raw"
pointcloud_topic: "/velodyne_points"
distortion_model: 0
scan_line: 16
chessboard:
length: 11
width: 8
grid_size: 50
board_dimension:
length: 550
width: 400
translation_error:
length: 0
width: 0
camera_matrix: !!opencv-matrix
rows: 3
cols: 3
dt: d
data: [1043.0036406910003, 0.0, 342.0724555165832, 0.0, 1042.8060574438787, 196.33141728684964, 0.0, 0.0, 1.0]
distortion_coefficients: !!opencv-matrix
rows: 5
cols: 1
dt: d
data: [-0.35027408726243775, -4.30986322326977, 0.003188885116184713, -0.00015712716097013787, 137.0452894833554]
image_pixel:
width: 640
height: 480
