|
|
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)
|
|
|
|
|
|
|
