724_deal_error

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)
+# 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)

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))

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")
+model = CameraModel(None)
+model.save_config("updated_config.json")

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

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)
+# result = vs_measurement(r"C:\Users\Administrator\Desktop\BYD\0722\new13.19351136278192.txt", position=784, config_path="updated_config.json")
+# # print(result)

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
 }