|
|
|
|
|
import math
|
|
|
|
|
|
import model
|
|
|
|
|
|
import calc_way
|
|
|
|
|
|
from scipy import stats
|
|
|
|
|
|
|
|
|
|
|
|
model = model.Model()
|
|
|
|
|
|
alpha = model.alpha
|
|
|
|
|
|
beta = model.beta
|
|
|
|
|
|
q = model.q
|
|
|
|
|
|
"""
|
|
|
|
|
|
计算地面点的竖直垂线在图像中的斜率
|
|
|
|
|
|
参数：点在图像中的坐标x、y，相机的外部偏转角参数alpha、beta
|
|
|
|
|
|
返回值：斜率k
|
|
|
|
|
|
"""
|
|
|
|
|
|
def get_k(alpha,beta,x,y):
|
|
|
|
|
|
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)))
|
|
|
|
|
|
Xw,Yw = calc_way.calc_distance(x,y,alpha,beta)
|
|
|
|
|
|
u = Xw * math.cos(gamma) - Yw * math.sin(gamma)
|
|
|
|
|
|
v = Xw * math.sin(gamma) + Yw * math.cos(gamma)
|
|
|
|
|
|
tanlamda = -v/u/math.sqrt(1+(math.tan(math.pi/2-seta))**2)
|
|
|
|
|
|
lamda = math.atan(tanlamda)
|
|
|
|
|
|
k = math.tan(math.pi/2-lamda)
|
|
|
|
|
|
return k
|
|
|
|
|
|
|
|
|
|
|
|
def get_k2(alpha,beta,Xw,Yw):
|
|
|
|
|
|
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)))
|
|
|
|
|
|
u = Xw * math.cos(gamma) - Yw * math.sin(gamma)
|
|
|
|
|
|
v = Xw * math.sin(gamma) + Yw * math.cos(gamma)
|
|
|
|
|
|
tanlamda = -v / u / math.sqrt(1 + (math.tan(math.pi / 2 - seta)) ** 2)
|
|
|
|
|
|
lamda = math.atan(tanlamda)
|
|
|
|
|
|
k = math.tan(math.pi / 2 - lamda)
|
|
|
|
|
|
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)
|
