byd/calc_way.py

import math
import model
from sympy import symbols, Eq, solve, sqrt, And , tan , cos, sin ,atan, sinh, cosh
from sympy.solvers.inequalities import reduce_inequalities
import numpy as np

model = model.Model()
u0 = model.u0
v0 = model.v0
f = model.f
H = model.H
dx = model.dx
dy = model.dy
alpha = model.alpha
beta = model.beta
"""
计算图像中一点到车辆坐标的距离
参数：点在图像中的坐标x、y，相机的外部偏转角参数alpha、beta
返回值：点在车辆坐标系下的二维坐标
"""
def calc_distance(x, y, alpha, beta):
    Xp = (x - u0) * dx
    Yp = -(y - v0) * dy
    sq = math.sqrt(f * f + Yp * Yp)
    eta = math.atan(Yp / f)
    gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta)))
    seta = math.atan(1/math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2)))
    MP = H * math.fabs(Xp) / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))
    OM = H / math.tan(seta + eta)
    epsilon = math.atan((H * Xp / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))) / OM)
    d = math.sqrt(MP ** 2 + OM ** 2)
    Xw = d * math.cos(gamma + epsilon)
    Yw = -d * math.sin(gamma + epsilon)
    return Xw, Yw

"""
计算图像中一点距离地面的高度
参数：地面端点在图像中的坐标x_bot、y_bot，垂线上端点在图像中的坐标x_top、y_top，相机的外部偏转角参数alpha、beta
返回值：点距离地面的高度
"""
def calc_height(x_bot,y_bot,x_top,y_top,alpha,beta):
    Xp = (x_bot - u0) * dx
    Yp = -(y_bot - v0) * dy
    sq = math.sqrt(f * f + Yp * Yp)
    eta = math.atan(Yp / f)
    gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta)))
    seta = math.atan(1/math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2)))
    MP = H * math.fabs(Xp) / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))
    OM = H/math.tan(seta + eta)
    Yp1 = -(y_top - v0) * dy
    sq1 = math.sqrt(f * f + Yp1 * Yp1)
    eta1 = math.atan(Yp1 / f)
    Zw = H - OM*math.tan(seta + eta1)
    return Zw

"""
计算图像中车辆坐标系Xw、Yw所对应的轴线
参数：
返回值：轴线上点在图像中的二维坐标x、y
"""
def calc_zeros():
    # 定义搜索范围和步长
    x_range = np.linspace(0, 1280, 1000)
    y_range = np.linspace(0, 960, 1000)

    # 存储解
    solutions = []
    tolerance = 1e-3  # 容忍误差

    # 网格搜索
    for x in x_range:
        for y in y_range:
            error = equations(x, y)
            if error < tolerance:
                solutions.append((x, y))

    # 去除近似重复的解
    unique_solutions = []
    for sol in solutions:
        # 检查是否与已找到的解近似
        is_unique = True
        for usol in unique_solutions:
            if np.allclose(sol, usol, atol=1):
                is_unique = False
                break
        if is_unique:
            unique_solutions.append(sol)
    x = []
    y = []
    # print("找到的解:")
    for sol in unique_solutions:
        # print(f"x = {sol[0]:.4f}, y = {sol[1]:.4f}")
        x.append(sol[0])
        y.append(sol[1])
    return x, y

# 定义方程
def equations(x, y):
    Xp = (x - u0) * dx
    Yp = -(y - v0) * dy
    sq = math.sqrt(f * f + Yp * Yp)
    eta = math.atan(Yp / f)
    gamma = math.atan(1 / (math.tan(alpha) * math.tan(beta)))
    seta = math.atan(1/math.sqrt(pow(math.tan(beta), 2) + 1 / pow(math.tan(alpha), 2)))
    MP = H * math.fabs(Xp) / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))
    OM = H / math.tan(seta + eta)
    epsilon = math.atan((H * Xp / (math.sqrt(f ** 2 + Yp ** 2) * math.sin(seta + eta))) / OM)
    d = math.sqrt(MP ** 2 + OM ** 2)
    Xw = d * math.cos(gamma + epsilon)
    Yw = -d * math.sin(gamma + epsilon)
    return Yw**2   # 误差平方和