l5kit.kinematic.ackerman_steering_model module

l5kit.kinematic.ackerman_steering_model.fit_ackerman_model_approximate(gx: numpy.ndarray, gy: numpy.ndarray, gr: numpy.ndarray, gv: numpy.ndarray, wx: numpy.ndarray, wy: numpy.ndarray, wr: numpy.ndarray, wv: numpy.ndarray, wgx: numpy.ndarray, wgy: numpy.ndarray, wgr: numpy.ndarray, wgv: numpy.ndarray) → Tuple[numpy.ndarray, numpy.ndarray, numpy.ndarray, numpy.ndarray]

Fits feasible ackerman-steering trajectory to groundtruth control points. Groundtruth is represented as 4 input numpy arrays (gx, gy, gr, gv) each of size (N,) representing position, rotation and velocity at time i. Returns 4 arrays (x, y, r, v) each of shape (N,) - the optimal trajectory. The solution is found as minimization of the following non-linear least squares problem: minimize F(x, y, r, v) = F_ground_cost(x, y, r, v) + F_kinematics_cost(x, y, r, v) where F_ground_cost(x, y, r, v) = 0.5 * sum( (wgx[i] * (x[i] - gx[i])) ** 2 + (wgy[i] * (y[i] - gy[i])) ** 2 + (wgr[i] * (r[i] - gr[i])) ** 2 + (wgv[i] * (v[i] - gv[i])) ** 2, i = 0 … N-1) and F_kinematics_cost(x, y, r, v) = 0.5 * sum( (wx * (x[i] + cos(r[i]) * v[i] - x[i+1])) ** 2 + (wy * (y[i] + sin(r[i]) * v[i] - y[i+1])) ** 2 + (wr * (r[i] - r[i+1])) ** 2 + (wv * (v[i] - v[i+1])) ** 2, i = 0 … N-2) Weights wg* control adherance to the control points while weights w* control obeying of underlying kinematic motion constrains.

Returns

4 arrays (x, y, r, v) each of shape (N,), the optimal trajectory.

l5kit.kinematic.ackerman_steering_model.fit_ackerman_model_exact(x0: numpy.ndarray, y0: numpy.ndarray, r0: numpy.ndarray, v0: numpy.ndarray, gx: numpy.ndarray, gy: numpy.ndarray, gr: numpy.ndarray, gv: numpy.ndarray, wgx: numpy.ndarray, wgy: numpy.ndarray, wgr: numpy.ndarray, wgv: numpy.ndarray, ws: float = 5.0, wa: float = 5.0, min_acc: float = - 0.3, max_acc: float = 0.3, min_steer: float = - 0.07853981633974483, max_steer: float = 0.07853981633974483) → Tuple[numpy.ndarray, numpy.ndarray, numpy.ndarray, numpy.ndarray, numpy.ndarray, numpy.ndarray]

Fits feasible ackerman-steering trajectory to groundtruth control points. Groundtruth is represented as 4 numpy arrays (gx, gy, gr, gv) each of shape (N,) representing position, rotation and velocity at time i. Returns 4 arrays (x, y, r, v) each of shape (N,) - the optimal trajectory.

The solution is found as minimisation of the following non-linear least squares problem: :: minimize F(steer, acc) = 0.5 * sum( (wgx[i] * (x[i] - gx[i])) ** 2 + (wgy[i] * (y[i] - gy[i])) ** 2 + (wgr[i] * (r[i] - gr[i])) ** 2 + (wgv[i] * (v[i] - gv[i])) ** 2 + (ws * steer[i]) ** 2 + (wa * acc[i]) ** 2) i = 1 … N) subject to following unicycle motion model equations: x[i+1] = x[i] + cos(r[i]) * v[i] y[i+1] = y[i] + sin(r[i]) * v[i] r[i+1] = r[i] + steer[i] v[i+1] = v[i] + acc[i] min_steer < steer[i] < max_steer min_acc < acc[i] < max_acc for i = 0 .. N Weights wg* control adherence to the control points In a typical usecase wgx = wgy = 1 and wgr = wgv = 0

Returns

4 arrays (x, y, r, v) each of shape (N,)- the optimal trajectory.