6.25

py/calc_slope_line.py

@@ -50,7 +50,7 @@ def get_b(x,y,k):
 """
 def linear_regression(x, y):
     slope, intercept, r_value, p_value, std_err = stats.linregress(x, y)
-    return slope, intercept, r_value**2
+    return slope, intercept, abs(r_value)
 
 """
 计算图像中两条直线的交点

py/calc_way.py

@@ -83,7 +83,7 @@ def calc_height(x_bot,y_bot,x_top,y_top,alpha,beta):
 参数：
 返回值：轴线上点在图像中的二维坐标x、y
 """
-def calc_zeros_yto0():
+def calc_zeros_yto0(val):
     # 定义搜索范围和步长
     x_range = np.linspace(0, 1280, 1000)
     y_range = np.linspace(0, 960, 1000)
@@ -95,7 +95,7 @@ def calc_zeros_yto0():
     # 网格搜索
     for x in x_range:
         for y in y_range:
-            error = equations_yto0(x, y)
+            error = equations_yto0(x, y, val)
             if error < tolerance:
                 solutions.append((x, y))
 
@@ -120,7 +120,7 @@ def calc_zeros_yto0():
     return x, y
 
 # 定义方程
-def equations_yto0(x, y):
+def equations_yto0(x, y, val):
     Xp = (x - u0) * dx
     Yp = -(y - v0) * dy
     sq = math.sqrt(f * f + Yp * Yp)
@@ -133,14 +133,14 @@ def equations_yto0(x, y):
     d = math.sqrt(MP ** 2 + OM ** 2)
     Xw = d * math.cos(gamma + epsilon)
     Yw = -d * math.sin(gamma + epsilon)
-    return Yw**2   # 误差平方和
+    return (Yw - val)**2   # 误差平方和
 
 """
 计算图像中车辆坐标系Yw=某一固定值所对应的轴线
 参数：
 返回值：轴线上点在图像中的二维坐标x、y
 """
-def calc_zeros_xto0():
+def calc_zeros_xto0(val):
     # 定义搜索范围和步长
     x_range = np.linspace(0, 1280, 1000)
     y_range = np.linspace(0, 960, 1000)
@@ -152,7 +152,7 @@ def calc_zeros_xto0():
     # 网格搜索
     for x in x_range:
         for y in y_range:
-            error = equations_xto0(x, y)
+            error = equations_xto0(x, y, val)
             if error < tolerance:
                 solutions.append((x, y))
 
@@ -176,7 +176,7 @@ def calc_zeros_xto0():
         y.append(sol[1])
     return x, y
 
-def equations_xto0(x, y):
+def equations_xto0(x, y, val):
     Xp = (x - u0) * dx
     Yp = -(y - v0) * dy
     sq = math.sqrt(f * f + Yp * Yp)
@@ -189,7 +189,7 @@ def equations_xto0(x, y):
     d = math.sqrt(MP ** 2 + OM ** 2)
     Xw = d * math.cos(gamma + epsilon)
     Yw = -d * math.sin(gamma + epsilon)
-    return (Xw-877)**2   # 误差平方和
+    return (Xw-val)**2   # 误差平方和
 
 # x , y = calc_zeros_xto0()
 # for i in range(len(x)):

py/get_data.py

@@ -1,4 +1,9 @@
+import numpy as np
 
+import calc_way
+from scipy import stats
+import calc_slope_line
+import matplotlib.pyplot as plt
 # 车胎横向分界线
 k = 0.62849534
 b = 21.8122503
@@ -8,38 +13,156 @@ b = 21.8122503
 参数：保存数据的txt文件路径
 返回值：在目标区域内的下侧数据点坐标x_bot、y_bot，上侧数据点坐标x_top,y_top
 """
+# def get_data(txt_name):
+#     with open(txt_name, 'r', encoding='utf-8') as f:
+#         lines = f.readlines()
+#     data = []
+#     for i, line in enumerate(lines, 1):
+#         data.append(line.split())
+#         # print(line.split())
+#     x_bot = []
+#     y_bot = []
+#     x_top = []
+#     y_top = []
+#     k_num = 0
+#     k_num2 = 0
+#     for i in range(len(data)):
+#         data[i][1] = 960 - int(data[i][1])
+#     for i in range(len(data)):
+#         if k * int(data[i][0]) + b - int(data[i][1]) < 0:
+#             k_num2 = i
+#             continue
+#         if i > 1:
+#             if (int(data[i][0]) - int(data[i-1][0]))*(int(data[i+2][0]) - int(data[i][0])) < 0:
+#                 k_num = i
+#                 # print(f"k_num:{k_num},data:")
+#                 break
+#     for i in range(len(data)):
+#         if k * int(data[i][0]) + b - int(data[i][1]) < 0:
+#             continue
+#         if i < k_num:
+#             x_bot.append(int(data[i][0]))
+#             y_bot.append(int(data[i][1]))
+#         else:
+#             x_top.append(int(data[i][0]))
+#             y_top.append(int(data[i][1]))
+#     # print(x_bot, y_bot, x_top, y_top)
+#     return x_bot, y_bot, x_top, y_top, k_num2
+
+
 def get_data(txt_name):
     with open(txt_name, 'r', encoding='utf-8') as f:
         lines = f.readlines()
     data = []
     for i, line in enumerate(lines, 1):
         data.append(line.split())
-        # print(line.split())
+    x = []
+    y = []
+    for i in range(len(data)):
+        data[i][1] = 960 - int(data[i][1])
+        x.append(int(data[i][0]))
+        y.append(int(data[i][1]))
+    x = np.array(x)
+    y = np.array(y)
+    slope, intercept, r_2 = calc_slope_line.linear_regression(x, y)
+    print(r_2)
     x_bot = []
     y_bot = []
     x_top = []
     y_top = []
-    k_num = 0
+    for i in range(len(x)):
+        if x[i] * slope + intercept - y[i] > 0:
+            x_bot.append(x[i])
+            y_bot.append(y[i])
+        else:
+            x_top.append(x[i])
+            y_top.append(y[i])
+    x_bot = np.array(x_bot)
+    y_bot = np.array(y_bot)
+    x_top = np.array(x_top)
+    y_top = np.array(y_top)
+    slope_bot, intercept_bot, r2_bot = calc_slope_line.linear_regression(x_bot, y_bot)
+    slope_top, intercept_top, r2_top = calc_slope_line.linear_regression(x_top, y_top)
+    print(f"r2_bot = {r2_bot},r2_top = {r2_top}")
+    if ((1-r2_bot) > 1e-2) or ((1-r2_bot) > 1e-2):
+        return None, None, None, None
+
+    delet = []
+    for i in range(len(x_bot)):
+        if abs(y_bot[i] - slope_bot * x_bot[i] - intercept_bot) > 10:
+            delet.append(i)
+    print(f"len(x_bot): {len(x_bot)},delet: {delet})")
+    x_bot = np.delete(x_bot, delet)
+    y_bot = np.delete(y_bot, delet)
+    # y_pred = slope_bot * x_bot + intercept_bot
+    delet = []
+    for i in range(len(x_top)):
+        if abs(y_top[i] - slope_top * x_top[i] - intercept_top) > 10:
+            delet.append(i)
+    x_top = np.delete(x_top, delet)
+    y_top = np.delete(y_top, delet)
+    # y_pred = slope_top * x_top + intercept_top
+    return x_bot, y_bot, x_top, y_top
+
+txt_name = "C:\\Users\\Administrator\\Desktop\\BYD\\20250520\\frame_7800_2.jpg_zuobiao.txt"
+test_name = r'C:\Users\Administrator\Desktop\BYD\error\20250620\20250620\INPUT_MUST_NOT_BE_EMPTY.txt'
+def test_get_data(txt_name):
+    with open(txt_name, 'r', encoding='utf-8') as f:
+        lines = f.readlines()
+    data = []
+    for i, line in enumerate(lines, 1):
+        data.append(line.split())
+    x = []
+    y = []
     for i in range(len(data)):
         data[i][1] = 960 - int(data[i][1])
-    for i in range(len(data)):
         # if k * int(data[i][0]) + b - int(data[i][1]) < 0:
         #     continue
-        if i > 1:
-            if (int(data[i][0]) - int(data[i-1][0]))*(int(data[i+2][0]) - int(data[i][0]))<0:
-                k_num = i
-                break;
-    for i in range(len(data)):
-        if k * int(data[i][0]) + b - int(data[i][1]) < 0:
-            continue
-        if i < k_num:
-            x_bot.append(int(data[i][0]))
-            y_bot.append(int(data[i][1]))
+        x.append(int(data[i][0]))
+        y.append(int(data[i][1]))
+    x = np.array(x)
+    y = np.array(y)
+    slope,intercept,r_2 = calc_slope_line.linear_regression(x, y)
+    fig, axes = plt.subplots(nrows=2, ncols=2, figsize=(10, 8))
+    axes[0, 0].scatter(x,y, color='blue', label='orgin')
+    # 绘制拟合线
+    y_pred = slope * x + intercept
+    axes[0, 0].plot(x, y_pred, color='red', label='fix')
+
+
+
+    print(r_2)
+
+    x_bot = []
+    y_bot = []
+    x_top = []
+    y_top = []
+    for i in range(len(x)):
+        if x[i] * slope + intercept - y[i] > 0:
+            x_bot.append(x[i])
+            y_bot.append(y[i])
         else:
-            x_top.append(int(data[i][0]))
-            y_top.append(int(data[i][1]))
-    # print(x_bot, y_bot, x_top, y_top)
-    return x_bot, y_bot, x_top, y_top
+            x_top.append(x[i])
+            y_top.append(y[i])
+    x_bot = np.array(x_bot)
+    y_bot = np.array(y_bot)
+    x_top = np.array(x_top)
+    y_top = np.array(y_top)
+    slope_bot, intercept_bot, r2_bot = calc_slope_line.linear_regression(x_bot, y_bot)
+    slope_top, intercept_top, r2_top = calc_slope_line.linear_regression(x_top, y_top)
+    print(f"r2_bot = {r2_bot},r2_top = {r2_top}")
+    axes[1, 0].scatter(x_bot,y_bot, color='blue', label='orgin')
+    # 绘制拟合线
+    y_pred = slope_bot * x_bot + intercept_bot
+    axes[1, 0].plot(x_bot, y_pred, color='red', label='fix')
 
 
+    axes[1, 1].scatter(x_top,y_top, color='blue', label='orgin')
+    # 绘制拟合线
+    y_pred = slope_top * x_top + intercept_top
+    axes[1, 1].plot(x_top, y_pred, color='red', label='fix')
+
+    plt.show()
+    return x_bot, y_bot, x_top, y_top
+
 

py/main_test.py

@@ -10,100 +10,43 @@ import model
 model = model.Model()
 alpha = model.alpha
 beta = model.beta
-
-img_path = r'C:\Users\Administrator\Desktop\BYD\20250520\frame__9600_2_yolo.jpg'
-txt_name = "C:\\Users\\Administrator\\Desktop\\BYD\\20250520\\frame_9600_2.jpg_zuobiao.txt"
-output_image = "9600_2.jpg"
-
-x_bot, y_bot, x_top, y_top = get_data.get_data(txt_name)
-
-x_bot = np.array(x_bot)
-y_bot = np.array(y_bot)
-x_top = np.array(x_top)
-y_top = np.array(y_top)
-slope_top, intercept_top ,r2 = calc_slope_line.linear_regression(x_top,y_top)
-
-x_zero, y_zero = calc_way.calc_zeros()
-x_zero = np.array(x_zero)
-y_zero = np.array(y_zero)
-slope_zero, intercept_zero,r2_zero = calc_slope_line.linear_regression(x_zero,y_zero)
-
-# 绘制原始数据
-plt.scatter(x_zero,y_zero, color='blue', label='orgin')
-
-# 绘制拟合线
-y_pred = slope_zero * x_zero + intercept_zero
-plt.plot(x_zero, y_pred, color='red', label='fix')
-
-plt.xlabel('X')
-plt.ylabel('Y')
-plt.legend()
-plt.show()
-
-
-# # 绘制原始数据
-# plt.scatter(x_top,y_top, color='blue', label='orgin')
-#
-# # 绘制拟合线
-# y_pred = slope_top * x_top + intercept_top
-# plt.plot(x_top, y_pred, color='red', label='fix')
-#
-# plt.xlabel('X')
-# plt.ylabel('Y')
-# plt.legend()
-# plt.show()
-
-# 1. 读取图像（替换为你的图片路径）
-image = cv2.imread(img_path)  # 默认读取BGR格式
-
-# 检查图像是否成功加载
-if image is None:
-    print("Error: 无法读取图像，请检查路径！")
-    exit()
-
-point1 = (int(x_zero[0]), int(960-y_zero[0]))
-point2 = (int(x_zero[-1]), int(960-y_zero[-1]))
-cv2.line(image, point1, point2, (0, 255, 0), 1)
-Z = 0
-
-for i in range(len(x_bot)):
-    k = calc_slope_line.get_k(alpha,beta,x_bot[i],y_bot[i])
-    print(f"第{i+1}个点的斜率为：{k}")
-    b = calc_slope_line.get_b(x_bot[i],y_bot[i],k)
-    print(f"第{i+1}个点的截距为：{b}")
-    x = (intercept_top - b) / (k - slope_top)
-    y = k * x + b
-    print(f"第{i+1}个点的坐标为为：{x_bot[i],y_bot[i]}")
-    print(f"第{i+1}个点对应的上沿点为：{x,y}")
-    Zw = calc_way.calc_height(x_bot[i],y_bot[i], x, y, alpha, beta)
-    Xw, Yw = calc_way.calc_distance(x_bot[i],y_bot[i], alpha, beta)
-    print(f"第{i+1}个点的坐标为为：{Xw, Yw }")
-    point1 = (x_bot[i], 960-y_bot[i])
-    point2 = (int(x), 960-int(y))
-    cv2.line(image, point1, point2, (0, 255, 0), 1)
-    # text = f"Xw,Yw:{int(Xw),int(Yw)},Zw:{int(Zw)}"
-    # position = (x_bot[i], 960-y_bot[i])  # 文字左下角坐标 (x, y)
-    # font = cv2.FONT_HERSHEY_SIMPLEX  # 字体类型
-    # font_scale = 1  # 字体大小
-    # color = (0, 255, 255)  # 黄色（BGR格式）
-    # thickness = 1  # 文字粗细
-    # cv2.putText(image, text, position, font, font_scale, color, thickness, cv2.LINE_AA)
-    print("路沿高度", Zw)
-    Z = Z + Zw
-print(f"平均路沿高度为：{Z/len(x_bot)}")
-
-text = f"avg_height:{Z/len(x_bot)}"
-position = (50, 200)           # 文字左下角坐标 (x, y)
-font = cv2.FONT_HERSHEY_SIMPLEX # 字体类型
-font_scale = 1               # 字体大小
-color = (0, 255, 255)          # 黄色（BGR格式）
-thickness = 1                  # 文字粗细
-
-# 3. 在图像上绘制文字
-cv2.putText(image, text, position, font, font_scale, color, thickness, cv2.LINE_AA)
-
-cv2.imshow("Image with Line", image)
-cv2.waitKey(0)          # 按任意键关闭窗口
-cv2.destroyAllWindows()
-
-cv2.imwrite(output_image, image)
+img_path = r'C:\Users\Administrator\Desktop\BYD\error\20250620\20250620\pic1.jpg'
+def draw_lines(img_path):
+    image = cv2.imread(img_path)
+    if image is None:
+            print("Error: 无法读取图像，请检查路径！")
+            exit()
+
+    x_zero, y_zero = calc_way.calc_zeros_xto0(862+60)
+    print(x_zero)
+    print(y_zero)
+    point1 = (int(x_zero[0]),960-int(y_zero[0]))
+    point2 = (int(x_zero[-1]),960-int(y_zero[-1]))
+    cv2.line(image, point1, point2, (0, 0, 255), 1)
+
+    x_zero, y_zero = calc_way.calc_zeros_yto0(0)
+    print(x_zero)
+    print(y_zero)
+    point1 = (int(x_zero[0]),960-int(y_zero[0]))
+    point2 = (int(x_zero[-1]),960-int(y_zero[-1]))
+    cv2.line(image, point1, point2, (0, 0, 255), 1)
+
+    x_zero, y_zero = calc_way.calc_zeros_xto0(862+300)
+    print(x_zero)
+    print(y_zero)
+    point1 = (int(x_zero[0]),960-int(y_zero[0]))
+    point2 = (int(x_zero[-1]),960-int(y_zero[-1]))
+    cv2.line(image, point1, point2, (0, 0, 255), 1)
+
+    x_zero, y_zero = calc_way.calc_zeros_yto0(0-480)
+    print(x_zero)
+    print(y_zero)
+    point1 = (int(x_zero[0]),960-int(y_zero[0]))
+    point2 = (int(x_zero[-1]),960-int(y_zero[-1]))
+    cv2.line(image, point1, point2, (0, 0, 255), 1)
+
+    cv2.imshow('image', image)
+    cv2.waitKey(0)
+    cv2.destroyAllWindows()
+
+draw_lines(img_path)

py/measure_lib.py

@@ -14,7 +14,7 @@ alpha = model.alpha
 beta = model.beta
 
 img_path = r'C:\Users\Administrator\Desktop\BYD\20250520\frame_7800_2_yolo.jpg'
-txt_name = "C:\\Users\\Administrator\\Desktop\\BYD\\20250520\\frame_6000_2.jpg_zuobiao.txt"
+txt_name = "C:\\Users\\Administrator\\Desktop\\BYD\\20250520\\frame_6600_2.jpg_zuobiao.txt"
 output_folder = 'C:\\Users\\Administrator\\Desktop\\BYD\\Visual measurement\\pic\\7800'
 
 """
@@ -175,25 +175,57 @@ output_folder = 'C:\\Users\\Administrator\\Desktop\\BYD\\Visual measurement\\pic
 
 def vs_measurement(txt_name,position):
     if not os.path.exists(txt_name):
-        return None,None,None,None
+        return 0, None,None,None,None
     # 获取数据
     x_bot, y_bot, x_top, y_top = get_data.get_data(txt_name)
-    x_bot = np.array(x_bot)
-    y_bot = np.array(y_bot)
-    x_top = np.array(x_top)
-    y_top = np.array(y_top)
+    if x_bot.any() == None:
+        return 0, None, None, None, None
+    # x_bot = np.array(x_bot)
+    # y_bot = np.array(y_bot)
+    # x_top = np.array(x_top)
+    # y_top = np.array(y_top)
+    # print(f"x_bot: {x_bot.shape}")
+    # print(f"x_top: {x_top.shape}")
 
+    # for i in range(len(x_bot)):
+    #     print(x_bot[i],y_bot[i])
+    # print("qiehuan")
+    # for i in range(len(x_top)):
+    #     print(x_top[i],y_top[i])
     # 拟合路沿上下直线方程
     slope_bot, intercept_bot, r2_bot = calc_slope_line.linear_regression(x_bot, y_bot)
+    # 绘制原始数据
+    # plt.scatter(x_bot,y_bot, color='blue', label='orgin')
+    #
+    # # 绘制拟合线
+    # y_pred = slope_bot * x_top + intercept_bot
+    # plt.plot(x_top, y_pred, color='red', label='fix')
+    #
+    # plt.xlabel('X')
+    # plt.ylabel('Y')
+    # plt.legend()
+    # plt.show()
     slope_top, intercept_top, r2_top = calc_slope_line.linear_regression(x_top,y_top)
+    # 绘制原始数据
+    # plt.scatter(x_top,y_top, color='blue', label='orgin')
+    #
+    # # 绘制拟合线
+    # y_pred = slope_top * x_top + intercept_top
+    # plt.plot(x_top, y_pred, color='red', label='fix')
+    #
+    # plt.xlabel('X')
+    # plt.ylabel('Y')
+    # plt.legend()
+    # plt.show()
 
     # 拟合车轮垂线方程
-    # x_zero_xto0, y_zero_xto0 = calc_way.calc_zeros_xto0()
+    # x_zero_xto0, y_zero_xto0 = calc_way.calc_zeros_xto0(922)
     # x_zero_xto0 = np.array(x_zero_xto0)
     # y_zero_xto0 = np.array(y_zero_xto0)
     # slope_zero_xto0, intercept_zero_xto0, r2_zero_xto0 = calc_slope_line.linear_regression(x_zero_xto0, y_zero_xto0)
-    slope_zero_xto0 = 0.6060163775784262
-    intercept_zero_xto0 = -16.33378114591926
+    slope_zero_xto0 = 0.5816899594078648
+    intercept_zero_xto0 = 28.214865671895723
+    # print(slope_zero_xto0, intercept_zero_xto0, r2_zero_xto0)
 
     # 计算路沿底部与车轮垂线的交点
     x_intersection,y_intersection = calc_slope_line.find_intersection((slope_bot, -1, intercept_bot), (slope_zero_xto0, -1, intercept_zero_xto0))
@@ -205,6 +237,8 @@ def vs_measurement(txt_name,position):
     Zw_intersection = calc_way.calc_height(x_intersection, y_intersection, x, y, alpha, beta)
     Xw_intersection, Yw_intersection = calc_way.calc_distance(x_intersection, y_intersection, alpha, beta)
 
+    # #位置修正
+    # Yw_intersection = Yw_intersection + model.y
     Xw_bot = []
     Yw_bot = []
     for i in range(len(x_bot)):
@@ -216,19 +250,22 @@ def vs_measurement(txt_name,position):
     slope_Xw, intercept_Xw, r2_Xw = calc_slope_line.linear_regression(Xw_bot, Yw_bot)
     angle = math.atan(slope_Xw)
 
+    # 位置修正
+    Yw_intersection = Yw_intersection + model.y
+    position = position + model.x
+
     # 计算给出postion的位置信息
-    Yw = slope_Xw * position + intercept_Xw
-    x_pos, y_pos = calc_way.calc_distance2(position, Yw, alpha, beta)
+    Yw_pos = slope_Xw * position + intercept_Xw
+    x_pos, y_pos = calc_way.calc_distance2(position, Yw_pos, alpha, beta)
     k_pos = calc_slope_line.get_k(alpha, beta, x_pos, y_pos)
     b_pos = calc_slope_line.get_b(x_pos, y_pos, k_pos)
     x_pos_top, y_pos_top = calc_slope_line.find_intersection((k_pos, -1, b_pos), (slope_top, -1, intercept_top))
     Zw_pos = calc_way.calc_height(x_pos, y_pos, x_pos_top, y_pos_top, alpha, beta)
 
-
-
-    distance = -(Yw_intersection-model.distance) * math.cos(angle)
-    distance_pos = ((-intercept_Xw / slope_Xw) - position)/ ((-intercept_Xw / slope_Xw) - Xw_intersection) * distance
-    return Zw_intersection, distance, Zw_pos , distance_pos
+    distance = - Yw_intersection * math.cos(angle)
+    distance_pos = -(Yw_pos + model.y) * math.cos(angle)
+    # distance_pos = ((-intercept_Xw / slope_Xw) - position) / ((-intercept_Xw / slope_Xw) - Xw_intersection) * distance
+    return 1, Zw_intersection, distance, Zw_pos, distance_pos
 
 
 if __name__ == '__main__':
@@ -242,7 +279,8 @@ if __name__ == '__main__':
     # print(f"x_zero: {x_zero}")
     # print(f"y_zero: {y_zero}")
     t = time.time()
-    Zw_intersection, distance, Zw_pos, distance_pos = vs_measurement(txt_name,1500)
+    test_name = r'C:\Users\Administrator\Desktop\BYD\error\20250620\20250620\CANNOT_CALCULATE_LINER_REGRESSION_.txt'
+    state, Zw_intersection, distance, Zw_pos, distance_pos = vs_measurement(test_name,900)
     # vs_measurement(txt_name)
     print(f"time: {time.time() - t}")
     print(Zw_intersection, distance, Zw_pos, distance_pos)

py/model.py

@@ -8,16 +8,21 @@ class Model:
                            [0, 801.7619, 532],
                            [0, 0, 1]])
         self.f = 3.6
-        self.H = 1019
+        self.H = 1019.0000170167332-9
         self.dx = self.f / self.K[0, 0]
         self.dy = self.f / self.K[1, 1]
         self.u0 = self.K[0, 2]
         self.v0 = self.K[1, 2]
         # self.alpha = 0.7474
         # self.beta = 0.954
-        self.alpha = 7.005e-01
-        self.beta = 9.143e-01
+        self.alpha = 0.7072338025822084
+        self.beta = 0.9077237961986776
+        # self.alpha = 0.7281555851322634
+        # self.beta = 0.8871365166099384
         # X轴距离车轮线的距离
         self.distance = 60
         self.tire_x = 160
-        self.tire_y = 22
+        self.tire_y = 22
+        #位置修正
+        self.y = -70
+        self.x = 22

py/new.txt

@@ -1,199 +0,0 @@
-261 123
-258 120
-255 120
-234 141
-228 141
-219 132
-219 129
-216 126
-216 123
-213 120
-210 120
-207 123
-207 126
-204 129
-204 132
-195 141
-195 144
-192 147
-189 147
-186 150
-186 153
-180 159
-180 162
-177 165
-177 168
-171 174
-171 177
-162 186
-162 189
-159 192
-159 195
-156 198
-156 204
-147 213
-147 216
-132 231
-132 234
-123 243
-123 246
-120 249
-120 252
-114 258
-114 261
-108 267
-108 270
-105 273
-105 276
-102 279
-102 282
-99 285
-99 288
-93 294
-93 297
-87 303
-87 306
-84 309
-84 312
-81 315
-81 318
-78 321
-78 324
-75 327
-75 330
-72 333
-72 336
-63 345
-63 348
-60 351
-60 354
-51 363
-51 366
-48 369
-48 372
-36 384
-36 387
-30 393
-30 396
-24 402
-24 405
-12 417
-12 420
-3 429
-0 429
-0 465
-3 465
-9 459
-9 456
-15 450
-15 447
-21 441
-21 438
-24 435
-24 432
-27 429
-30 429
-30 426
-33 423
-33 420
-45 408
-45 405
-48 402
-48 399
-60 387
-60 378
-66 372
-69 372
-72 369
-72 366
-78 360
-81 360
-93 348
-99 348
-114 333
-114 330
-120 324
-123 324
-123 321
-138 306
-138 303
-141 300
-141 297
-144 294
-147 294
-147 291
-150 288
-150 282
-162 270
-162 264
-168 258
-171 258
-171 255
-174 252
-174 249
-186 237
-186 234
-189 231
-189 228
-192 225
-195 225
-198 222
-198 213
-204 207
-207 207
-207 204
-210 201
-210 198
-222 186
-222 180
-228 174
-231 174
-234 171
-234 168
-243 159
-243 156
-249 150
-249 144
-258 135
-258 126
-261 123
-267 114
-267 111
-279 99
-291 78
-294 78
-294 75
-300 66
-303 66
-306 63
-306 57
-312 51
-315 51
-315 48
-333 30
-333 24
-336 21
-336 15
-324 15
-321 18
-318 18
-315 21
-312 21
-309 24
-306 24
-306 36
-309 39
-309 42
-306 45
-306 48
-303 51
-303 54
-300 57
-300 66
-294 75
-291 75
-291 78
-279 99
-279 99
-267 111
-267 114

py/setup_folder/rename.py

@@ -1,12 +0,0 @@
-import os
-
-
-# 获取指定路径下的文件
-dirs = os.listdir("/")
-# 循环读取路径下的文件并筛选输出
-for i in dirs:
-    if os.path.splitext(i)[-1] == ".so":
-        old_name = i
-        new_name = i.split('.')[0] + ".so"
-        os.rename(old_name, new_name)
-        print("rename:" + old_name + "->" + new_name) 

py/setup_folder/setup.py

@@ -1,9 +0,0 @@
-from distutils.core import setup
-from Cython.Build import cythonize
-
-setup(ext_modules=cythonize(["../measure_lib.py"]))
-setup(ext_modules=cythonize(["../calc_way.py"]))
-setup(ext_modules=cythonize(["../calc_slope_line.py"]))
-setup(ext_modules=cythonize(["../get_data.py"]))
-setup(ext_modules=cythonize(["../model.py"]))
-setup(ext_modules=cythonize(["../main_test.py"]))

py/setup_folder/test.py

@@ -1 +0,0 @@
-import measure_lib

py/test.py

@@ -29,158 +29,197 @@ alpha = model.alpha
 beta = model.beta
 
 img_path = r'C:\Users\Administrator\Desktop\BYD\20250520\frame_7800_2_yolo.jpg'
-txt_name = "C:\\Users\\Administrator\\Desktop\\BYD\\20250520\\frame_7800_2.jpg_zuobiao.txt"
+txt_name = "C:\\Users\\Administrator\\Desktop\\BYD\\20250520\\frame_9600_2.jpg_zuobiao.txt"
 output_folder = 'C:\\Users\\Administrator\\Desktop\\BYD\\Visual measurement\\pic\\7800'
-
-
-def vs_measurement(txt_name):
-    os.makedirs(output_folder, exist_ok=True)
+test_name = r'C:\Users\Administrator\Desktop\BYD\error\20250620\20250620\CANNOT_CALCULATE_LINER_REGRESSION_.txt'
+n="CANNOT_CALCULATE_LINER_REGRESSION_"
+n='INPUT_MUST_NOT_BE_EMPTY'
+def vs_measurement(txt_name,position):
+    if not os.path.exists(txt_name):
+        return None,None,None,None
     # 获取数据
-    x_bot, y_bot, x_top, y_top = get_data.get_data(txt_name)
-    x_bot = np.array(x_bot)
-    y_bot = np.array(y_bot)
-    x_top = np.array(x_top)
-    y_top = np.array(y_top)
-
-    # 拟合路沿上下直线方程
-    slope_bot, intercept_bot ,r2_bot = calc_slope_line.linear_regression(x_bot, y_bot)
-    slope_top, intercept_top ,r2_top = calc_slope_line.linear_regression(x_top,y_top)
-
-    # 拟合车轮垂线方程
-    x_zero_xto0, y_zero_xto0 = calc_way.calc_zeros_xto0()
-    x_zero_xto0 = np.array(x_zero_xto0)
-    y_zero_xto0 = np.array(y_zero_xto0)
-    slope_zero_xto0, intercept_zero_xto0,r2_zero_xto0 = calc_slope_line.linear_regression(x_zero_xto0, y_zero_xto0)
-
-    # 拟合X轴线方程
-    x_zero, y_zero = calc_way.calc_zeros_yto0()
-    x_zero = np.array(x_zero)
-    y_zero = np.array(y_zero)
-    print(x_zero,y_zero)
-    slope_zero, intercept_zero, r2_zero = calc_slope_line.linear_regression(x_zero, y_zero)
-
-    # 计算路沿底部与车轮垂线得交点
-    x_jiao, y_jiao =calc_slope_line.find_intersection((slope_bot,-1,intercept_bot),(slope_zero_xto0,-1,intercept_zero_xto0))
-
-
+    # x_bot, y_bot, x_top, y_top, k_num = get_data.get_data(txt_name)
+    # print(k_num)
+    # x_bot = np.array(x_bot)
+    # y_bot = np.array(y_bot)
+    # x_top = np.array(x_top)
+    # y_top = np.array(y_top)
+    # # print(x_bot[0],x_bot[1],x_bot[2])
+    # # 拟合路沿上下直线方程
+    # slope_bot, intercept_bot, r2_bot = calc_slope_line.linear_regression(x_bot, y_bot)
+    # slope_top, intercept_top, r2_top = calc_slope_line.linear_regression(x_top,y_top)
+    # print(f"r2_bot = {r2_bot}")
     # # 绘制原始数据
-    # plt.scatter(x_top,y_top, color='blue', label='orgin')
+    # plt.scatter(x_bot,y_bot, color='blue', label='orgin')
     #
     # # 绘制拟合线
-    # y_pred = slope_top * x_top + intercept_top
+    # y_pred = slope_bot * x_top + intercept_bot
     # plt.plot(x_top, y_pred, color='red', label='fix')
+    # #
+    # # plt.xlabel('X')
+    # # plt.ylabel('Y')
+    # # plt.legend()
+    # # plt.show()
+    #
+    # Zw_old = []
+    # for i in range(len(x_bot)):
+    #     k = calc_slope_line.get_k(alpha,beta,x_bot[i],y_bot[i])
+    #     b = calc_slope_line.get_b(x_bot[i],y_bot[i],k)
+    #     x = (intercept_top - b) / (k - slope_top)
+    #     y = k * x + b
+    #     Zw = calc_way.calc_height(x_bot[i],y_bot[i], x, y, alpha, beta)
+    #     Zw_old.append(Zw)
+    # Zw_old = np.array(Zw_old)
+    #
+    #
+    #
+    # slope_zero_xto0 = 0.6060163775784262
+    # intercept_zero_xto0 = -16.33378114591926
+    #
+    # # 计算路沿底部与车轮垂线的交点
+    # x_intersection,y_intersection = calc_slope_line.find_intersection((slope_bot, -1, intercept_bot), (slope_zero_xto0, -1, intercept_zero_xto0))
+    #
+    # #计算交点的位置信息
+    # k = calc_slope_line.get_k(alpha, beta, x_intersection,y_intersection)
+    # b = calc_slope_line.get_b(x_intersection, y_intersection, k)
+    # x, y = calc_slope_line.find_intersection((k, -1, b), (slope_top, -1, intercept_top))
+    # Zw_intersection = calc_way.calc_height(x_intersection, y_intersection, x, y, alpha, beta)
+    # Xw_intersection, Yw_intersection = calc_way.calc_distance(x_intersection, y_intersection, alpha, beta)
+    #
+    # Xw_bot = []
+    # Yw_bot = []
+    # for i in range(len(x_bot)):
+    #     Xw, Yw = calc_way.calc_distance(x_bot[i], y_bot[i], alpha, beta)
+    #     Xw_bot.append(Xw)
+    #     Yw_bot.append(Yw)
+    #
+    # # 计算路沿与车的夹角
+    # slope_Xw, intercept_Xw, r2_Xw = calc_slope_line.linear_regression(Xw_bot, Yw_bot)
+    # angle = math.atan(slope_Xw)
     #
+    # # 计算给出postion的位置信息
+    # Yw = slope_Xw * position + intercept_Xw
+    # x_pos, y_pos = calc_way.calc_distance2(position, Yw, alpha, beta)
+    # k_pos = calc_slope_line.get_k(alpha, beta, x_pos, y_pos)
+    # b_pos = calc_slope_line.get_b(x_pos, y_pos, k_pos)
+    # x_pos_top, y_pos_top = calc_slope_line.find_intersection((k_pos, -1, b_pos), (slope_top, -1, intercept_top))
+    # Zw_pos = calc_way.calc_height(x_pos, y_pos, x_pos_top, y_pos_top, alpha, beta)
+    #
+    # distance = -(Yw_intersection-model.distance) * math.cos(angle)
+    # distance_pos = ((-intercept_Xw / slope_Xw) - position)/ ((-intercept_Xw / slope_Xw) - Xw_intersection) * distance
+    # Z1 = Zw_pos
+    # D1 = distance_pos
+    # print(f"Zw_pos = {Zw_pos}, D1 = {D1}")
+
+    x_bot, y_bot, x_top, y_top = get_data.test_get_data(txt_name)
+    # print(x_bot[137],x_bot[138],x_bot[139])
+    slope_bot, intercept_bot, r2_bot = calc_slope_line.linear_regression(x_bot, y_bot)
+    slope_top, intercept_top, r2_top = calc_slope_line.linear_regression(x_top, y_top)
+    Zw_new = []
+    for i in range(len(x_bot)):
+        k = calc_slope_line.get_k(alpha, beta, x_bot[i], y_bot[i])
+        b = calc_slope_line.get_b(x_bot[i], y_bot[i], k)
+        x = (intercept_top - b) / (k - slope_top)
+        y = k * x + b
+        Zw = calc_way.calc_height(x_bot[i], y_bot[i], x, y, alpha, beta)
+        Zw_new.append(Zw)
+    Zw_new = np.array(Zw_new)
+    error = []
+    # for i in range(len(Zw_old)):
+    #     error.append(Zw_old[i] - Zw_new[i+k_num])
+    # print(error)
+    fig, axes = plt.subplots(nrows=3, ncols=2, figsize=(10, 8))
+    y_pred = slope_bot * x_bot + intercept_bot
+    axes[0,0].scatter(x_bot,y_bot-y_pred, color='blue', label='orgin')
+    y_pred = slope_top * x_top + intercept_top
+    axes[0,1].scatter(x_top,y_top-y_pred, color='blue', label='orgin')
+    delet = []
+    for i in range(len(x_bot)):
+        if abs(y_bot[i]-slope_bot * x_bot[i] - intercept_bot) > 10:
+            delet.append(i)
+    print(f"len(x_bot): {len(x_bot)},delet: {delet})")
+    x_bot = np.delete(x_bot, delet)
+    y_bot = np.delete(y_bot, delet)
+    y_pred = slope_bot * x_bot + intercept_bot
+    axes[1,0].scatter(x_bot,y_bot-y_pred, color='blue', label='orgin')
+
+    delet = []
+    for i in range(len(x_top)):
+        if abs(y_top[i] - slope_top * x_top[i] - intercept_top) > 10:
+            delet.append(i)
+    x_top = np.delete(x_top, delet)
+    y_top = np.delete(y_top, delet)
+    y_pred = slope_top * x_top + intercept_top
+    axes[1, 1].scatter(x_top, y_top - y_pred, color='blue', label='orgin')
+
+
+    slope_bot, intercept_bot, r2_bot = calc_slope_line.linear_regression(x_bot, y_bot)
+    slope_top, intercept_top, r2_top = calc_slope_line.linear_regression(x_top, y_top)
+    print(f"r2_bot = {r2_bot},r2_top = {r2_top}")
+
+    axes[2, 0].scatter(x_bot, y_bot, color='blue', label='orgin')
+    # 绘制拟合线
+    y_pred = slope_bot * x_bot + intercept_bot
+    axes[2, 0].plot(x_bot, y_pred, color='red', label='fix')
+
+    axes[2, 1].scatter(x_top, y_top, color='blue', label='orgin')
+    # 绘制拟合线
+    y_pred = slope_top * x_top + intercept_top
+    axes[2, 1].plot(x_top, y_pred, color='red', label='fix')
+    plt.show()
+
+    # 绘制拟合线
+    y_pred = slope_bot * x_bot + intercept_bot
+    # plt.plot(x_bot, y_pred, color='red', label='fix')
+    # plt.scatter(x_bot,y_bot-y_pred, color='blue', label='orgin')
     # plt.xlabel('X')
     # plt.ylabel('Y')
     # plt.legend()
     # plt.show()
+    # 拟合车轮垂线方程
+    # x_zero_xto0, y_zero_xto0 = calc_way.calc_zeros_xto0()
+    # x_zero_xto0 = np.array(x_zero_xto0)
+    # y_zero_xto0 = np.array(y_zero_xto0)
+    # slope_zero_xto0, intercept_zero_xto0, r2_zero_xto0 = calc_slope_line.linear_regression(x_zero_xto0, y_zero_xto0)
+    slope_zero_xto0 = 0.6060163775784262
+    intercept_zero_xto0 = -16.33378114591926
+
+    # 计算路沿底部与车轮垂线的交点
+    x_intersection,y_intersection = calc_slope_line.find_intersection((slope_bot, -1, intercept_bot), (slope_zero_xto0, -1, intercept_zero_xto0))
+
+    #计算交点的位置信息
+    k = calc_slope_line.get_k(alpha, beta, x_intersection,y_intersection)
+    b = calc_slope_line.get_b(x_intersection, y_intersection, k)
+    x, y = calc_slope_line.find_intersection((k, -1, b), (slope_top, -1, intercept_top))
+    Zw_intersection = calc_way.calc_height(x_intersection, y_intersection, x, y, alpha, beta)
+    Xw_intersection, Yw_intersection = calc_way.calc_distance(x_intersection, y_intersection, alpha, beta)
 
-    Z = 0
-    Y = 0
-    max_Zw = 0
-    max_Zw_index = 0
-    min_Zw = 0
-    min_Zw_index = 0
-    max_Yw = 0
-    max_Yw_index = 0
-    min_Yw = 0
-    min_Yw_index = 0
     Xw_bot = []
     Yw_bot = []
-
     for i in range(len(x_bot)):
-        image = cv2.imread(img_path)  # 默认读取BGR格式
-        if image is None:
-            print("Error: 无法读取图像，请检查路径！")
-            exit()
+        Xw, Yw = calc_way.calc_distance(x_bot[i], y_bot[i], alpha, beta)
+        Xw_bot.append(Xw)
+        Yw_bot.append(Yw)
 
-        # 定义点的坐标 (x, y)
-        point = (int(x_jiao), 960-int(y_jiao))
+    # 计算路沿与车的夹角
+    slope_Xw, intercept_Xw, r2_Xw = calc_slope_line.linear_regression(Xw_bot, Yw_bot)
+    angle = math.atan(slope_Xw)
 
-        # 画一个红色圆点（半径5，颜色BGR格式，线宽-1表示填充）
-        cv2.circle(image, point, 5, (0, 0, 255), -1)
+    # 计算给出postion的位置信息
+    Yw = slope_Xw * position + intercept_Xw
+    x_pos, y_pos = calc_way.calc_distance2(position, Yw, alpha, beta)
+    k_pos = calc_slope_line.get_k(alpha, beta, x_pos, y_pos)
+    b_pos = calc_slope_line.get_b(x_pos, y_pos, k_pos)
+    x_pos_top, y_pos_top = calc_slope_line.find_intersection((k_pos, -1, b_pos), (slope_top, -1, intercept_top))
+    Zw_pos = calc_way.calc_height(x_pos, y_pos, x_pos_top, y_pos_top, alpha, beta)
 
-        point1 = (int(x_zero[0]), int(960 - y_zero[0]))
-        point2 = (int(x_zero[-1]), int(960 - y_zero[-1]))
-        cv2.line(image, point1, point2, (0, 0, 255), 2)
+    distance = -(Yw_intersection-model.distance) * math.cos(angle)
+    distance_pos = ((-intercept_Xw / slope_Xw) - position)/ ((-intercept_Xw / slope_Xw) - Xw_intersection) * distance
+    Z2 = Zw_pos
+    D2 = distance_pos
+    print(f"Zw_pos = {Zw_pos}, D2 = {D2}")
+    return Zw_intersection, distance, Zw_pos , distance_pos
 
 
-        point1 = (int(x_zero_xto0[0]), int(960 - y_zero_xto0[0]))
-        point2 = (int(x_zero_xto0[-1]), int(960 - y_zero_xto0[-1]))
-        cv2.line(image, point1, point2, (0, 255, 255), 2)
 
-        k = calc_slope_line.get_k(alpha,beta,x_bot[i],y_bot[i])
-        b = calc_slope_line.get_b(x_bot[i],y_bot[i],k)
-        x = (intercept_top - b) / (k - slope_top)
-        y = k * x + b
-        Zw = calc_way.calc_height(x_bot[i],y_bot[i], x, y, alpha, beta)
-        Xw, Yw = calc_way.calc_distance(x_bot[i],y_bot[i], alpha, beta)
-        Xw_bot.append(Xw)
-        Yw_bot.append(Yw)
-        if i == 0:
-            max_Zw = Zw
-            min_Zw = Zw
-            max_Yw = Yw
-            min_Yw = Yw
-        else:
-            if Zw > max_Zw:
-                max_Zw = Zw
-                max_Zw_index = i
-            if Zw < min_Zw:
-                min_Zw = Zw
-                min_Zw_index = i
-            if Yw > max_Yw:
-                max_Yw = Yw
-                max_Yw_index = i
-            if Yw < min_Yw:
-                min_Yw = Yw
-                min_Yw_index = i
-        point1 = (x_bot[i], 960-y_bot[i])
-        point2 = (int(x), 960-int(y))
-        cv2.line(image, point1, point2, (0, 255, 0), 1)
-        text = f"Xw,Yw:{int(Xw),int(Yw)},Zw:{int(Zw)}"
-        position = (int(x), 960-int(y))
-        font = cv2.FONT_HERSHEY_SIMPLEX
-        font_scale = 1
-        color = (0, 0, 255)
-        thickness = 2
-        cv2.putText(image, text, position, font, font_scale, color, thickness, cv2.LINE_AA)
-        output_path = os.path.join(output_folder, f'{i+1}.jpg')
-        cv2.imwrite(output_path, image)
-        Z = Z + Zw
-        Y = Y + Yw
-    file_path = os.path.join(output_folder, 'data.txt')
-
-    with open(file_path, 'w', encoding='utf-8') as file:
-        file.write("该图片数据如下\n")
-        file.write(f"路沿平均高度为：{Z/len(x_bot)}\n")
-        file.write(f"最大高度为图{max_Zw_index+1},高度为：{max_Zw}\n")
-        file.write(f"最小高度为图{min_Zw_index+1},高度为：{min_Zw}\n")
-        file.write(f"路沿距离车辆平均距离为：{-Y/len(x_bot)}\n")
-        file.write(f"最远距离为图{min_Yw_index + 1},距离为：{min_Yw}\n")
-        file.write(f"最近距离为图{max_Yw_index + 1},距离为：{max_Yw}\n")
-    # 计算路沿与车的夹角
-    slope_Xw ,intercept_Xw ,r2_Xw = calc_slope_line.linear_regression(Xw_bot, Yw_bot)
-    angle = math.atan(slope_Xw)
-    Xw , Yw = calc_way.calc_distance(x_jiao, y_jiao, alpha, beta)
-    distance = -Yw * math.cos(angle)
-    distance_door = (-intercept_Xw/slope_Xw)/ ((-intercept_Xw/slope_Xw)-Xw)*distance
-
-    image = cv2.imread(img_path)  # 默认读取BGR格式
-    if image is None:
-        print("Error: 无法读取图像，请检查路径！")
-        exit()
-
-    cv2.circle(image, point, 5, (0, 0, 255), -1)
-    text = f"Xw,Yw:{int(Xw), int(Yw)},distance:{int(distance)},distance_door:{int(distance_door)}"
-    position = (int(x_jiao), 960 - int(y_jiao))
-    font = cv2.FONT_HERSHEY_SIMPLEX
-    font_scale = 0.5
-    color = (0, 0, 255)
-    thickness = 2
-    cv2.putText(image, text, position, font, font_scale, color, thickness, cv2.LINE_AA)
-    cv2.imshow("Image with Line", image)
-    cv2.waitKey(0)          # 按任意键关闭窗口
-    cv2.destroyAllWindows()
-
-vs_measurement(txt_name)
+vs_measurement(txt_name,877)