You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
225 lines
9.3 KiB
225 lines
9.3 KiB
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
|
|
|
|
|
|
#
|