From c0fef392011d31eb13d51cf53557e77a9f9040f3 Mon Sep 17 00:00:00 2001 From: Paul597 <2454344015@qq.com> Date: Thu, 24 Jul 2025 21:36:36 +0800 Subject: [PATCH] 724_deal_error --- .../py/Calibration.py | 50 +++++++++---- Visual measurement-cameramodel/py/calc_way.py | 47 +++++++++++- .../py/cameramodel.py | 33 ++++++--- Visual measurement-cameramodel/py/get_data.py | 74 +++++++++++++++---- .../py/measure_lib.py | 8 +- .../py/updated_config.json | 44 +++++------ 6 files changed, 187 insertions(+), 69 deletions(-) diff --git a/Visual measurement-cameramodel/py/Calibration.py b/Visual measurement-cameramodel/py/Calibration.py index 7efe33b..21bd89e 100644 --- a/Visual measurement-cameramodel/py/Calibration.py +++ b/Visual measurement-cameramodel/py/Calibration.py @@ -6,6 +6,8 @@ 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 @@ -216,24 +218,24 @@ def fun(x,cameraModel,corners): 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)) + d_y = abs(Yw - (-90 - 60 * k + cameraModel.position_offset_y)) error_y = error_y + d_y - d_x = abs(Xw - (40 + 60 * k1 + 540)) + 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 + 540)}, d_y: {Yw - (-90 - 60 * k + 53)}") + 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) + # print(gamma) return 1*error/80 + 1*error_y/80 + 1*error_x/80 -def get_result(cls, corners): - params = cls, corners +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=[1.0, 0.7, 0,1007], + x0=[cameraModel.rotation_alpha, cameraModel.rotation_beta, 0, cameraModel.height], args=params, # method='Nelder-Mead', # 或 'trust-constr' method='L-BFGS-B', bounds=bounds, @@ -284,6 +286,7 @@ def calibrate_Extrinsic_Parameters(image_path, config, columns, rows): 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]) @@ -295,7 +298,7 @@ def calibrate_Extrinsic_Parameters(image_path, config, columns, rows): def check_Extrinsic_Parameters(image_path, output_path, config): cameraModel = cameramodel.CameraModel(config) - x_zeros, y_zeros = calc_way.calc_zeros_yto0(cameraModel, 53) + x_zeros, y_zeros = calc_way.calc_zeros_yto0(cameraModel, 32) # 读取图像 img = cv2.imread(image_path) @@ -305,9 +308,9 @@ def check_Extrinsic_Parameters(image_path, output_path, config): pt2 = (int(x_zeros[-1]), int(960 - y_zeros[-1])) # 画红色线条，粗细为3 - cv2.line(img, pt1, pt2, (0, 0, 255), 3) + cv2.line(img, pt1, pt2, (0, 255, 255), 3) - x_zeros, y_zeros = calc_way.calc_zeros_xto0(cameraModel, 540) + 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}") # @@ -319,7 +322,7 @@ def check_Extrinsic_Parameters(image_path, output_path, config): cv2.line(img, pt1, pt2, (0, 0, 255), 3) # 保存结果 - x_zeros, y_zeros = calc_way.calc_zeros_yto0(cameraModel, -600 + 53) + 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])) @@ -327,17 +330,34 @@ def check_Extrinsic_Parameters(image_path, output_path, config): # 画红色线条，粗细为3 cv2.line(img, pt1, pt2, (0, 255, 255), 3) - x_zeros, y_zeros = calc_way.calc_zeros_xto0(cameraModel, 540 + 800) + 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, 255, 255), 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_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) \ No newline at end of file +# 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) \ No newline at end of file diff --git a/Visual measurement-cameramodel/py/calc_way.py b/Visual measurement-cameramodel/py/calc_way.py index 20a6919..9603bc9 100644 --- a/Visual measurement-cameramodel/py/calc_way.py +++ b/Visual measurement-cameramodel/py/calc_way.py @@ -176,8 +176,8 @@ def calc_height( """ def calc_zeros_yto0(cameraModel, val): # 定义搜索范围和步长 - x_range = np.linspace(0, 1280, 1000) - y_range = np.linspace(0, 960, 1000) + x_range = np.linspace(0, 1280, 2560) + y_range = np.linspace(0, 960, 1920) # 存储解 solutions = [] @@ -216,8 +216,8 @@ def equations_yto0(cameraModel, x, y, val): def calc_zeros_xto0(cameraModel, val): # 定义搜索范围和步长 - x_range = np.linspace(0, 1280, 1000) - y_range = np.linspace(0, 960, 1000) + x_range = np.linspace(0, 1280, 2560) + y_range = np.linspace(0, 960, 1920) # 存储解 solutions = [] @@ -254,6 +254,45 @@ def equations_xto0(cameraModel, x, y, val): Xw, Yw = calc_distance(cameraModel, x, y) return (Xw - val)**2 # 误差平方和 +def calc_zeros_xandyto0(cameraModel, xval, yval): + # 定义搜索范围和步长 + x_range = np.linspace(0, 1280, 5000) + y_range = np.linspace(0, 960, 5000) + + # 存储解 + solutions = [] + tolerance = 1e-1 # 容忍误差 + + # 网格搜索 + for x in x_range: + for y in y_range: + error = equations_xandyto0(cameraModel, x, y, xval, yval) + if error < tolerance: + solutions.append((x, y)) + + # 去除近似重复的解 + unique_solutions = [] + for sol in solutions: + # 检查是否与已找到的解近似 + is_unique = True + for usol in unique_solutions: + if np.allclose(sol, usol, atol=1): + is_unique = False + break + if is_unique: + unique_solutions.append(sol) + x = [] + y = [] + # print("找到的解:") + for sol in unique_solutions: + # print(f"x = {sol[0]:.4f}, y = {sol[1]:.4f}") + x.append(sol[0]) + y.append(sol[1]) + return x, y + +def equations_xandyto0(cameraModel, x, y, xval, yval): + Xw, Yw = calc_distance(cameraModel, x, y) + return (Xw - xval)**2 + (Yw - yval)**2 # 误差平方和 # model = cameramodel.CameraModel("updated_config.json") # print(calc_distance(714,370,model)) # print(calc_distance2(638,-665,model)) \ No newline at end of file diff --git a/Visual measurement-cameramodel/py/cameramodel.py b/Visual measurement-cameramodel/py/cameramodel.py index 233dbd9..cf58b67 100644 --- a/Visual measurement-cameramodel/py/cameramodel.py +++ b/Visual measurement-cameramodel/py/cameramodel.py @@ -20,14 +20,20 @@ class CameraModel: [0, 0, 1]]), "dist_coeffs": np.array([-0.450688803, 0.260760637, 0.000210619624, -0.000669674309, -0.0447846385]), "focal_length": 3.6, - "height": 1007, # 1.007e+03 - 9 - "rotation_alpha": 0.9135, - "rotation_beta": 0.7271, - "position_offset_x": 0, - "position_offset_y": 53, - "rotation_camera": -0.02892, + "height": 1007.0633042608584, # 1.007e+03 - 9 + "rotation_alpha": 0.8892272342384955, + "rotation_beta": 0.6786627929104145, + "position_offset_x": 507, + "position_offset_y": 32, + "rotation_camera": 0.03985022053087201, + + "filter": True, "filt_percent": 0.1, - "ransac_residual_threshold": 2.5 + "outlier_num": 10, + "ransac_residual_threshold": 2.5, + "grid_downsample": True, + "cell_size": 20, + } # 如果提供了配置文件，则从文件加载参数 @@ -50,9 +56,12 @@ class CameraModel: self.position_offset_x = params["position_offset_x"] self.position_offset_y = params["position_offset_y"] self.rotation_camera = params["rotation_camera"] + self.filter = params["filter"] self.filt_percent = params["filt_percent"] + self.outlier_num = params["outlier_num"] self.ransac_residual_threshold = params["ransac_residual_threshold"] - + self.grid_downsample = params["grid_downsample"] + self.cell_size = params["cell_size"] # 计算派生属性 self.pixel_size_x = self.focal_length / self.undistor_K[0, 0] self.pixel_size_y = self.focal_length / self.undistor_K[1, 1] @@ -79,7 +88,11 @@ class CameraModel: "position_offset_x": self.position_offset_x, "position_offset_y": self.position_offset_y, "rotation_camera": self.rotation_camera, + "filter": self.filter, "filt_percent": self.filt_percent, + "outlier_num": self.outlier_num, + "grid_downsample": self.grid_downsample, + "cell_size": self.cell_size, "ransac_residual_threshold": self.ransac_residual_threshold } with open(file_path, 'w') as f: @@ -97,5 +110,5 @@ class CameraModel: else: raise AttributeError(f"参数 {key} 不存在") -# model = CameraModel(None) -# model.save_config("updated_config.json") \ No newline at end of file +model = CameraModel(None) +model.save_config("updated_config.json") \ No newline at end of file diff --git a/Visual measurement-cameramodel/py/get_data.py b/Visual measurement-cameramodel/py/get_data.py index dc3844e..3a9ab14 100644 --- a/Visual measurement-cameramodel/py/get_data.py +++ b/Visual measurement-cameramodel/py/get_data.py @@ -1,8 +1,16 @@ import numpy as np from sklearn.linear_model import RANSACRegressor, LinearRegression import matplotlib.pyplot as plt - -def filter_middle_percent(data, percent): +import pandas as pd +import calc_way +def grid_downsample(points, cell_size=15): + """网格化降采样，保持空间结构""" + df = pd.DataFrame(points, columns=['x', 'y']) + df['x_grid'] = (df['x'] // cell_size) * cell_size + df['y_grid'] = (df['y'] // cell_size) * cell_size + sampled = df.groupby(['x_grid', 'y_grid']).first().reset_index() + return sampled[['x', 'y']].values +def filter_middle_percent(data_x, data_y, cameraModel): """ 保留数组中间80%的数据（删除首尾各10%）。 @@ -13,8 +21,13 @@ def filter_middle_percent(data, percent): np.ndarray: 中间80%的数据。 """ # 展平数组（确保处理的是所有数据点） - flattened_data = data.flatten() - + # Xw_bot = [] + # for i in range(len(data_x)): + # Xw, Yw = calc_way.calc_distance(cameraModel, data_x[i], data_y[i]) + # Xw_bot.append(Xw) + # Xw_bot = np.array(Xw_bot) + # flattened_data = Xw_bot.flatten() + flattened_data = data_x.flatten() # # 计算10%和90%分位数 # lower_bound = np.percentile(flattened_data, 15) # upper_bound = np.percentile(flattened_data, 75) @@ -25,10 +38,11 @@ def filter_middle_percent(data, percent): data_min = np.min(flattened_data) data_max = np.max(flattened_data) data_range = data_max - data_min + # print(f'data_range: {data_range}, data_min: {data_min}, data_max: {data_max}') # 计算中间80%的上下界 - lower_bound = data_min + percent * data_range - upper_bound = data_max - percent * data_range + lower_bound = data_min + cameraModel.filt_percent * data_range + upper_bound = data_max - cameraModel.filt_percent * data_range # 筛选数据 mask = (flattened_data >= lower_bound) & (flattened_data <= upper_bound) @@ -45,13 +59,20 @@ def load_data(cameraModel, txt_name): try: data = np.loadtxt(filepath) + if cameraModel.grid_downsample: + int_data = data.astype(int) + grid_sampled = grid_downsample(int_data, cell_size=cameraModel.cell_size) + data = grid_sampled if data.shape[1] != 2: print("错误：文件必须包含两列数据（X和Y）") continue x = data[:, 0].reshape(-1, 1) y = data[:, 1].reshape(-1, 1) y = cameraModel.camera_height - y - mask = filter_middle_percent(x, cameraModel.filt_percent) + if cameraModel.filter: + mask = filter_middle_percent(x, y, cameraModel) + else: + mask = filter_middle_percent(x, y, cameraModel) x_clean = x[mask] y_clean = y[mask] return x_clean.reshape(-1, 1), y_clean.reshape(-1, 1) @@ -77,6 +98,7 @@ def ransac_fit(x, y, residual_threshold=2.5): def get_data(cameraModel, txt_name): # 加载数据 x, y = load_data(cameraModel, txt_name) + # print(f"文件数据点个数为：{len(x)}\n") if x is None: return 0, None, None, None, None, None, None, None, None @@ -92,9 +114,13 @@ def get_data(cameraModel, txt_name): # 第二次RANSAC拟合（在外点上） model2, inlier_mask2, outlier_mask2 = None, None, None - if len(x_outliers1) > 10: # 确保有足够的外点进行第二次拟合 + # print(f"第一次拟合内点数量：{len(x_inliers1)}\n外点数量：{len(x_outliers1)}\n") + if len(x_outliers1) > cameraModel.outlier_num: # 确保有足够的外点进行第二次拟合 model2, inlier_mask2, outlier_mask2 = ransac_fit(x_outliers1, y_outliers1, residual_threshold=cameraModel.ransac_residual_threshold) + else: + print(f"外点数量不足\n第一次拟合内点数量：{len(x_inliers1)}\n外点数量：{len(x_outliers1)}\n") + return 0, None, None, None, None, None, None, None, None x_inliers2 = x_outliers1[inlier_mask2] y_inliers2 = y_outliers1[inlier_mask2] @@ -102,7 +128,7 @@ def get_data(cameraModel, txt_name): # 获取第二次拟合的外点 x_outliers2 = x_outliers1[outlier_mask2] y_outliers2 = y_outliers1[outlier_mask2] - + print(f"第二次拟合内点数量：{len(x_inliers2)}\n外点数量：{len(x_outliers2)}\n") m1 = model1.predict(np.array([600]).reshape(-1, 1)) m2 = model2.predict(np.array([600]).reshape(-1, 1)) # 判断上下沿 @@ -123,17 +149,35 @@ def get_data(cameraModel, txt_name): # plt.figure(figsize=(14, 7)) # # # 绘制原始内点 - # plt.scatter(x_bot, y_bot, + # plt.scatter(x_inliers1, y_inliers1, # color='limegreen', marker='o', s=30, alpha=0.7, - # label='bot') - # plt.scatter(x_top, y_top, - # color='red', marker='*', s=100, edgecolor='black', - # label='top') + # label='first_inliers') + # + # # 绘制第一次拟合的直线 + # line_x = np.array([x.min(), x.max()]).reshape(-1, 1) + # line_y_1 = model1.predict(line_x) + # plt.plot(line_x, line_y_1, color='darkgreen', linewidth=3, + # label='first RANSAC') + # + # plt.scatter(x_inliers2, y_inliers2, + # color='yellow', marker='o', s=100, edgecolor='black', + # label='second_inliers') + # + # # 绘制第二次拟合的直线 + # line_x = np.array([x.min(), x.max()]).reshape(-1, 1) + # line_y_1 = model2.predict(line_x) + # plt.plot(line_x, line_y_1, color='darkgreen', linewidth=3, + # label='second RANSAC') + # + # plt.scatter(x_outliers2, y_outliers2, + # color='gray', marker='*', s=100, edgecolor='black', + # label='second outliner') + # # plt.xlabel('X', fontsize=12) # plt.ylabel('Y', fontsize=12) # plt.title(f'{txt_name}', fontsize=14) # plt.legend(fontsize=10, loc='best') # plt.grid(True, alpha=0.3) - # # plt.tight_layout() + # plt.tight_layout() # plt.show() return 1, x_bot, y_bot, slope_bot, intercept_bot, x_top, y_top, slope_top, intercept_top diff --git a/Visual measurement-cameramodel/py/measure_lib.py b/Visual measurement-cameramodel/py/measure_lib.py index 3b89101..629b505 100644 --- a/Visual measurement-cameramodel/py/measure_lib.py +++ b/Visual measurement-cameramodel/py/measure_lib.py @@ -18,7 +18,7 @@ def vs_measurement(txt_name, position=784, config_path: str = None): # 加载数据 state, x_bot, y_bot, slope_bot, intercept_bot, x_top, y_top, slope_top, intercept_top = get_data.get_data(cameraModel, txt_name) if state == 0: - return 0, None, None + return 0, None, None, None, None # 计算底部路沿 Xw_bot = [] @@ -46,7 +46,7 @@ def vs_measurement(txt_name, position=784, config_path: str = None): distance_pos = (-Yw_pos + cameraModel.position_offset_y) * math.cos(angle) return 1, Zw_pos, distance_pos, int(x_pos), cameraModel.camera_height - int(y_pos) -# folder_path = r"C:\Users\Administrator\Desktop\BYD\7.16\0716" +# folder_path = r"C:\Users\Administrator\Desktop\BYD\0722" # txt_files = glob.glob(os.path.join(folder_path, '*.txt')) # for file_path in txt_files: # with open(file_path, 'r', encoding='utf-8') as file: @@ -54,5 +54,5 @@ def vs_measurement(txt_name, position=784, config_path: str = None): # result = vs_measurement(file_path, position=784, config_path="updated_config.json") # print(result) -# result = vs_measurement(r"C:\Users\Administrator\Desktop\BYD\0718\new233.1718914966782.txt", position=200) -# print(result) \ No newline at end of file +# result = vs_measurement(r"C:\Users\Administrator\Desktop\BYD\0722\new13.19351136278192.txt", position=784, config_path="updated_config.json") +# # print(result) \ No newline at end of file diff --git a/Visual measurement-cameramodel/py/updated_config.json b/Visual measurement-cameramodel/py/updated_config.json index 52af80b..d0f683d 100644 --- a/Visual measurement-cameramodel/py/updated_config.json +++ b/Visual measurement-cameramodel/py/updated_config.json @@ -3,14 +3,14 @@ "camera_height": 960, "origin_K": [ [ - 1086.3271127158384, + 1086.5841123501, 0.0, - 657.4107885675492 + 656.379279306796 ], [ 0.0, - 1086.4432934668534, - 476.33985513371346 + 1086.65442545872, + 470.898109737448 ], [ 0.0, @@ -20,14 +20,14 @@ ], "undistor_K": [ [ - 839.0823587825538, + 838.19297892, 0.0, - 658.546503195964 + 658.51580973 ], [ 0.0, - 836.9498850040919, - 483.50294900514444 + 836.93529706, + 475.57774991 ], [ 0.0, @@ -36,21 +36,23 @@ ] ], "dist_coeffs": [ - [ - -0.4506888029066432, - 0.26076063677005185, - 0.00021061962350248572, - -0.0006696743088625306, - -0.044784638487560764 - ] + -0.450688803, + 0.260760637, + 0.000210619624, + -0.000669674309, + -0.0447846385 ], "focal_length": 3.6, - "height": 1007, - "rotation_alpha": 0.9135, - "rotation_beta": 0.7271, - "position_offset_x": 0, - "position_offset_y": 53, - "rotation_camera": -0.02892, + "height": 1007.0633042608584, + "rotation_alpha": 0.8892272342384955, + "rotation_beta": 0.6786627929104145, + "position_offset_x": 507, + "position_offset_y": 32, + "rotation_camera": 0.03985022053087201, + "filter": true, "filt_percent": 0.1, + "outlier_num": 10, + "grid_downsample": true, + "cell_size": 20, "ransac_residual_threshold": 2.5 } \ No newline at end of file