4种插值算法
如果有一些稀疏的轨迹，如何将这些轨迹平滑插值处理呢？
方法1：线性插值
方法2：三次样条插值
方法3：贝塞尔曲线插值
方法4：拉格朗日插值
线性插值：在两两相邻的点之间差值，两点间所有插值点都在一条直线上。
贝塞尔曲线：贝塞尔曲线不会经过所有的坐标点，会根据坐标点的排列趋势去拟合出一条相对平滑的从第1个点到最后一个点之间的曲线。
三次样条插值：插值函数会经过所有的坐标点，拟合函数平滑。
拉格朗日插值：点太多，会出现不稳定的结果。见下图：
前三种插值算法都有特定的使用场景，按需使用就好了。
import time
import numpy as np
from scipy.interpolate import interp1d
from scipy.special import comb
def linear_interpolation(route, num_points):
# 1. 线性插值
# 将经纬度分开
lons = np.array([point[0] for point in route])
lats = np.array([point[1] for point in route])
# 创建插值函数
distance = np.cumsum(np.sqrt(np.ediff1d(lons, to_begin=0) ** 2 + np.ediff1d(lats, to_begin=0) ** 2))
distance /= distance[-1]
# 创建插值函数
lon_interp = interp1d(distance, lons, kind='linear')
lat_interp = interp1d(distance, lats, kind='linear')
# 生成新的距离点
new_distance = np.linspace(0, 1, num_points)
# 插值
new_lons = lon_interp(new_distance)
new_lats = lat_interp(new_distance)
return list(zip(new_lons, new_lats))
def cubic_spline_interpolation(route, num_points):
# 2. 三次样条插值
lons = np.array([point[0] for point in route])
lats = np.array([point[1] for point in route])
distance = np.cumsum(np.sqrt(np.ediff1d(lons, to_begin=0) ** 2 + np.ediff1d(lats, to_begin=0) ** 2))
distance /= distance[-1]
lon_interp = interp1d(distance, lons, kind='cubic')
lat_interp = interp1d(distance, lats, kind='cubic')
new_distance = np.linspace(0, 1, num_points)
new_lons = lon_interp(new_distance)
new_lats = lat_interp(new_distance)
return list(zip(new_lons, new_lats))
def bernstein_poly(i, n, t):
return comb(n, i) * (t ** i) * ((1 - t) ** (n - i))
def bezier_curve(route, num_points=100):
# 3. 贝塞尔曲线插值
n = len(route) - 1
t = np.linspace(0, 1, num_points)
lons = np.array([point[0] for point in route])
lats = np.array([point[1] for point in route])
new_lons = np.zeros(num_points)
new_lats = np.zeros(num_points)
for i in range(n + 1):
new_lons += bernstein_poly(i, n, t) * lons[i]
new_lats += bernstein_poly(i, n, t) * lats[i]
return list(zip(new_lons, new_lats))
import matplotlib.pyplot as plt
def plot_routes(original, linear, cubic, bezier):
plt.figure(figsize=(12, 8))
# 原始轨迹
orig_lons, orig_lats = zip(*original)
plt.plot(orig_lons, orig_lats, 'ro-', label='Original', alpha=0.5)
# 线性插值
lin_lons, lin_lats = zip(*linear)
plt.plot(lin_lons, lin_lats, 'b-', label='Linear', alpha=0.7)
# 三次样条插值
cub_lons, cub_lats = zip(*cubic)
plt.plot(cub_lons, cub_lats, 'g-', label='Cubic Spline', alpha=0.7)
# 贝塞尔曲线
bez_lons, bez_lats = zip(*bezier)
plt.plot(bez_lons, bez_lats, 'm-', label='Bezier', alpha=0.7)
plt.legend()
plt.xlabel('Longitude')
plt.ylabel('Latitude')
plt.title('Trajectory Interpolation Comparison')
plt.grid(True)
plt.show()
if __name__ == '__main__':
# 原始轨迹数据
route = [
[122.123456, 31.123456],
[122.234567, 31.234567],
[122.345678, 31.345678],
[122.456789, 31.456789],
[122.567890, 31.567890],
[122.678901, 31.578901],
[122.789012, 31.789012],
[122.890123, 31.890123],
[122.901234, 31.901234],
]
start_time = time.time()
# 线性插值
linear_route = linear_interpolation(route, 1000)
print("线性插值结果 (前5个点):", linear_route[:5])
print("线性插值用时:", time.time() - start_time, "秒")
start_time = time.time()
# 三次样条插值
cubic_route = cubic_spline_interpolation(route, 1000)
print("三次样条插值结果 (前5个点):", cubic_route[:5])
print("三次样条插值用时:", time.time() - start_time, "秒")
start_time = time.time()
# 贝塞尔曲线插值
bezier_route = bezier_curve(route, 1000)
print("贝塞尔曲线插值结果 (前5个点):", bezier_route[:5])
print("贝塞尔曲线插值用时:", time.time() - start_time, "秒")
# 绘制比较图
plot_routes(route, linear_route, cubic_route, bezier_route)
# 4、拉格朗日插值算法
import time
from scipy.interpolate import lagrange
import numpy as np
def lagrange_interp(x, y, x_new):
"""
Lagrange interpolation
:param x: x coordinates of data points
:param y: y coordinates of data points
:param x_new: x coordinates of new interpolated points
:return: y coordinates of new interpolated points
"""
f = lagrange(x, y)
y_new = f(x_new)
return y_new
if __name__ == '__main__':
# 原始数据
route = [
[122.123456, 31.123456],
[122.234567, 31.234567],
[122.345678, 31.345678],
[122.456789, 31.456789],
[122.567890, 31.567890],
[122.678901, 31.678901],
[122.789012, 31.789012],
[122.890123, 31.890123],
[122.901234, 31.901234],
]
x_list = [i[0] for i in route]
y_list = [i[1] for i in route]
# 新数据
x_new = np.arange(122.123456, 122.990123, 0.01)
y_new = lagrange_interp(x_list, y_list, x_new)
# 绘图
import matplotlib.pyplot as plt
plt.plot(x_list, y_list, 'o', label='original data')
plt.plot(x_new, y_new, label='interpolated data')
plt.legend()
plt.show()
神武大闹天宫怎么玩中国女排世界杯夺冠历程