|
|
|
|
import math
|
|
|
|
|
import numpy as np
|
|
|
|
|
import matplotlib.pyplot as plt
|
|
|
|
|
import calc_way
|
|
|
|
|
import get_data
|
|
|
|
|
import calc_slope_line
|
|
|
|
|
import cv2
|
|
|
|
|
import os
|
|
|
|
|
import time
|
|
|
|
|
import glob
|
|
|
|
|
import cameramodel
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def vs_measurement(txt_name, position=784, config_path: str = None):
|
|
|
|
|
# 初始化相机模型
|
|
|
|
|
cameraModel = cameramodel.CameraModel(config_path)
|
|
|
|
|
|
|
|
|
|
# 加载数据
|
|
|
|
|
state, x_bot, y_bot, slope_bot, intercept_bot, x_top, y_top, slope_top, intercept_top = get_data.get_data(cameraModel, txt_name)
|
|
|
|
|
if state == 0:
|
|
|
|
|
return 0, None, None, None, None
|
|
|
|
|
|
|
|
|
|
# 计算底部路沿
|
|
|
|
|
Xw_bot = []
|
|
|
|
|
Yw_bot = []
|
|
|
|
|
for i in range(len(x_bot)):
|
|
|
|
|
Xw, Yw = calc_way.calc_distance(cameraModel, x_bot[i], y_bot[i])
|
|
|
|
|
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)
|
|
|
|
|
|
|
|
|
|
# 确认目标点位置
|
|
|
|
|
Yw_pos = slope_Xw * position + intercept_Xw
|
|
|
|
|
x_pos, y_pos = calc_way.calc_distance2(cameraModel, position, Yw_pos)
|
|
|
|
|
k_pos = calc_slope_line.get_k(cameraModel, x_pos, y_pos, calc_way.calc_distance)
|
|
|
|
|
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(cameraModel, x_pos, y_pos, x_pos_top, y_pos_top)
|
|
|
|
|
|
|
|
|
|
# 位置修正
|
|
|
|
|
distance_pos = (-Yw_pos + cameraModel.position_offset_y) * math.cos(angle)
|
|
|
|
|
return 1, Zw_pos, distance_pos, int(x_pos), cameraModel.camera_height - int(y_pos)
|
|
|
|
|
|
|
|
|
|
# folder_path = r"C:\Users\Administrator\Desktop\BYD\0722"
|
|
|
|
|
# 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, config_path="updated_config.json")
|
|
|
|
|
# print(result)
|
|
|
|
|
|
|
|
|
|
# result = vs_measurement(r"C:\Users\Administrator\Desktop\BYD\0722\new13.19351136278192.txt", position=784, config_path="updated_config.json")
|
|
|
|
|
# # print(result)
|
