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.
46 lines
1.5 KiB
46 lines
1.5 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
|
|
import time
|
|
|
|
model = model.Model()
|
|
alpha = model.alpha
|
|
beta = model.beta
|
|
|
|
|
|
def vs_measurement(txt_name, position=900):
|
|
# 加载数据
|
|
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 + model.x
|
|
# 确认目标点位置并计算高度
|
|
Yw_pos = slope_Xw * position + intercept_Xw
|
|
x_pos, y_pos = calc_way.calc_distance2(position, Yw_pos, 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_pos = -(Yw_pos + model.y) * math.cos(angle)
|
|
return 1, Zw_pos, distance_pos
|
|
|