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 import time 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_6000_2.jpg_zuobiao.txt" output_folder = 'C:\\Users\\Administrator\\Desktop\\BYD\\Visual measurement\\pic\\7800' """ 计算图像中识别到的路沿距离车辆坐标原点的距离、高度信息 参数：保存数据的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_yto0() # 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"路沿平均高度为：{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") # 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 def vs_measurement(txt_name,position): if not os.path.exists(txt_name): return None,None,None,None # 获取数据 try: 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) except Exception as e: print(f"处理过程中发生错误: {e}") return 1,1,1,1 # 拟合路沿上下直线方程 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) 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 return Zw_intersection, distance, Zw_pos , distance_pos if __name__ == '__main__': # t=time.time() # h, w = vs_measurement(r'C:\Users\Administrator\Desktop\BYD\Visual measurement\py\new.txt') # print(h,w) # print(f"time: {time.time() - t}") # x_zero, y_zero = calc_way.calc_zeros() # x_zero = np.array(x_zero) # y_zero = np.array(y_zero) # 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) # vs_measurement(txt_name) print(f"time: {time.time() - t}") print(Zw_intersection, distance, Zw_pos, distance_pos)