OpenDRIVE地圖中Geometry線段對應x/y座標的計算

OpenDrive地圖解析代碼能夠參考，https://github.com/liuyf5231/opendriveparser

---

在OpenDRIVE地圖的解析和繪製過程當中，最關鍵的一點：在x/y座標系下，利用起點(x,y)、曲線的相對長度(s座標，該點距離起點的線段長度)計算出相應長度處所對應的x/y座標。目前使用較多的Geometry類型爲直線line、arc、spiral。

1.直線line的解析最簡單，知道起點(x,y)，直線角度hdg，距離起點的線段長度s，既能夠計算出相對起點的dx,dy。

dx = s * cos(hdg)
dy = s * sin(hdg)

2.對於arc，有一個恆定的參數曲率curvature，dx，dy計算方式以下，

c = curvature
 hdg = heading - PI / 2

 a = 2 / c * sin(s * c / 2)
 alpha = (PI - s * c) / 2 - hdg

 dx = -1 * a * cos(alpha)
 dy = a * sin(alpha)

3.spiral最複雜，不過OpenDRIVE官網給出了一個計算的例子(Sample code for the calculation of spirals)，關鍵代碼以下(c語言)，能夠按照原理利用具體的編程語言實現，

/**
* compute the actual "standard" spiral, starting with curvature 0
* @param s      run-length along spiral
* @param cDot   first derivative of curvature [1/m2]
* @param x      resulting x-coordinate in spirals local co-ordinate system [m]
* @param y      resulting y-coordinate in spirals local co-ordinate system [m]
* @param t      tangent direction at s [rad]
*/

void odrSpiral( double s, double cDot, double *x, double *y, double *t )
{
    double a;

    a = 1.0 / sqrt( fabs( cDot ) );
    a *= sqrt( M_PI );
    
    fresnel( s / a, y, x );
    
    *x *= a;
    *y *= a;
    
    if ( cDot < 0.0 )
        *y *= -1.0;

    *t = s * s * cDot * 0.5;
}

將地圖中的全部線段都解析爲x/y座標後，咱們很容易利用一些畫圖工具繪製出相應的地圖，下圖爲利用python matplotlib繪製的地圖，地圖數據基於OpenDRIVE官網的Crossing8Course.xodr。只繪製了lane type爲driving的車道線。

下圖爲官方工具OpenDRIVE Viewer繪製的地圖(linux下運行，不開源)。