byd/Visual measurement-cameramodel/py/Calibration.py

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] -0
    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-9,1020-9]]
    # bounds = [(0.1, 1.7), (0.1, 1.7)]
    result = minimize(
        fun,
        x0=[cameraModel.rotation_alpha, cameraModel.rotation_beta, 0, 998],
        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"
# find_corners(r"C:\Users\Administrator\Desktop\BYD\7.15\)
# 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)

# image_path = r"C:\Users\Administrator\Desktop\BYD\8.5\*.jpg"
# output_fold = r"C:\Users\Administrator\Desktop\BYD\8.5\undistor_img"
# config = r"updated_config.json"
# # find_corners(image_path,7,6)
# calibrate_and_undistort(image_path,output_fold,config,9,8,100)