Bootstrap

opendrive地图数据解析算法

首先感谢github大佬@Stefan Urban

首先geometry类型paramPoly3,相对简单,在计算车道边界点的时候比较复杂,计算当前点角度时要先求导计算dx,dy,利用arctan求出角度。附odr数据解析python核心代码,

一、uv坐标系转xy坐标系:

double u = aU + bU * p + cU * p * p + dU * p * p* p;
double v = aV + bV * p + cV * p * p + dV * p * p* p;

double phi  = hdg;
double cos_phi = cos(phi);
double sin_phi = sin(phi);

double x = (cos_phi * u - sin_phi * v)   +  x;
double y = (sin_phi  * u + cos_phi * v)   +  y;

二、三次多项式计算点以及角度

def calcPosition(self, s):

    # Position
    S = (s / self._length) * self._pRange

    coeffsU = [self._aU, self._bU, self._cU, self._dU]
    coeffsV = [self._aV, self._bV, self._cV, self._dV]

    #(s) ->(u,v)
    u = np.polynomial.polynomial.polyval(S, coeffsU)
    v = np.polynomial.polynomial.polyv
;