import cv2 import numpy as np import glob from scipy.optimize import minimize import cameramodel import os import math import calc_way import measure_lib import time def needs_correction(dist_coeffs, error_threshold=0.5): k1, k2, p1, p2, k3 = dist_coeffs.ravel() if (abs(k1) > 0.1 or abs(k2) > 0.01 or abs(p1) > 0.005 or abs(p2) > 0.005 or abs(k3) > 0.01): return True return False def calibrate(image_fold, columns, rows, size): # 设置棋盘格参数 chessboard_size = (columns, rows) # 内部角点数量 (columns, rows) square_size = size # 棋盘格方块实际大小（单位：毫米/厘米/英寸等） # 准备对象点 (0,0,0), (1,0,0), (2,0,0) ..., (8,5,0) 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) * square_size # 存储对象点和图像点的数组 objpoints = [] # 3D点（真实世界坐标） imgpoints = [] # 2D点（图像坐标） image_size = None # 用于存储图像尺寸 # 获取标定图像 images = glob.glob(image_fold) # print("找到的图像文件:", images) for fname in images: img = cv2.imread(fname) if img is None: continue # 只在第一次成功读取图像时记录尺寸 if image_size is None: image_size = img.shape[:2][::-1] # 存储为 (width, height) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # 查找棋盘格角点 ret, corners = cv2.findChessboardCorners(gray, chessboard_size, None) # 如果找到，添加对象点和图像点 if ret: objpoints.append(objp) # 亚像素级精确化 criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) imgpoints.append(corners2) # 绘制并显示角点 cv2.drawChessboardCorners(img, chessboard_size, corners2, ret) cv2.imshow('Corners', img) cv2.waitKey(500) cv2.destroyAllWindows() # 相机标定 ret, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera( objpoints, imgpoints, gray.shape[::-1], None, None) # 输出标定结果 print("相机内参矩阵（K矩阵）:\n", camera_matrix) print("\n畸变系数（k1, k2, p1, p2, k3）:\n", dist_coeffs) print("\n重投影误差:", ret) if needs_correction(dist_coeffs): print("需要矫正：畸变系数过大") else: print("无需矫正：畸变可忽略") return camera_matrix, dist_coeffs, rvecs, tvecs,image_size def find_corners(image_path, columns, rows): # 读取图像 image = cv2.imread(image_path) gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) # 定义棋盘格尺寸 (内角点数量，非方格数) pattern_size = (columns, rows) # 例如8x8的棋盘有7x7内角点 # 查找棋盘格角点 ret, corners = cv2.findChessboardCorners(gray, pattern_size, None) if ret: # 提高角点检测精度 criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) # 绘制检测结果 cv2.drawChessboardCorners(image, pattern_size, corners, ret) cv2.imshow('Chessboard Corners', image) cv2.waitKey(0) cv2.destroyAllWindows() else: print("未检测到棋盘格") print(corners) # 将形状从 (N,1,2) 转换为 (N,2) corners_reshaped = corners.reshape(-1, 2) # print("所有角点坐标:\n", corners_reshaped) fixed_value = 960 # 固定值 column_index = 1 # 操作第2列（索引从0开始） # 方法：固定值 - 列值 corners_reshaped[:, column_index] = fixed_value - corners_reshaped[:, column_index] print(corners_reshaped) return corners_reshaped def undistort_image(image_path, camera_matrix, dist_coeffs): # 读取图像 img = cv2.imread(image_path) # 获取图像尺寸 h, w = img.shape[:2] # 优化相机矩阵 new_camera_matrix, roi = cv2.getOptimalNewCameraMatrix( camera_matrix, dist_coeffs, (w, h), 1, (w, h)) # 使用undistort dst = cv2.undistort(img, camera_matrix, dist_coeffs, None, new_camera_matrix) # 裁剪图像(使用roi) # x, y, w, h = roi # dst = dst[y:y + h, x:x + w] return dst, new_camera_matrix def calc_distance(cameraModel, alpha, beta, rotation_camera, H, x: float, y: float) -> tuple[float, float]: """计算像素坐标 (x, y) 对应的世界坐标 (Xw, Yw)。 参数: x, y: 像素坐标 cameraModel: 包含相机参数的类实例，需有以下属性: principal_point: 主点坐标 (cx, cy) pixel_size_x, pixel_size_y: 像素尺寸 rotation_camera: 相机旋转角度 focal_length: 焦距 rotation_alpha, rotation_beta: 旋转角度 height: 相机高度 返回: 世界坐标系下的 (Xw, Yw) """ # 1. 像素坐标转归一化平面坐标（考虑主点和旋转） Xp = (x - cameraModel.principal_point[0]) * cameraModel.pixel_size_x Yp = -(y - cameraModel.principal_point[1]) * cameraModel.pixel_size_y # 2. 应用相机旋转 cos_rot = math.cos(rotation_camera) sin_rot = math.sin(rotation_camera) Xp_rotation = Xp * cos_rot + Yp * sin_rot Yp_rotation = -Xp * sin_rot + Yp * cos_rot # 3. 预计算常用中间变量 focal_sq = cameraModel.focal_length ** 2 Yp_sq = Yp_rotation ** 2 sqrt_focal_Yp = math.sqrt(focal_sq + Yp_sq) # 4. 计算角度 eta 和 seta eta = math.atan2(Yp_rotation, cameraModel.focal_length) # 改用 atan2 避免除零 tan_alpha = math.tan(alpha) tan_beta = math.tan(beta) seta = math.atan2(1, math.sqrt(tan_beta ** 2 + 1 / tan_alpha ** 2)) # 5. 计算距离 d 和世界坐标 seta_eta = seta + eta OM = (H) / math.tan(seta_eta) MP = (H) * abs(Xp_rotation) / (sqrt_focal_Yp * math.sin(seta_eta)) # epsilon = math.atan2(MP, OM) # 改用 atan2 提高数值稳定性 epsilon = math.atan(((H) * Xp_rotation / (sqrt_focal_Yp * math.sin(seta_eta))) / OM) d = math.hypot(MP, OM) # 等价于 sqrt(MP**2 + OM**2)，但更高效 gamma = math.atan2(1, tan_alpha * tan_beta) # 6. 计算最终世界坐标 Xw = d * math.cos(gamma + epsilon) Yw = -d * math.sin(gamma + epsilon) return Xw, Yw def fun(x,cameraModel,corners): print("标定开始") # print(corners) error = 0 error_y = 0 error_x = 0 f = cameraModel.focal_length H = x[3] - 9 gamma = math.atan(1 / (math.tan(x[0]) * math.tan(x[1]))) seta = math.atan(1 / math.sqrt(pow(math.tan(x[1]), 2) + 1 / pow(math.tan(x[0]), 2))) column = 0 k = 0 k1 = 0 for index, value in enumerate(corners): # print(index,value) if index % 11 == 10: column += 1 index += 1 k += 1 k1 = 0 continue k1 += 1 Xw, Yw = calc_distance(cameraModel, x[0], x[1], x[2], H, corners[index][0], corners[index][1]) Xw1, Yw1 = calc_distance(cameraModel, x[0], x[1], x[2], H, corners[index+1][0], corners[index+1][1]) print(f"第{index}个点") print(f"Xw: {Xw}, Yw: {Yw}") print(f"Xw1: {Xw1}, Yw1: {Yw1}") print(f"Xw的理论值为：{40 + 60 * k1 + 540}，Yw的理论值为：{-90 - 60 * k + 53}") d2 = math.sqrt((Xw1 - Xw) ** 2 + (Yw1 - Yw) ** 2) print(f"两点距离为：{d2:.2f}") error = error + abs(d2 - 60) d_y = abs(Yw - (-90 - 60 * k + cameraModel.position_offset_y)) error_y = error_y + d_y d_x = abs(Xw - (40 + 60 * k1 + cameraModel.position_offset_x)) error_x = error_x + d_x print(f"d_x: {Xw - (40 + 60 * k1 + 507)}, d_y: {Yw - (-90 - 60 * k + 32)}") print(f"平均误差为：{error/80:.2f}") print(f"errpr_y:{error_y/80:.2f}") print(f"errpr_x:{error_x/80:.2f}") # print(gamma) return 1*error/80 + 1*error_y/80 + 1*error_x/80 def get_result(cameraModel, corners): params = cameraModel, corners bounds = [(0.1, 1.7), (0.1, 1.7), [-0.5, 0.5],[1000,1020]] # bounds = [(0.1, 1.7), (0.1, 1.7)] result = minimize( fun, x0=[cameraModel.rotation_alpha, cameraModel.rotation_beta, 0, cameraModel.height], args=params, # method='Nelder-Mead', # 或 'trust-constr' method='L-BFGS-B', bounds=bounds, tol=1e-6, # 高精度容差 options={'gtol': 1e-6, 'maxiter': 1000} ) return result def calibrate_and_undistort(image_path, output_fold, config, columns, rows, size): # 创建输出目录（如果不存在） global new_camera_matrix os.makedirs(output_fold, exist_ok=True) # 相机内参标定 cameraModel = cameramodel.CameraModel() camera_matrix, dist_coeffs, rvecs, tvecs, image_size = calibrate(image_path, columns, rows, size) camera_matrix[1][2] = image_size[1] - camera_matrix[1][2] # 畸变矫正 if needs_correction(dist_coeffs): images = glob.glob(image_path) for fname in images: undistor_img, new_camera_matrix = undistort_image(fname, camera_matrix, dist_coeffs) # 提取纯文件名（不含路径） base_name = os.path.basename(fname) # 构建输出路径（跨平台兼容） output_path = os.path.join(output_fold, base_name) # 保存图像（自动创建目录） try: cv2.imwrite(output_path, undistor_img) # print(f"Saved to: {output_path}") except Exception as e: print(f"Failed to save {output_path}: {str(e)}") print("相机无畸变内参矩阵：\n",new_camera_matrix ) new_camera_matrix[1][2] = image_size[1] - new_camera_matrix[1][2] cameraModel.update_parameter("origin_K", camera_matrix) cameraModel.update_parameter("dist_coeffs", dist_coeffs) cameraModel.update_parameter("camera_width", image_size[0]) cameraModel.update_parameter("camera_height", image_size[1]) cameraModel.update_parameter("undistor_K", new_camera_matrix) cameraModel.save_config(config) def calibrate_Extrinsic_Parameters(image_path, config, columns, rows): cameraModel = cameramodel.CameraModel(config) result = get_result(cameraModel, find_corners(image_path, columns, rows)) print(result) if result.success: print(f"Success to Extrinsic_Parameters {image_path}") cameraModel.update_parameter("rotation_alpha", result.x[0]) cameraModel.update_parameter("rotation_beta", result.x[1]) cameraModel.update_parameter("rotation_camera", result.x[2]) cameraModel.update_parameter("height", result.x[3]) cameraModel.save_config(config) else: print(f"Failed to Extrinsic_Parameters {image_path}") def check_Extrinsic_Parameters(image_path, output_path, config): cameraModel = cameramodel.CameraModel(config) x_zeros, y_zeros = calc_way.calc_zeros_yto0(cameraModel, 32) # 读取图像 img = cv2.imread(image_path) # 定义两点坐标 pt1 = (int(x_zeros[0]), int(960 - y_zeros[0])) pt2 = (int(x_zeros[-1]), int(960 - y_zeros[-1])) # 画红色线条，粗细为3 cv2.line(img, pt1, pt2, (0, 255, 255), 3) x_zeros, y_zeros = calc_way.calc_zeros_xto0(cameraModel, 507) # slope_Xw, intercept_Xw, r2_Xw = calc_slope_line.linear_regression(x_zeros,y_zeros) # print(f"slope_Xw: {slope_Xw}") # # print(f"intercept_Xw: {intercept_Xw}") pt1 = (int(x_zeros[0]), int(960 - y_zeros[0])) pt2 = (int(x_zeros[-1]), int(960 - y_zeros[-1])) # 画红色线条，粗细为3 cv2.line(img, pt1, pt2, (0, 0, 255), 3) # 保存结果 x_zeros, y_zeros = calc_way.calc_zeros_yto0(cameraModel, -600 + 32) # 定义两点坐标 pt1 = (int(x_zeros[0]), int(960 - y_zeros[0])) pt2 = (int(x_zeros[-1]), int(960 - y_zeros[-1])) # 画红色线条，粗细为3 cv2.line(img, pt1, pt2, (0, 255, 255), 3) x_zeros, y_zeros = calc_way.calc_zeros_xto0(cameraModel, 507 + 800) pt1 = (int(x_zeros[0]), int(960 - y_zeros[0])) pt2 = (int(x_zeros[-1]), int(960 - y_zeros[-1])) # 画红色线条，粗细为3 cv2.line(img, pt1, pt2, (0, 0, 255), 3) # # # x_zero, y_zero = calc_way.calc_zeros_xandyto0(cameraModel, 784, 53) # # print(x_zero, y_zero) cv2.circle(img, (int(188.70974194838968), 960-int(179.17183436687336)), radius=10, color=(0, 0, 255), thickness=-1) cv2.imwrite(output_path, img) # image_path = r"C:\Users\Administrator\Desktop\BYD\Visual measurement_model\Visual measurement\img\calibration\origin_img\*.jpg" # output_fold =r"C:\Users\Administrator\Desktop\BYD\Visual measurement_model\Visual measurement\img\calibration\undistor_img" # config = r"updated_config.json" # calibrate_and_undistort(image_path,output_fold,config,11,8,60) # calibrate_Extrinsic_Parameters("corrected.jpg", config, 11, 8) # check_Extrinsic_Parameters("corrected.jpg", "output.jpg", config) # t = time.time() # cameraModel = cameramodel.CameraModel(config) # x_zero, y_zero = calc_way.calc_zeros_xandyto0(cameraModel, 784, 32) # print(x_zero, y_zero) # print(time.time() - t) # result = measure_lib.vs_measurement(r"C:\Users\Administrator\Desktop\BYD\0718\new415.379489173224.txt", position=784, config_path=config ) # print(result) # img = cv2.imread(r"C:\Users\Administrator\Desktop\BYD\0718\new415.379489173224.jpg") # cv2.line(img, (int(229.67793558711742), 960-int(151.71034206841367)), (result[3],result[4]), (0, 255, 255), 3) # cv2.imwrite(r"test.jpg", img)