Skip to content

Commit

Permalink
plot xy 2d interpolation
Browse files Browse the repository at this point in the history
@ShisatoYano
ShisatoYano committed Aug 25, 2024
1 parent 0206e35 commit a629bb7
Showing 2 changed files with 34 additions and 428 deletions.
Binary file added src/simulations/course/cubic_spline/cubic_spline_2d.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
462 changes: 34 additions & 428 deletions src/simulations/course/cubic_spline/cubic_spline_2d_plot.py
View file
Original file line number Diff line number Diff line change
@@ -1,452 +1,58 @@
# """
# cubic_spline_2d_plot.py

# Author: Shisato Yano
# """

# # import path setting
# import numpy as np
# import sys
# import matplotlib.pyplot as plt
# from pathlib import Path

# abs_dir_path = str(Path(__file__).absolute().parent)
# relative_path = "/../../../components/"

# sys.path.append(abs_dir_path + relative_path + "course/cubic_spline_course")

# # import component module
# from cubic_spline_2d import CubicSpline2D


# # flag to show plot figure
# # when executed as unit test, this flag is set as false
# show_plot = True


# def main():
# """
# Main process function
# """

# x_points = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0]
# y_points = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0]

# ds = 0.1 # distance between 2 interpolated points

# cs = CubicSpline2D(x_points, y_points)
# s = np.arange(0, cs.s[-1], ds)

# xs, ys, yaws, curvs = [], [], [], []
# for i_s in s:
# i_x, i_y = cs.calc_interpolated_xy(i_s)
# xs.append(i_x)
# ys.append(i_y)

# plt.subplots(1)
# plt.plot(x_points, y_points, "xb", label="Input points")
# # plt.plot(xs, ys, "-r", label="Cubic spline path")
# plt.grid(True)
# plt.axis("equal")
# plt.xlabel("X[m]")
# plt.ylabel("Y[m]")
# plt.legend()

# if show_plot: plt.savefig("cubic_spline_2d.png")


# if __name__ == "__main__":
# main()

"""
Cubic spline planner
Author: Atsushi Sakai(@Atsushi_twi)
cubic_spline_2d_plot.py
Author: Shisato Yano
"""
import math
import numpy as np
import bisect


class CubicSpline1D:
"""
1D Cubic Spline class
Parameters
----------
x : list
x coordinates for data points. This x coordinates must be
sorted
in ascending order.
y : list
y coordinates for data points
Examples
--------
You can interpolate 1D data points.
>>> import numpy as np
>>> import matplotlib.pyplot as plt
>>> x = np.arange(5)
>>> y = [1.7, -6, 5, 6.5, 0.0]
>>> sp = CubicSpline1D(x, y)
>>> xi = np.linspace(0.0, 5.0)
>>> yi = [sp.calc_position(x) for x in xi]
>>> plt.plot(x, y, "xb", label="Data points")
>>> plt.plot(xi, yi , "r", label="Cubic spline interpolation")
>>> plt.grid(True)
>>> plt.legend()
>>> plt.show()
.. image:: cubic_spline_1d.png
"""

def __init__(self, x, y):

h = np.diff(x)
if np.any(h < 0):
raise ValueError("x coordinates must be sorted in ascending order")

self.a, self.b, self.c, self.d = [], [], [], []
self.x = x
self.y = y
self.nx = len(x) # dimension of x

# calc coefficient a
self.a = [iy for iy in y]

# calc coefficient c
A = self.__calc_A(h)
B = self.__calc_B(h, self.a)
self.c = np.linalg.solve(A, B)

# calc spline coefficient b and d
for i in range(self.nx - 1):
d = (self.c[i + 1] - self.c[i]) / (3.0 * h[i])
b = 1.0 / h[i] * (self.a[i + 1] - self.a[i]) \
- h[i] / 3.0 * (2.0 * self.c[i] + self.c[i + 1])
self.d.append(d)
self.b.append(b)

def calc_position(self, x):
"""
Calc `y` position for given `x`.
if `x` is outside the data point's `x` range, return None.
Returns
-------
y : float
y position for given x.
"""
if x < self.x[0]:
return None
elif x > self.x[-1]:
return None

i = self.__search_index(x)
dx = x - self.x[i]
position = self.a[i] + self.b[i] * dx + \
self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0

return position

def calc_first_derivative(self, x):
"""
Calc first derivative at given x.
if x is outside the input x, return None
Returns
-------
dy : float
first derivative for given x.
"""

if x < self.x[0]:
return None
elif x > self.x[-1]:
return None

i = self.__search_index(x)
dx = x - self.x[i]
dy = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0
return dy

def calc_second_derivative(self, x):
"""
Calc second derivative at given x.

if x is outside the input x, return None
Returns
-------
ddy : float
second derivative for given x.
"""

if x < self.x[0]:
return None
elif x > self.x[-1]:
return None
# import path setting
import numpy as np
import sys
import matplotlib.pyplot as plt
from pathlib import Path

i = self.__search_index(x)
dx = x - self.x[i]
ddy = 2.0 * self.c[i] + 6.0 * self.d[i] * dx
return ddy
abs_dir_path = str(Path(__file__).absolute().parent)
relative_path = "/../../../components/"

def __search_index(self, x):
"""
search data segment index
"""
return bisect.bisect(self.x, x) - 1
sys.path.append(abs_dir_path + relative_path + "course/cubic_spline_course")

def __calc_A(self, h):
"""
calc matrix A for spline coefficient c
"""
A = np.zeros((self.nx, self.nx))
A[0, 0] = 1.0
for i in range(self.nx - 1):
if i != (self.nx - 2):
A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1])
A[i + 1, i] = h[i]
A[i, i + 1] = h[i]
# import component module
from cubic_spline_2d import CubicSpline2D

A[0, 1] = 0.0
A[self.nx - 1, self.nx - 2] = 0.0
A[self.nx - 1, self.nx - 1] = 1.0
return A

def __calc_B(self, h, a):
"""
calc matrix B for spline coefficient c
"""
B = np.zeros(self.nx)
for i in range(self.nx - 2):
B[i + 1] = 3.0 * (a[i + 2] - a[i + 1]) / h[i + 1]\
- 3.0 * (a[i + 1] - a[i]) / h[i]
return B
# flag to show plot figure
# when executed as unit test, this flag is set as false
show_plot = True


class CubicSpline2D:
def main():
"""
Cubic CubicSpline2D class
Parameters
----------
x : list
x coordinates for data points.
y : list
y coordinates for data points.
Examples
--------
You can interpolate a 2D data points.
>>> import matplotlib.pyplot as plt
>>> x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0]
>>> y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0]
>>> ds = 0.1 # [m] distance of each interpolated points
>>> sp = CubicSpline2D(x, y)
>>> s = np.arange(0, sp.s[-1], ds)
>>> rx, ry, ryaw, rk = [], [], [], []
>>> for i_s in s:
... ix, iy = sp.calc_position(i_s)
... rx.append(ix)
... ry.append(iy)
... ryaw.append(sp.calc_yaw(i_s))
... rk.append(sp.calc_curvature(i_s))
>>> plt.subplots(1)
>>> plt.plot(x, y, "xb", label="Data points")
>>> plt.plot(rx, ry, "-r", label="Cubic spline path")
>>> plt.grid(True)
>>> plt.axis("equal")
>>> plt.xlabel("x[m]")
>>> plt.ylabel("y[m]")
>>> plt.legend()
>>> plt.show()
.. image:: cubic_spline_2d_path.png
>>> plt.subplots(1)
>>> plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw")
>>> plt.grid(True)
>>> plt.legend()
>>> plt.xlabel("line length[m]")
>>> plt.ylabel("yaw angle[deg]")
.. image:: cubic_spline_2d_yaw.png
>>> plt.subplots(1)
>>> plt.plot(s, rk, "-r", label="curvature")
>>> plt.grid(True)
>>> plt.legend()
>>> plt.xlabel("line length[m]")
>>> plt.ylabel("curvature [1/m]")
.. image:: cubic_spline_2d_curvature.png
Main process function
"""

def __init__(self, x, y):
self.s = self.__calc_s(x, y)
self.sx = CubicSpline1D(self.s, x)
self.sy = CubicSpline1D(self.s, y)

def __calc_s(self, x, y):
dx = np.diff(x)
dy = np.diff(y)
self.ds = np.hypot(dx, dy)
s = [0]
s.extend(np.cumsum(self.ds))
return s

def calc_position(self, s):
"""
calc position
Parameters
----------
s : float
distance from the start point. if `s` is outside the data point's
range, return None.
Returns
-------
x : float
x position for given s.
y : float
y position for given s.
"""
x = self.sx.calc_position(s)
y = self.sy.calc_position(s)

return x, y

def calc_curvature(self, s):
"""
calc curvature
Parameters
----------
s : float
distance from the start point. if `s` is outside the data point's
range, return None.
Returns
-------
k : float
curvature for given s.
"""
dx = self.sx.calc_first_derivative(s)
ddx = self.sx.calc_second_derivative(s)
dy = self.sy.calc_first_derivative(s)
ddy = self.sy.calc_second_derivative(s)
k = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2))
return k

def calc_yaw(self, s):
"""
calc yaw
Parameters
----------
s : float
distance from the start point. if `s` is outside the data point's
range, return None.
Returns
-------
yaw : float
yaw angle (tangent vector) for given s.
"""
dx = self.sx.calc_first_derivative(s)
dy = self.sy.calc_first_derivative(s)
yaw = math.atan2(dy, dx)
return yaw

x_points = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0]
y_points = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0]

def calc_spline_course(x, y, ds=0.1):
sp = CubicSpline2D(x, y)
s = list(np.arange(0, sp.s[-1], ds))
ds = 0.1 # distance between 2 interpolated points

rx, ry, ryaw, rk = [], [], [], []
for i_s in s:
ix, iy = sp.calc_position(i_s)
rx.append(ix)
ry.append(iy)
ryaw.append(sp.calc_yaw(i_s))
rk.append(sp.calc_curvature(i_s))

return rx, ry, ryaw, rk, s


def main_1d():
print("CubicSpline1D test")
import matplotlib.pyplot as plt
x = np.arange(5)
y = [1.7, -6, 5, 6.5, 0.0]
sp = CubicSpline1D(x, y)
xi = np.linspace(0.0, 5.0)

plt.plot(x, y, "xb", label="Data points")
plt.plot(xi, [sp.calc_position(x) for x in xi], "r",
label="Cubic spline interpolation")
plt.grid(True)
plt.legend()
plt.show()


def main_2d(): # pragma: no cover
print("CubicSpline1D 2D test")
import matplotlib.pyplot as plt
x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0]
y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0]
ds = 0.1 # [m] distance of each interpolated points
cs = CubicSpline2D(x_points, y_points)
s = np.arange(0, cs.s[-1], ds)

sp = CubicSpline2D(x, y)
s = np.arange(0, sp.s[-1], ds)

rx, ry, ryaw, rk = [], [], [], []
xs, ys, yaws, curvs = [], [], [], []
for i_s in s:
ix, iy = sp.calc_position(i_s)
rx.append(ix)
ry.append(iy)
ryaw.append(sp.calc_yaw(i_s))
rk.append(sp.calc_curvature(i_s))

i_x, i_y = cs.calc_interpolated_xy(i_s)
xs.append(i_x)
ys.append(i_y)

plt.subplots(1)
plt.plot(x, y, "xb", label="Data points")
plt.plot(rx, ry, "-r", label="Cubic spline path")
plt.plot(x_points, y_points, "xb", label="Input points")
plt.plot(xs, ys, "-r", label="Cubic spline path")
plt.grid(True)
plt.axis("equal")
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.xlabel("X[m]")
plt.ylabel("Y[m]")
plt.legend()

plt.savefig("cubic_spline_2d.png")

plt.subplots(1)
plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("yaw angle[deg]")

plt.savefig("cubic_spline_yaw_angle.png")

plt.subplots(1)
plt.plot(s, rk, "-r", label="curvature")
plt.grid(True)
plt.legend()
plt.xlabel("line length[m]")
plt.ylabel("curvature [1/m]")

plt.savefig("cubic_spline_curvature.png")
if show_plot: plt.savefig("cubic_spline_2d.png")


if __name__ == '__main__':
# main_1d()
main_2d()
if __name__ == "__main__":
main()

0 comments on commit a629bb7

Please sign in to comment.