|
|
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
|
|
|
import glob
|
|
|
import JS
|
|
|
model = model.Model()
|
|
|
alpha = model.alpha
|
|
|
beta = model.beta
|
|
|
q=model.q
|
|
|
|
|
|
def vs_measurement(txt_name, position = 784, config_path: str = None):
|
|
|
cameraModel = JS.CameraModel(config_path)
|
|
|
# 加载数据
|
|
|
state, x_bot, y_bot, slope_bot, intercept_bot, x_top, y_top, slope_top, intercept_top = get_data.get_data(txt_name)
|
|
|
if state == 0:
|
|
|
return 0, None, None
|
|
|
# # 拟合上下沿
|
|
|
# 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)
|
|
|
|
|
|
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)
|
|
|
# 位置修正
|
|
|
position = position
|
|
|
# 确认目标点位置并计算高度
|
|
|
Yw_pos = slope_Xw * position + intercept_Xw
|
|
|
x_pos, y_pos = calc_way.calc_distance2(position, Yw_pos, alpha, beta)
|
|
|
# print(x_pos, y_pos)
|
|
|
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)
|
|
|
# print(k_pos, b_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_pos = -(Yw_pos - 53) * math.cos(angle)
|
|
|
return 1, Zw_pos, distance_pos
|
|
|
|
|
|
|
|
|
def vs_measurement_pic(txt_name, img_path, output_folder):
|
|
|
# #画图
|
|
|
# fig, axes = plt.subplots(nrows=4, ncols=2, figsize=(10, 8))
|
|
|
|
|
|
state, x_bot, y_bot, slope_bot, intercept_bot, x_top, y_top, slope_top, intercept_top = get_data.get_data(txt_name)
|
|
|
if state == 0:
|
|
|
return 0, None, None
|
|
|
|
|
|
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)
|
|
|
# 3. 读取图像
|
|
|
image = cv2.imread(img_path)
|
|
|
|
|
|
# 检查图像是否成功加载
|
|
|
if image is None:
|
|
|
print(f"错误: 无法读取图像 {img_path}")
|
|
|
exit()
|
|
|
# 计算直线上的两个点（x=0 和 x=图像宽度）
|
|
|
# x1 = 0
|
|
|
# y1 = 960 - int(slope_top * x1 + intercept_top) # 计算 y1
|
|
|
#
|
|
|
# x2 = int(x_top[-1]) # 图像宽度
|
|
|
# y2 = 960 - int(slope_top * x2 + intercept_top) # 计算 y2
|
|
|
#
|
|
|
# # 绘制直线（BGR 颜色：(0, 255, 0) 绿色，线宽 2）
|
|
|
# cv2.line(image, (x1, y1), (x2, y2), (0, 255, 0), 1)
|
|
|
#
|
|
|
# x1 = 0
|
|
|
# y1 = 960 - int(slope_bot * x1 + intercept_bot) # 计算 y1
|
|
|
#
|
|
|
# x2 = int(x_bot[-1]) # 图像宽度
|
|
|
# y2 = 960 - int(slope_bot * x2 + intercept_bot) # 计算 y2
|
|
|
#
|
|
|
# # 绘制直线（BGR 颜色：(0, 255, 0) 绿色，线宽 2）
|
|
|
# cv2.line(image, (x1, y1), (x2, y2), (0, 255, 0), 1)
|
|
|
|
|
|
cv2.line(image, (int(x_bot[i]), 960 - int(y_bot[i])), (int(x), 960 - int(y)), (0, 0, 255), 2)
|
|
|
cv2.putText(image, f"Xw = {int(Xw)},Yw = {int(Yw)},Zw = {int(Zw)}", (640, 200), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2)
|
|
|
cv2.putText(image, f"x = {x_bot[i]}, y = {y_bot[i]}", (int(x_bot[i]), 960 - int(y_bot[i])),
|
|
|
cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2)
|
|
|
filename = f"{i}.jpg"
|
|
|
output_path = os.path.join(output_folder, filename)
|
|
|
cv2.imwrite(output_path, image)
|
|
|
|
|
|
|
|
|
# t = time.time()
|
|
|
# result = vs_measurement(r"C:\Users\Administrator\Desktop\BYD\0718\new428.75662880363143.txt", position=500)
|
|
|
# print(result)
|
|
|
# vs_measurement_pic(r"C:\Users\Administrator\Desktop\BYD\0718\new428.75662880363143.txt",r"C:\Users\Administrator\Desktop\BYD\0718\new428.75662880363143.jpg",r"C:\Users\Administrator\Desktop\BYD\0718\new428.75662880363143")
|
|
|
# print(f"time: {time.time() - t}
|
|
|
folder_path = r"C:\Users\Administrator\Desktop\BYD\7.16\0716"
|
|
|
txt_files = glob.glob(os.path.join(folder_path, '*.txt'))
|
|
|
for file_path in txt_files:
|
|
|
with open(file_path, 'r', encoding='utf-8') as file:
|
|
|
print(file_path)
|
|
|
result = vs_measurement(file_path, position=784)
|
|
|
print(result) |
