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.polyval(S, coeffsV)

    xrot = u * np.cos(self._heading) - v * np.sin(self._heading)
    yrot = u * np.sin(self._heading) + v * np.cos(self._heading)

    # Tangent is defined by derivation
    dCoeffsU = coeffsU[1:] * np.array(np.arange(1, len(coeffsU)))
    dCoeffsV = coeffsV[1:] * np.array(np.arange(1, len(coeffsV)))

    dx = np.polynomial.polynomial.polyval(S, dCoeffsU)
    dy = np.polynomial.polynomial.polyval(S, dCoeffsV)

    tangent = np.arctan2(dy, dx)


    return (self._startPosition + np.array([xrot, yrot]), self._heading + tangent)

三、边界点计算

def calc(self, sPos, addOffset=0.0):
    """ Calculate the border  """

    if isinstance(self._reference, PlanView):
        # Last reference has to be a reference geometry
        refPos, refTang = self._reference.calc(self._refOffset + sPos)

    elif isinstance(self._reference, Border):
        # Offset of all inner lanes
        refPos, refTang = self._reference.calc(self._refOffset + sPos)

    else:
        raise Exception("Reference must be plan view or other lane border.")

    if not self._coeffs or not self._coeffsOffsets:
        raise Exception("No entries for width definitions.")

    # Find correct coefficients
    widthIdx = next((self._coeffsOffsets.index(n) for n in self._coeffsOffsets[::-1] if n <= sPos), len(self._coeffsOffsets))

    # Calculate width at sPos
    distance = np.polynomial.polynomial.polyval(sPos - self._coeffsOffsets[widthIdx], self._coeffs[widthIdx]) + addOffset

    # New point is in orthogonal direction
    ortho = refTang + np.pi / 2
    newPos = refPos + np.array([distance * np.cos(ortho), distance * np.sin(ortho)])

    return newPos, refTang