|
|
import cv2
|
|
|
import numpy as np
|
|
|
import glob
|
|
|
from scipy.optimize import minimize
|
|
|
import model
|
|
|
import math
|
|
|
import calc_way
|
|
|
import JS
|
|
|
import calc_slope_line
|
|
|
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
|
|
|
error_x = 0
|
|
|
model = cls()
|
|
|
f = model.f
|
|
|
# H = x[2] - 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_way.calc_distance(corners[index][0], corners[index][1], x[0], x[1], x[2])
|
|
|
Xw1, Yw1 = calc_way.calc_distance(corners[index+1][0], corners[index+1][1], x[0], x[1], x[2])
|
|
|
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 + 53))
|
|
|
error_y = error_y + d_y
|
|
|
d_x = abs(Xw - (40 + 60 * k1 + 540))
|
|
|
error_x = error_x + d_x
|
|
|
print(f"d_x: {Xw - (40 + 60 * k1 + 540)}, d_y: {Yw - (-90 - 60 * k + 53)}")
|
|
|
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 fun_test1(x,cls,corners):
|
|
|
# print("标定开始")
|
|
|
# print(corners)
|
|
|
error = 0
|
|
|
error_y = 0
|
|
|
error_x = 0
|
|
|
model = cls()
|
|
|
f = model.f
|
|
|
# H = x[2] - 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
|
|
|
row = 0
|
|
|
m = 0
|
|
|
for index, value in enumerate(corners):
|
|
|
row, column = divmod(index, 11)
|
|
|
Xw, Yw = calc_way.calc_distance(corners[index][0], corners[index][1], x[0], x[1], x[2])
|
|
|
print(f"基点为第{row}行,第{column}列,Xw: {Xw}, Yw: {Yw}\n")
|
|
|
for i in range(87 - index):
|
|
|
Xw1, Yw1 = calc_way.calc_distance(corners[index + i + 1][0], corners[index + i + 1][1], x[0], x[1], x[2])
|
|
|
d2 = math.sqrt((Xw1 - Xw) ** 2 + (Yw1 - Yw) ** 2)
|
|
|
row1, column1 = divmod(index+i+1, 11)
|
|
|
# print(f"距离点为第{row1}行,第{column1}列,Xw1: {Xw1}, Yw1: {Yw1}")
|
|
|
d_real = math.sqrt(pow(60*(row1-row), 2) + pow(60*(column1-column), 2))
|
|
|
dif = abs(d_real - d2)/math.sqrt(pow(row1-row, 2) + pow(column1-column, 2))
|
|
|
print(f"理论距离为：{d_real},实际距离为:{d2},误差为：{dif}")
|
|
|
if column == column1:
|
|
|
dif = 7.5*dif
|
|
|
error = error + dif
|
|
|
m = m + 1
|
|
|
# print(f"m = {m}")
|
|
|
print(f"平均误差为：{error/3828:.2f}")
|
|
|
print(gamma)
|
|
|
return error/3828 +0*abs(0.5776040711992061-gamma)
|
|
|
def fun_test2(x,cls,corners):
|
|
|
# print("标定开始")
|
|
|
# print(corners)
|
|
|
error = 0
|
|
|
error_y = 0
|
|
|
error_x = 0
|
|
|
model = cls()
|
|
|
f = model.f
|
|
|
# H = x[2] - 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
|
|
|
row = 0
|
|
|
m = 0
|
|
|
for index, value in enumerate(corners):
|
|
|
row, column = divmod(index, 11)
|
|
|
Xw, Yw = calc_way.calc_distance(corners[index][0], corners[index][1], x[0], x[1],x[2])
|
|
|
print(f"基点为第{row}行,第{column}列,Xw: {Xw}, Yw: {Yw}\n")
|
|
|
for i in range(87 - index):
|
|
|
Xw1, Yw1 = calc_way.calc_distance(corners[index + i + 1][0], corners[index + i + 1][1], x[0], x[1],x[2])
|
|
|
row1, column1 = divmod(index+i+1, 11)
|
|
|
dx_error = abs(Xw - Xw1 - 60*(column - column1))/math.sqrt(pow(row1-row, 2) + pow(column1-column, 2))
|
|
|
dy_error = abs(Yw - Yw1 + 60*(row - row1))/math.sqrt(pow(row1-row, 2) + pow(column1-column, 2))
|
|
|
# print(f"距离点为第{row1}行,第{column1}列,Xw1: {Xw1}, Yw1: {Yw1}")
|
|
|
print(f"dx_error: {(Xw - Xw1- 60*(column - column1))/math.sqrt(pow(row1-row, 2) + pow(column1-column, 2))}, dy_error: {(Yw - Yw1 + 60*(row - row1))/math.sqrt(pow(row1-row, 2) + pow(column1-column, 2))}")
|
|
|
error_x = error_x + dx_error
|
|
|
error_y = error_y + dy_error
|
|
|
print(f"error_x: {error_x}, error_y: {error_y}")
|
|
|
return error_x+1.33*error_y
|
|
|
|
|
|
def get_result_test(cls,corners):
|
|
|
params = cls,corners
|
|
|
bounds = [(0.1, 1.7), (0.1, 1.7), [-0.5, 0.5]]
|
|
|
# bounds = [(0.1, 1.7), (0.1, 1.7)]
|
|
|
result = minimize(
|
|
|
fun_test,
|
|
|
x0=[1.0, 0.7,0],
|
|
|
args=params,
|
|
|
# method='Nelder-Mead', # 或 'trust-constr'
|
|
|
method='L-BFGS-B', bounds=bounds,
|
|
|
tol=1e-12, # 高精度容差
|
|
|
options={'gtol': 1e-12, '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, new_camera_matrix
|
|
|
|
|
|
|
|
|
# 示例使用
|
|
|
# 假设你已经通过相机标定获得了相机矩阵和畸变系数
|
|
|
|
|
|
|
|
|
|
|
|
# result = calibrate(r"C:\Users\Administrator\Desktop\BYD\20250711\*.jpg",11,8,60)
|
|
|
# camera_matrix = result[0]
|
|
|
# dist_coeffs = result[1]
|
|
|
# model = model.Model()
|
|
|
model = JS.CameraModel("updated_config.json")
|
|
|
camera_matrix = model.K
|
|
|
dist_coeffs = model.dist_coeffs
|
|
|
print(f"camera_matrix: {camera_matrix}, dist_coeffs: {dist_coeffs}")
|
|
|
corrected_img, new_camera_matrix = undistort_image(r"C:\Users\Administrator\Desktop\BYD\7.15\image.jpg", camera_matrix, dist_coeffs)
|
|
|
cv2.imwrite(r"corrected.jpg", corrected_img)
|
|
|
# result = get_result_test(model.Model,find_corners(r"corrected.jpg",11,8))
|
|
|
# print(result)
|
|
|
# corners = find_corners("corrected.jpg",11,8)
|
|
|
# model = model.Model()
|
|
|
# for corner in corners:
|
|
|
# img = cv2.imread(r"corrected.jpg")
|
|
|
# Xw, Yw = calc_way.calc_distance(corner[0], corner[1], model.alpha,model.beta )
|
|
|
# cv2.circle(img, (int(corner[0]), 960-int(corner[1])), 3, (0, 0, 255), -1)
|
|
|
# cv2.putText(img, f"Xw = {int(Xw)},Yw = {int(Yw)}", (int(corner[0]), 960-int(corner[1])),
|
|
|
# cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
|
|
|
# cv2.imshow("img", img)
|
|
|
# cv2.waitKey(0)
|
|
|
# cv2.destroyAllWindows()
|
|
|
|
|
|
# x_zeros,y_zeros = calc_way.calc_zeros_yto0(53)
|
|
|
#
|
|
|
# # 读取图像
|
|
|
# img = cv2.imread(r"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)
|
|
|
#
|
|
|
# x_zeros,y_zeros = calc_way.calc_zeros_xto0(540)
|
|
|
# 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(-600+53)
|
|
|
# # 定义两点坐标
|
|
|
# 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(540+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, 255, 255), 3)
|
|
|
# cv2.imwrite("output.jpg", img)
|
|
|
|
|
|
|
|
|
# result = calibrate(r"C:\Users\Administrator\Desktop\BYD\20250711\undistor\*.jpg",11,8,60)
|
|
|
# K = result[0]
|
|
|
# dist_coeffs = result[1]
|
|
|
# # 步骤2：读取原始图像
|
|
|
# img = cv2.imread(r"C:\Users\Administrator\Desktop\BYD\20250711\frame_6300_2.jpg")
|
|
|
# h, w = img.shape[:2]
|
|
|
#
|
|
|
# # 步骤3：生成新内参矩阵（保持原始尺寸，不裁剪黑边）
|
|
|
# new_K = cv2.getOptimalNewCameraMatrix(K, dist_coeffs, (w, h), alpha=1)[0] # alpha=1保留所有像素
|
|
|
# print(f"new_K: {new_K}")
|
|
|
# # 步骤4：严格畸变校正
|
|
|
# undistorted_img = cv2.undistort(img, K, dist_coeffs, None, new_K)
|
|
|
#
|
|
|
# # 步骤5：显示校正结果（黑边可见）
|
|
|
# cv2.imshow('Original', img)
|
|
|
# cv2.imshow('Undistorted (Geometric True)', undistorted_img)
|
|
|
# cv2.waitKey(0)
|
|
|
# cv2.destroyAllWindows()
|
|
|
#
|
|
|
# # 步骤6：保存结果（保留黑边）
|
|
|
# cv2.imwrite(r"C:\Users\Administrator\Desktop\BYD\20250711\undistor\frame_6300_2.jpg", undistorted_img)
|
|
|
#
|
|
|
# # 可选：统计黑边像素占比
|
|
|
# gray = cv2.cvtColor(undistorted_img, cv2.COLOR_BGR2GRAY)
|
|
|
# black_pixels = np.sum(gray == 0)
|
|
|
# print(f"黑边像素占比: {100 * black_pixels / (h * w):.2f}%") |
