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

import cv2
import numpy as np
import glob
from scipy.optimize import minimize
import model
import math
import calc_way

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点（图像坐标）

    # 获取标定图像
    images = glob.glob(image_fold)
    # print("找到的图像文件:", images)

    for fname in images:
        img = cv2.imread(fname)
        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

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 fun_test(x,cls,corners):
    print("标定开始")
    # print(corners)
    error = 0
    error_y = 0
    model = cls()
    f = model.f
    H = model.H - 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
    for index, value in enumerate(corners):
        if index % 11 == 10:
            column += 1
            index += 1
            k += 1
            continue
        Xw, Yw = calc_way.calc_distance(value[0], value[1], x[0], x[1])
        Xw1, Yw1 = calc_way.calc_distance(corners[index+1][0], corners[index+1][1], x[0], x[1])
        print(f"第{index}个点")
        print(f"Xw: {Xw}, Yw: {Yw}")
        print(f"Xw1: {Xw1}, Yw1: {Yw1}")
        d2 = math.sqrt((Xw1 - Xw) ** 2 + (Yw1 - Yw) ** 2)
        print(f"两点距离为：{d2:.2f}")
        error = error + abs(d2 - 60)
        d_y = abs(570-60*k-50+Yw)
        error_y = error_y + d_y
    print(f"平均误差为：{error/80:.2f}")
    print(f"errpr_y:{error/80:.2f}")
    return error/80 + error_y/80

def get_result_test(cls,corners):
        params = cls,corners
        bounds = [(0.1, 1.7), (0.1, 1.7)]
        result = minimize(
            fun_test,
            x0=[0.7, 0.9],
            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 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


# 示例使用
# 假设你已经通过相机标定获得了相机矩阵和畸变系数



# result = calibrate(r"C:\Users\Administrator\Desktop\BYD\20250711\*.jpg",11,8,60)
# camera_matrix = result[0]
# dist_coeffs = result[1]
# corrected_img = undistort_image(r"C:\Users\Administrator\Desktop\BYD\20250711\frame_2100_3.jpg", camera_matrix, dist_coeffs)
# cv2.imwrite("corrected.jpg", corrected_img)
# result = get_result_test(model.Model,find_corners("corrected.jpg",11,8))
# print(result)
# find_corners("corrected.jpg",11,8)
# x_zeros,y_zeros = calc_way.calc_zeros_yto0(-200)
# print(x_zeros,y_zeros)
# # 读取图像
# img = cv2.imread("corrected.jpg")
#
# # 定义两点坐标
# 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)
#
# # 保存结果
# cv2.imwrite("output.jpg", img)