byd/Visual measurement-cameramodel/py/calc_slope_line.py

import math
import cameramodel
import calc_way
from scipy import stats


def get_k(
        cameraModel,
        x: float,
        y: float,
        calc_distance_func  # 传入计算距离的函数
) -> float:
    """计算斜率参数 k。

    参数:
        cameraModel: 包含相机参数的类实例，需有以下属性:
            rotation_alpha, rotation_beta: 旋转角度
        x, y: 像素坐标
        calc_distance_func: 计算距离的函数，需返回 (Xw, Yw)

    返回:
        斜率参数 k
    """
    # 1. 预计算常用三角函数值
    tan_alpha = math.tan(cameraModel.rotation_alpha)
    tan_beta = math.tan(cameraModel.rotation_beta)

    # 2. 计算 gamma 和 seta 角度
    gamma = math.atan2(1, tan_alpha * tan_beta)  # 使用 atan2 提高稳定性
    seta = math.atan2(1, math.sqrt(tan_beta ** 2 + 1 / tan_alpha ** 2))

    # 3. 计算世界坐标
    Xw, Yw = calc_distance_func(cameraModel, x, y)

    # 4. 坐标旋转
    cos_gamma = math.cos(gamma)
    sin_gamma = math.sin(gamma)
    u = Xw * cos_gamma - Yw * sin_gamma
    v = Xw * sin_gamma + Yw * cos_gamma

    # 5. 处理u=0的边界情况
    if math.isclose(u, 0, abs_tol=1e-10):
        return float('inf')  # 垂直线斜率为无穷大

    # 6. 计算 lambda 角度
    tan_seta_complement = math.tan(math.pi / 2 - seta)
    denominator = u * math.sqrt(1 + tan_seta_complement ** 2)
    tan_lambda = -v / denominator
    lambda_angle = math.atan(tan_lambda)

    # 7. 计算斜率 k
    k = math.tan(math.pi / 2 - lambda_angle)

    return k


def get_k2(
        cameraModel,
        Xw: float,
        Yw: float
) -> float:
    """计算从世界坐标(Xw,Yw)到斜率k的转换。

    参数:
        cameraModel: 包含相机参数的类实例，需有以下属性:
            rotation_alpha, rotation_beta: 旋转角度
        Xw, Yw: 世界坐标系下的坐标

    返回:
        斜率参数k
    """
    # 1. 预计算常用三角函数值
    tan_alpha = math.tan(cameraModel.rotation_alpha)
    tan_beta = math.tan(cameraModel.rotation_beta)

    # 2. 计算gamma和seta角度（使用更稳定的atan2）
    gamma = math.atan2(1, tan_alpha * tan_beta)  # 避免除零错误
    seta = math.atan2(1, math.sqrt(tan_beta ** 2 + 1 / tan_alpha ** 2))

    # 3. 坐标旋转（预计算sin/cos）
    cos_gamma = math.cos(gamma)
    sin_gamma = math.sin(gamma)
    u = Xw * cos_gamma - Yw * sin_gamma
    v = Xw * sin_gamma + Yw * cos_gamma

    # 4. 处理u=0的边界情况
    if math.isclose(u, 0, abs_tol=1e-10):
        return float('inf')  # 垂直线斜率为无穷大

    # 5. 计算lambda角度（更稳定的表达式）
    tan_seta_complement = math.tan(math.pi / 2 - seta)
    denominator = u * math.sqrt(1 + tan_seta_complement ** 2)
    tan_lambda = -v / denominator
    lambda_angle = math.atan(tan_lambda)

    # 6. 计算最终斜率k
    k = math.tan(math.pi / 2 - lambda_angle)

    return k


"""
计算地面点的竖直垂线在图像中的截距
参数：点在图像中的坐标x、y，斜率k
返回值：截距b
"""
def get_b(x,y,k):
    b = y - x * k
    return b


"""
将检测到的路沿上侧数据点拟合为一条直线
参数：上侧数据点坐标x_top,y_top
返回值：拟合出直线的斜率slope、截距intercept、r方值
"""
def linear_regression(x, y):
    slope, intercept, r_value, p_value, std_err = stats.linregress(x, y)
    return slope, intercept, abs(r_value)

"""
计算图像中两条直线的交点
参数：   line1: 第一条直线的系数 (A1, B1, C1)
        line2: 第二条直线的系数 (A2, B2, C2)
返回值：交点在图像中的二维坐标x、y
"""
def find_intersection(line1, line2):
    A1, B1, C1 = line1
    A2, B2, C2 = line2

    # 计算行列式
    denominator = A1 * B2 - A2 * B1

    if denominator == 0:
        # 直线平行或重合
        return None
    else:
        x = (B1 * C2 - B2 * C1) / denominator
        y = (A2 * C1 - A1 * C2) / denominator
        return (x, y)