곡률반경 공식

픽셀의 2D 좌표

# ARC_M_Y: 호의 중점 D의 y좌표값
L_H = math.sqrt((L_FIT_X[L_ARC_M_Y]*x_mpp - (L_FIT_X[0]*x_mpp + L_FIT_X[-1]*x_mpp)/2)**2 + (L_ARC_M_Y*y_mpp - 360*y_mpp)**2)
R_H = math.sqrt((R_FIT_X[R_ARC_M_Y]*x_mpp - (R_FIT_X[0]*x_mpp + R_FIT_X[-1]*x_mpp)/2)**2 + (R_ARC_M_Y*y_mpp - 360*y_mpp)**2)
L_W = math.sqrt((L_FIT_X[0]*x_mpp - L_FIT_X[-1]*x_mpp)**2 + (720*y_mpp)**2)
R_W = math.sqrt((R_FIT_X[0]*x_mpp - R_FIT_X[-1]*x_mpp)**2 + (720*y_mpp)**2)
    
L_RAD = ((L_H/2) + (L_W**2 / (8*L_H)))
R_RAD = ((R_H/2) + (R_W**2 / (8*R_H)))

D와 M을 이용해 H를 구하고, 현재 pixel인 단위를 meter로 변환한 뒤 곡률 반경 공식에 대입하여 Radius를 구한다.

H : 선분 DM의 길이

W : 선분 AB의 길

L_RAD : 왼쪽 곡률반경 R_RAD : 오른쪽 곡률반경

L_FIT_X : 검출한 왼쪽 차선의 x좌표들을 담아둔 배열 (y좌표가 낮은 순서대로 담겨있다)

x_mpp : x축 방향의 단위변환을 위해 임의로 곱한 값(mpp는 meter per pixel)

L_FIT_X[L_ARC_M_Y] : D의 x 좌표 **(L_FIT_X[0]x_mpp + L_FIT_X[-1]x_mpp)/2) : M의 x 좌표 L_ARC_M_Y : D의 y 좌표 360 : M의 y 좌표

L_H = [(D의 x 좌표 - M의 x 좌표)^2 + (D의 y좌표 - M의 y좌표)^2] 의 제곱근 (왼쪽) R_H = [(D의 x 좌표 - M의 x 좌표)^2 + (D의 y좌표 - M의 y좌표)^2] 의 제곱근 (오른쪽)