摄像头-激光雷达在线标定相机脚本（ROS 版）

实时订阅相机图像，检测棋盘格角点，按 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 = Falsedef 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 errorexcept 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_pathexcept Exception as e:print(f"保存图像失败: {e}")return None, Nonedef image_callback(msg):global current_frame, current_gray, current_found, current_corners, frame_size, image_receivedif len(saved) >= TARGET_IMAGES or calibration_triggered:returntry:# 转换图像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 = Noneexcept Exception as e:print(f"图像处理错误: {e}")def save_current_image():"""保存当前图像并计算误差"""global objpoints, imgpoints, savedif not current_found or current_corners is None:print("当前图像没有检测到角点！")returnif current_frame is None:print("当前没有图像数据！")returntry:# 保存数据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 = resultsaved.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_triggeredcalibration_triggered = Truedef 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 * 1000board_width  = PATTERN[0] * SQUARE_SIZE * 1000f.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_triggeredwhile 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) & 0xFFif 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()breakelse:print("没有保存的图像，无法进行标定！")elif key == ord('q'):print("用户退出")calibration_triggered = Truerospy.signal_shutdown("User quit")breakelse: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 Nonefor 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 Noneexcept Exception as e:print(f"检查话题时出错: {e}")return "/usb_cam/image_raw"  # 返回默认话题def main():global calibration_triggered, image_receivedtry: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")returnprint(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 = 0while not image_received and not rospy.is_shutdown():rospy.sleep(0.1)timeout_count += 1if timeout_count > 50:  # 5秒超时print("警告: 5秒内未接收到图像数据")print("请检查:")print("1. 相机节点是否运行")print("2. 话题名称是否正确")breakif not image_received:print("未接收到图像数据，程序退出")returnprint("开始接收图像，请操作OpenCV窗口...")# 启动显示线程display_thread_handle = threading.Thread(target=display_thread)display_thread_handle.daemon = Truedisplay_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()breakrate.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