byd/py/test.py

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)