|
|
|
|
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
|
|
|
|
|
q=model.q
|
|
|
|
|
|
|
|
|
|
def vs_measurement(txt_name, position = 784):
|
|
|
|
|
# 加载数据
|
|
|
|
|
state, x_bot, y_bot, x_top, y_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
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
result = vs_measurement(r"C:\Users\Administrator\Desktop\BYD\7.16\0716\new175.8595023857456.txt", position=900)
|
|
|
|
|
print(result)
|
