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, abs(r_value)
+    return slope, intercept, r_value**2
 
 """
 计算图像中两条直线的交点

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

py/get_data.py

@@ -1,9 +1,4 @@
-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
@@ -13,156 +8,38 @@ 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())
-    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)
+        # print(line.split())
     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(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 = []
+    k_num = 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:
         #     continue
-        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])
+        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]))
         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}")
-    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()
+            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
 
 
+

py/main_test.py

@@ -10,43 +10,100 @@ import model
 model = model.Model()
 alpha = model.alpha
 beta = model.beta
-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)
+
+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)

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_6600_2.jpg_zuobiao.txt"
+txt_name = "C:\\Users\\Administrator\\Desktop\\BYD\\20250520\\frame_6000_2.jpg_zuobiao.txt"
 output_folder = 'C:\\Users\\Administrator\\Desktop\\BYD\\Visual measurement\\pic\\7800'
 
 """
@@ -175,57 +175,25 @@ output_folder = 'C:\\Users\\Administrator\\Desktop\\BYD\\Visual measurement\\pic
 
 def vs_measurement(txt_name,position):
     if not os.path.exists(txt_name):
-        return 0, None,None,None,None
+        return None,None,None,None
     # 获取数据
     x_bot, y_bot, x_top, y_top = get_data.get_data(txt_name)
-    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}")
+    x_bot = np.array(x_bot)
+    y_bot = np.array(y_bot)
+    x_top = np.array(x_top)
+    y_top = np.array(y_top)
 
-    # 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(922)
+    # 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.5816899594078648
-    intercept_zero_xto0 = 28.214865671895723
-    # print(slope_zero_xto0, intercept_zero_xto0, r2_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))
@@ -237,8 +205,6 @@ 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)):
@@ -250,22 +216,19 @@ 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_pos = slope_Xw * position + intercept_Xw
-    x_pos, y_pos = calc_way.calc_distance2(position, Yw_pos, alpha, beta)
+    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 * 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
+
+
+    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
 
 
 if __name__ == '__main__':
@@ -279,8 +242,7 @@ if __name__ == '__main__':
     # print(f"x_zero: {x_zero}")
     # print(f"y_zero: {y_zero}")
     t = time.time()
-    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)
+    Zw_intersection, distance, Zw_pos, distance_pos = vs_measurement(txt_name,1500)
     # vs_measurement(txt_name)
     print(f"time: {time.time() - t}")
     print(Zw_intersection, distance, Zw_pos, distance_pos)

py/model.py

@@ -8,21 +8,16 @@ class Model:
                            [0, 801.7619, 532],
                            [0, 0, 1]])
         self.f = 3.6
-        self.H = 1019.0000170167332-9
+        self.H = 1019
         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 = 0.7072338025822084
-        self.beta = 0.9077237961986776
-        # self.alpha = 0.7281555851322634
-        # self.beta = 0.8871365166099384
+        self.alpha = 7.005e-01
+        self.beta = 9.143e-01
         # X轴距离车轮线的距离
         self.distance = 60
         self.tire_x = 160
-        self.tire_y = 22
-        #位置修正
-        self.y = -70
-        self.x = 22
+        self.tire_y = 22

py/new.txt

@@ -0,0 +1,199 @@
+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

@@ -0,0 +1,12 @@
+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

@@ -0,0 +1,9 @@
+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

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

py/test.py

@@ -29,197 +29,158 @@ 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_9600_2.jpg_zuobiao.txt"
+txt_name = "C:\\Users\\Administrator\\Desktop\\BYD\\20250520\\frame_7800_2.jpg_zuobiao.txt"
 output_folder = 'C:\\Users\\Administrator\\Desktop\\BYD\\Visual measurement\\pic\\7800'
-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
+
+
+def vs_measurement(txt_name):
+    os.makedirs(output_folder, exist_ok=True)
     # 获取数据
-    # 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}")
+    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))
+
+
     # # 绘制原始数据
-    # plt.scatter(x_bot,y_bot, color='blue', label='orgin')
+    # plt.scatter(x_top,y_top, color='blue', label='orgin')
     #
     # # 绘制拟合线
-    # y_pred = slope_bot * x_top + intercept_bot
+    # 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()
-    #
-    # 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)):
-        Xw, Yw = calc_way.calc_distance(x_bot[i], y_bot[i], alpha, beta)
-        Xw_bot.append(Xw)
-        Yw_bot.append(Yw)
+        image = cv2.imread(img_path)  # 默认读取BGR格式
+        if image is None:
+            print("Error: 无法读取图像，请检查路径！")
+            exit()
 
-    # 计算路沿与车的夹角
-    slope_Xw, intercept_Xw, r2_Xw = calc_slope_line.linear_regression(Xw_bot, Yw_bot)
-    angle = math.atan(slope_Xw)
+        # 定义点的坐标 (x, y)
+        point = (int(x_jiao), 960-int(y_jiao))
 
-    # 计算给出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)
+        # 画一个红色圆点（半径5，颜色BGR格式，线宽-1表示填充）
+        cv2.circle(image, point, 5, (0, 0, 255), -1)
 
-    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[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)
 
 
+        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)
 
-vs_measurement(txt_name,877)
+        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)