|
|
|
|
|
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)
|
