1

py/get_data.py

@@ -23,8 +23,8 @@ def get_data(txt_name):
     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 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

py/main.py

@@ -1,175 +0,0 @@
-
-import numpy as np
-import matplotlib.pyplot as plt
-import calc_way
-import get_data
-import calc_slope_line
-import cv2
-import model
-import os
-
-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_folder = 'C:\\Users\\Administrator\\Desktop\\BYD\\Visual measurement\\pic\\9600'
-
-"""
-计算图像中识别到的路沿距离车辆坐标原点的距离、高度信息
-参数：保存数据的txt文件路径
-返回值：
-"""
-# def vs_measurement(txt_name):
-#
-#     os.makedirs(output_folder, exist_ok=True)
-#     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_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()
-#
-#     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
-#
-#     for i in range(len(x_bot)):
-#         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, 0, 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)
-#         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.0
-#         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"路沿平均高度为：{}\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")
-
-def vs_measurement(txt_name):
-
-    # os.makedirs(output_folder, exist_ok=True)
-    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)
-
-
-    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
-
-    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)
-        Xw, Yw = calc_way.calc_distance(x_bot[i],y_bot[i], alpha, beta)
-        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
-        Z = Z + Zw
-        Y = Y + Yw
-    return Z/len(x_bot),-max_Yw
-
-if __name__ == '__main__':
-    vs_measurement(txt_name)

py/measure_lib.py

@@ -174,6 +174,8 @@ output_folder = 'C:\\Users\\Administrator\\Desktop\\BYD\\Visual measurement\\pic
 #     return Z/len(x_bot),-max_Yw
 
 def vs_measurement(txt_name,position):
+    if 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)

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/setup.py

@@ -6,6 +6,4 @@ 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"]))
-
-#python setup.py build_ext --inplace
+setup(ext_modules=cythonize(["../main_test.py"]))

py/test.py

@@ -0,0 +1,186 @@
+import math
+
+import numpy as np
+import matplotlib.pyplot as plt
+import calc_way
+import get_data
+import calc_slope_line
+import cv2
+import model
+import os
+# model = model.Model()
+# x,y=calc_way.calc_distance(model.tire_x, model.tire_y,model.alpha,model.beta)
+# print(x,y)
+# img_path = r'C:\Users\Administrator\Desktop\BYD\20250520\frame_7800_2_yolo.jpg'
+# x_zero, y_zero = calc_way.calc_zeros_xto0()
+# x_zero = np.array(x_zero)
+# y_zero = np.array(y_zero)
+# print(x_zero,y_zero)
+# image = cv2.imread(img_path)  # 默认读取BGR格式
+# 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)
+# cv2.imshow("Image with Line", image)
+# cv2.waitKey(0)          # 按任意键关闭窗口
+# cv2.destroyAllWindows()
+
+model = model.Model()
+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"
+output_folder = 'C:\\Users\\Administrator\\Desktop\\BYD\\Visual measurement\\pic\\7800'
+
+
+def vs_measurement(txt_name):
+    os.makedirs(output_folder, exist_ok=True)
+    # 获取数据
+    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_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()
+
+    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()
+
+        # 定义点的坐标 (x, y)
+        point = (int(x_jiao), 960-int(y_jiao))
+
+        # 画一个红色圆点（半径5，颜色BGR格式，线宽-1表示填充）
+        cv2.circle(image, point, 5, (0, 0, 255), -1)
+
+        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)
+
+        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)