首先感谢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