import numpy as np
import math
from scipy.sparse import linalg, lil_matrix
import tqdm
'''
used to interpolate and smooth the roadway
'''
def _fit_open(pts):
'''
fitting open curve
:param pts: points to be fitted
:return: fitted points
'''
d, n = pts.shape
n -= 1
Y = np.array([None for _ in range(n+1)], dtype='float')
M = np.zeros((n+1, n+1))
for i in range(n):
M[i, i] = 4.0
M[i, i + 1] = 1.0
M[i + 1, i] = 1.0
M[n, n] = 2.0
M[0, 0] = 2.0
M = lil_matrix(M)
retval = [None for _ in range(d)]
for k in range(d):
for i in range(n+1):
ind_hi = min(i + 1, n - 1)
ind_lo = max(0, i - 1)
Y[i] = 3 * (pts[k, ind_hi] - pts[k, ind_lo])
D = linalg.lsqr(M, Y)[0]
spline_coeffs = np.zeros((4, n))
spline_coeffs[0, :] = pts[k, :n] # x₀
spline_coeffs[1, :] = D[:n] # x'₀
spline_coeffs[2, :] = 3 * (pts[k, 1:n + 1] - pts[k, :n]) - 2 * D[:n] - D[1:n + 1] # -3x₀ + 3x₁ - 2x'₀ - x'₁
spline_coeffs[3, :] = 2 * (pts[k, :n] - pts[k, 1:n + 1]) + D[:n] + D[1:n + 1] # 2x₀ - 2x₁ + x'₀ + x'₁
retval[k] = spline_coeffs
return retval
[docs]def fit_cubic_spline(pts, open: bool = True):
if open:
return _fit_open(pts)
[docs]def sample_spline_derivative(spline_coeffs, t: float):
# here t is generally expected to be t ∈ [0,1]
return spline_coeffs[1] + t * (2 * spline_coeffs[2] + t * 3 * spline_coeffs[3])
[docs]def sample_spline_derivative2(spline_coeffs, t: float):
# here t is generally expected to be t ∈ [0,1]
return 2 * spline_coeffs[3] + t * 6 * spline_coeffs[4]
[docs]def sample_spline_speed_1(spline_coeffs_x, spline_coeffs_y, t: float):
dxdt = sample_spline_derivative(spline_coeffs_x, t)
dydt = sample_spline_derivative(spline_coeffs_y, t)
return math.hypot(dxdt, dydt)
[docs]def sample_spline_speed_2(spline_coeffs_x, spline_coeffs_y, t: float):
n = spline_coeffs_x.shape[1]
assert spline_coeffs_x.shape[0] == 4
assert spline_coeffs_y.shape[0] == 4
assert n == spline_coeffs_y.shape[1]
col_ind = max(1, min(int(n), int(math.ceil(t))))
return sample_spline_speed_1(spline_coeffs_x[:, col_ind - 1], spline_coeffs_y[:, col_ind - 1], t - col_ind + 1)
[docs]def calc_curve_length_1(spline_coeffs_x, spline_coeffs_y, n_intervals: int = 100):
# integrate using Simpson's rule
# _integrate_simpsons(t->sample_spline_speed(spline_coeffs_x, spline_coeffs_y, t), 0.0, 1.0, n_intervals)
a = 0.0
b = 1.0
n = n_intervals
h = (b - a) / n
retval = sample_spline_speed_1(spline_coeffs_x, spline_coeffs_y, a) + sample_spline_speed_1(spline_coeffs_x,
spline_coeffs_y, b)
flip = True
for i in range(1, n):
retval += sample_spline_speed_1(spline_coeffs_x, spline_coeffs_y, a + i * h) * (4 if flip else 2)
flip = not flip
return h / 3 * retval
[docs]def calc_curve_length_2(spline_coeffs_x, spline_coeffs_y, n_intervals_per_segment: int = 100):
n = spline_coeffs_x.shape[1]
assert spline_coeffs_y.shape[1] == n
assert spline_coeffs_x.shape[0] == 4 and spline_coeffs_y.shape[0] == 4
len = 0.0
for i in range(n):
print("cal curve length: {}/{}".format(i, n))
len += calc_curve_length_1(spline_coeffs_x[:, i], spline_coeffs_y[:, i], n_intervals=n_intervals_per_segment)
return len
[docs]def calc_curve_param_given_arclen(spline_coeffs_x, spline_coeffs_y, s_arr, max_iterations: int = 50,
curve_length: float = None, epsilon: float = 1e-4, n_intervals_in_arclen: int = 100):
n_segments = spline_coeffs_x.shape[1]
assert spline_coeffs_x.shape[0] == 4 and spline_coeffs_y.shape[0] == 4
assert spline_coeffs_y.shape[1] == n_segments
n = len(s_arr)
t_arr = np.zeros((n,))
s = s_arr[0]
t = s / curve_length
if s <= 0.0:
t = 0.0
elif s >= curve_length:
return float(n_segments)
lo = 0.0
# print("L: ", curve_length)
# print("s_max: ", s_arr[-1])
for (i, s) in tqdm.tqdm(enumerate(s_arr)):
if s <= 0.0:
t = 0.0
t_arr[i] = lo = t
continue
elif s >= curve_length:
t = float(n_segments)
t_arr[i] = lo = t
continue
hi = float(n_segments)
for iter in range(max_iterations):
F = arclength_2(spline_coeffs_x, spline_coeffs_y, 0.0, t, n_intervals_in_arclen) - s
if abs(F) < epsilon:
break
DF = sample_spline_speed_2(spline_coeffs_x, spline_coeffs_y, t)
tCandidate = t - F / DF
if F > 0:
hi = t
t = 0.5 * (lo + hi) if tCandidate <= lo else tCandidate
else:
lo = t
t = 0.5 * (lo + hi) if tCandidate >= hi else tCandidate
t_arr[i] = lo = t
return t_arr
[docs]def arclength_1(spline_coeffs_x, spline_coeffs_y, t_min: float = 0.0, t_max: float = 1.0, n_intervals: int = 100):
if math.isclose(t_min, t_max):
return 0.0
a = t_min
b = t_max
n = n_intervals
h = (b - a) / n
retval = sample_spline_speed_1(spline_coeffs_x, spline_coeffs_y, a) + sample_spline_speed_1(spline_coeffs_x,
spline_coeffs_y, b)
flip = True
for i in range(1, n):
retval += sample_spline_speed_1(spline_coeffs_x, spline_coeffs_y, a + i * h) * (4 if flip else 2)
flip = not flip
return h / 3 * retval
[docs]def arclength_2(spline_coeffs_x, spline_coeffs_y, t_min: float = 0.0, t_max: float = None,
n_intervals_per_segment: int = 100):
if t_max is None:
t_max = spline_coeffs_x.shape[1]
n = spline_coeffs_x.shape[1]
assert spline_coeffs_y.shape[1] == n
assert spline_coeffs_x.shape[0] == 4 and spline_coeffs_y.shape[0] == 4
if math.isclose(t_min, t_max):
return 0.0
len = 0.0
for i in range(int(math.floor(t_min)), min(int(math.floor(t_max)), n-1) + 1):
t_lo, t_hi = float(i), i + 1.0
spline_ind = i + 1
t_in_min = max(t_lo, t_min) - t_lo
t_in_max = min(t_hi, t_max) - t_lo
len += arclength_1(spline_coeffs_x[:, spline_ind - 1], spline_coeffs_y[:, spline_ind - 1], t_in_min, t_in_max,
n_intervals_per_segment)
return len
[docs]def sample_spline_1(spline_coeffs, t: float):
# here t is generally expected to be t ∈ [0,1]
return spline_coeffs[0] + t * (spline_coeffs[1] + t * (spline_coeffs[2] + t * spline_coeffs[3]))
[docs]def sample_spline_2(spline_coeffs, t_arr):
assert spline_coeffs.shape[0] == 4
retval = np.zeros((len(t_arr,)))
for (i, t) in enumerate(t_arr):
col_ind = max(1, min(spline_coeffs.shape[1], int(math.ceil(t))))
retval[i] = sample_spline_1(spline_coeffs[:, col_ind - 1], t - col_ind + 1)
return retval
[docs]def sample_spline_theta_1(spline_coeffs_x, spline_coeffs_y, t: float, stepsize = 1e-4):
t_lo, t_hi = t, t + stepsize
if t_hi > 1.0:
t_lo, t_hi = t - min(1000 * stepsize, 0.1), t
x1 = sample_spline_1(spline_coeffs_x, t_lo)
x2 = sample_spline_1(spline_coeffs_x, t_hi)
y1 = sample_spline_1(spline_coeffs_y, t_lo)
y2 = sample_spline_1(spline_coeffs_y, t_hi)
# println("(t, lo, hi) $t $t_lo $t_hi, ($(atan(y2-y1, x2-x1)))")
return math.atan2(y2-y1, x2-x1)
[docs]def sample_spline_theta_2(spline_coeffs_x, spline_coeffs_y, t_arr):
n = spline_coeffs_x.shape[1]
assert spline_coeffs_y.shape[1] == n
assert spline_coeffs_x.shape[0] == 4 and spline_coeffs_y.shape[0] == 4
retval = np.zeros((len(t_arr),))
for (i, t) in enumerate(t_arr):
col_ind = max(1, min(n, int(math.ceil(t))))
retval[i] = sample_spline_theta_1(spline_coeffs_x[:, col_ind - 1], spline_coeffs_y[:, col_ind - 1],
t - col_ind + 1)
return retval
[docs]def sample_spline_curvature_1(spline_coeffs_x, spline_coeffs_y, t: float):
# computes the signed curvature
dx = sample_spline_derivative(spline_coeffs_x, t)
dy = sample_spline_derivative(spline_coeffs_y, t)
ddx = sample_spline_derivative2(spline_coeffs_x, t)
ddy = sample_spline_derivative2(spline_coeffs_y, t)
return (dx*ddy - dy*ddx)/(dx*dx + dy*dy)**1.5
[docs]def sample_spline_curvature_2(spline_coeffs_x, spline_coeffs_y, t_arr):
n = spline_coeffs_x.shape[1]
assert spline_coeffs_y.shape[1] == n
assert spline_coeffs_x.shape[0] == 4 and spline_coeffs_y.shape[0] == 4
retval = np.zeros((len(t_arr),))
for (i, t) in enumerate(t_arr):
col_ind = max(1, min(n, int(math.ceil(t))))
retval[i] = sample_spline_curvature_1(spline_coeffs_x[:, col_ind - 1], spline_coeffs_y[:, col_ind - 1],
t - col_ind + 1)
return retval
[docs]def sample_spline_derivative_of_curvature_1(spline_coeffs_x, spline_coeffs_y, t, stepsize = 1e-4):
# computes the derivative of the signed curvature
t_lo, t_hi = t, t + stepsize
if t_hi > 1.0:
t_lo, t_hi = t - stepsize, t
k_hi = sample_spline_curvature_1(spline_coeffs_x, spline_coeffs_y, t_hi)
k_lo = sample_spline_curvature_1(spline_coeffs_x, spline_coeffs_y, t_lo)
return (k_hi - k_lo) / stepsize
[docs]def sample_spline_derivative_of_curvature_2(spline_coeffs_x, spline_coeffs_y, t_arr, stepsize = 1e-4):
n = spline_coeffs_x.shape[1]
assert spline_coeffs_y.shape[1] == n
assert spline_coeffs_x.shape[0] == 4 and spline_coeffs_y.shape[0] == 4
retval = np.zeros((len(t_arr),))
for (i, t) in enumerate(t_arr):
col_ind = max(1, min(n, int(math.ceil(t))))
retval[i] = sample_spline_derivative_of_curvature_1(spline_coeffs_x[:, col_ind - 1], spline_coeffs_y[:, col_ind - 1],
t - col_ind + 1, stepsize = stepsize)
return retval