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_9600_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 # 获取数据 # 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_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() # # 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) 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 Z2 = Zw_pos D2 = distance_pos print(f"Zw_pos = {Zw_pos}, D2 = {D2}") return Zw_intersection, distance, Zw_pos , distance_pos #