opendrive中三次参数方程的表达

1.基于四个点的基础做法

（1）先将xy坐标系转换到uv(和st坐标系一致），其中aU，aV为0；
（2）以4个点为一组，第一个点为s=0，计算后三个点到第一个点的距离，该距离为当前参数三次方程参数p的值[p1,p2,p3]；
（3）使用这三个点的p值以及对应的uv，放到公示中计算参数aU, bU, cU, dU, aV, bV, cV, dV：

u(p) = aU + bU*p + cU*p2 + dU*p³
v(p) = aV + bV*p + cV*p2 + dV*p³

[aU, bU, cU, dU]*[p1,p1**2,p1**3]

（4）每组数据的hdg为第一个点到第二个点的向量与x轴夹角（弧度）；

## 获取uv坐标系下的坐标值（局部坐标系与s/t坐标系保持一致）
def covt_uv(self, point, x0, y0, hdg):
    x = point[0]
    y = point[1]
    hdg_sin = math.sin(hdg)
    hdg_cos = math.cos(hdg)
    u = (y - y0) * hdg_sin + (x - x0) * hdg_cos
    v = (x0 - x) * hdg_sin + (y - y0) * hdg_cos
    return u, v

## vector2 = [1,0]
# 获取startpoint到endpoint的向量与x轴夹角（弧度）
# range:[-pi,pi]
def get_hdg_math(self, startpoint, endpoint):
    point_length = get_dist(startpoint, endpoint)
    dot = endpoint[0] - startpoint[0]
    alph = np.arccos(dot / point_length)
    if endpoint[1] - startpoint[1] > 0:
        return alph
    else:
        return -alph

## 获取uv坐标系三次参数方程参数值,三个点拟合一个三次参数方程：aU、bU和aV为0，确保参考线平滑
def get_param_dict(self, points, hdg, ps):
    """Get aU (0), bU, cU, dU, aV (0), bV, cV, dV
    Args:
        points (list[point]): 4 points' Length must be 4
        hdg (float): hdg of the (points[0]->points[1])
        ps (list[float]): distance between each point in points and points[0]
    Returns:
        tuple: us, vs
    """
    start = points.pop(0)
    x0 = start[0]
    y0 = start[1]
    u0, v0 = self.covt_uv(points[0], x0, y0, hdg)
    u1, v1 = self.covt_uv(points[1], x0, y0, hdg)
    u2, v2 = self.covt_uv(points[2], x0, y0, hdg)
    a = np.array(
        [
            [ps[0], ps[0] ** 2, ps[0] ** 3],
            [ps[1], ps[1] ** 2, ps[1] ** 3],
            [ps[2], ps[2] ** 2, ps[2] ** 3],
        ]
    )
    b = np.array([u0, u1, u2])
    us = linalg.solve(a, b)
    us = np.insert(us, 0, 0, axis=0)
    b = np.array([v0, v1, v2])
    vs = linalg.solve(a, b)
    vs = np.insert(vs, 0, 0, axis=0)
    return us, vs

2.优化道路参考线几何三次参数表达

方法：

（1）计算该参考线下每个点基于起点的距离（累加距离和）

plist=[p1,p2,p3,...,pn]

（2）生成矩阵pp

（3）生成对应u(p),v(p)列表

u(p)=[u(p1),u(p2),u(p3),...,u(pn)]

v(p)=[v(p1),v(p2),v(p3),...,v(pn)]

（4）计算p关于u,v的三次参数方程

def get_uv_param(self, ulist, vlist, plist):
    """
    功能：计算三次参数曲线的方程参数
    参数：
    ulist:[u(p1),u(p2),...]
    vlist:[v(p1),v(p2),...]
    plist:[p1,p2,...]
    三个列表长度需一致
    返回：
    u:[au,bu,cu,du]
    v:[av,bv,cv,dv]
    """
    if len(ulist) == len(vlist) == len(plist):
        pp = np.array(plist)
        matrix = np.vstack((pp**0, pp**1, pp**2, pp**3))
        p_inv = np.linalg.pinv(matrix)
        u = np.matmul(np.array(ulist), p_inv)
        v = np.matmul(np.array(vlist), p_inv)
        return u, v