对轨迹进行重新采样，使每个样本中的欧氏距离相等

假设我们有一个 x,y 点列表：

x = [0, 0, 0]
y = [0, 10, 100]

点之间的欧几里得距离现在为 [10, 90]。
我正在寻找一个接受x、y 和sample_rate 的函数，并且可以输出相等的距离点。例如。：

x = [0, 0, 0]
y = [0, 10, 100]

resample_distance = 10
resampler(x, y, resample_distance)
# Outputs:
# [0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100]
# [0, 0, 0, 0, 0,0, 0, 0, 0, 0, 0]

使用类似的answer，我们可以获得几乎正确的值，但这并不准确：

resample_trajectory_same_distance(data[0], data[1], 10)
# Output:
# [ 0.        , 10.27027027, 20.81081081, 31.08108108, 41.62162162, 51.89189189, 62.43243243, 72.7027027 , 83.24324324, 93.78378378]
# [0., 0., 0., 0., 0., 0., 0., 0., 0., 0.]

使用任何第三方库（如 numpy、scipy 等）都可以。

我实施了下一个解决方案。

所有提高效率的函数都是由Numba编译器/优化器支持准时制/提前时间技术。 Numba 转换所有标记为@numba.njit装饰器函数要纯C/C++每当Python代码启动时，C++代码就会自动编译为机器代码。这些函数中没有与Python进行交互，内部仅使用低级快速结构。因此，Numba 通常能够提高几乎所有代码的速度50x-200x平均次数，非常快！此类 Python 编译代码的速度通常非常接近于纯 C/C++ 中手动实现的相同算法的速度。要使用 Numba，只需安装接下来的两个 python 包：python -m pip install numpy numba.

接下来的步骤在我的代码中完成：

1. 输入函数由两个一维数组表示x and y.
2. 输入函数（点）为近似值（或以不同方式称为插值) 通过两种分段函数之一 - 1)Linear 2) 三次样条.
3. 线性分段逼近函数仅连接给定的点数组(x0, y0)所以这对两个连续点(x1, y1) and (x2, y2)由直线（线段）连接。
4. 三次样条是平滑逼近函数的更高级方法，它连接所有点(x0, y0)使得每对点(x1, y1) and (x2, y2)由立方线段连接，表示为三次多项式这样它经过这两个端点加上公共点内相邻段的一阶和二阶导数相等，这些都确保函数看起来非常平滑和漂亮，并且在计算机图形学中非常流行，用于进行自然/真实的近似/可视化功能/表面。
5. 然后使用速度非常快二分查找算法我正在这个插值函数上逐一搜索点，这样的点欧氏距离每两个连续点之间的值恰好等于所需的（提供给算法）值d.
6. 以上只是计算部分。其余步骤将部分可视化，使用绘图matplotlib图书馆。绘图的详细描述位于绘图之前的代码之后。

为了使用这个实现的欧几里德等距离重采样算法，您只需要import我的下一个脚本模块，然后做xr, yr = module_name.resample_euclid_equidist(x, y, dist)其中输入和输出x and y都是具有浮点数的一维 numpy 数组，这将返回在以下位置重新采样的输入点dist欧几里得距离。更多使用示例位于我的代码中test()功能。第一次运行非常慢（可以花大约15秒），这次运行只是编译运行，我的所有代码都会自动预编译为 C/C++，然后是机器代码，接下来的运行速度非常快，尤其是重采样函数本身只需要几毫秒。另外，要仅使用代码的计算部分，您需要安装python -m pip install numpy numba，并运行我的整个代码，包括测试和可视化（只需运行我的脚本），您需要安装python -m pip install numpy numba matplotlib就一次。

在线尝试一下！

# Needs:
#     For computation: python -m pip install numpy numba
#     For testing:     python -m pip install matplotlib

if __name__ == '__main__':
    print('Compiling...', flush = True)
    
import numba, numpy as np

# Linear Approximator related functions

# Spline value calculating function, given params and "x"
@numba.njit(cache = True, fastmath = True, inline = 'always')
def func_linear(x, ix, x0, y0, k):
    return (x - x0[ix]) * k[ix] + y0[ix]
    
# Compute piece-wise linear function for "x" out of sorted "x0" points
@numba.njit([f'f{ii}[:](f{ii}[:], f{ii}[:], f{ii}[:], f{ii}[:], f{ii}[:], f{ii}[:])' for ii in (4, 8)],
    cache = True, fastmath = True, inline = 'always')
def piece_wise_linear(x, x0, y0, k, dummy0, dummy1):
    xsh = x.shape
    x = x.ravel()
    ix = np.searchsorted(x0[1 : -1], x)
    y = func_linear(x, ix, x0, y0, k)
    y = y.reshape(xsh)
    return y
    
# Spline Approximator related functions
    
# Solves linear system given by Tridiagonal Matrix
# Helper for calculating cubic splines
@numba.njit(cache = True, fastmath = True, inline = 'always')
def tri_diag_solve(A, B, C, F):
    n = B.size
    assert A.ndim == B.ndim == C.ndim == F.ndim == 1 and (
        A.size == B.size == C.size == F.size == n
    ) #, (A.shape, B.shape, C.shape, F.shape)
    Bs, Fs = np.zeros_like(B), np.zeros_like(F)
    Bs[0], Fs[0] = B[0], F[0]
    for i in range(1, n):
        Bs[i] = B[i] - A[i] / Bs[i - 1] * C[i - 1]
        Fs[i] = F[i] - A[i] / Bs[i - 1] * Fs[i - 1]
    x = np.zeros_like(B)
    x[-1] = Fs[-1] / Bs[-1]
    for i in range(n - 2, -1, -1):
        x[i] = (Fs[i] - C[i] * x[i + 1]) / Bs[i]
    return x
    
# Calculate cubic spline params
@numba.njit(cache = True, fastmath = True, inline = 'always')
def calc_spline_params(x, y):
    a = y
    h = np.diff(x)
    c = np.concatenate((np.zeros((1,), dtype = y.dtype),
        np.append(tri_diag_solve(h[:-1], (h[:-1] + h[1:]) * 2, h[1:],
        ((a[2:] - a[1:-1]) / h[1:] - (a[1:-1] - a[:-2]) / h[:-1]) * 3), 0)))
    d = np.diff(c) / (3 * h)
    b = (a[1:] - a[:-1]) / h + (2 * c[1:] + c[:-1]) / 3 * h
    return a[1:], b, c[1:], d
    
# Spline value calculating function, given params and "x"
@numba.njit(cache = True, fastmath = True, inline = 'always')
def func_spline(x, ix, x0, a, b, c, d):
    dx = x - x0[1:][ix]
    return a[ix] + (b[ix] + (c[ix] + d[ix] * dx) * dx) * dx
    
# Compute piece-wise spline function for "x" out of sorted "x0" points
@numba.njit([f'f{ii}[:](f{ii}[:], f{ii}[:], f{ii}[:], f{ii}[:], f{ii}[:], f{ii}[:])' for ii in (4, 8)],
    cache = True, fastmath = True, inline = 'always')
def piece_wise_spline(x, x0, a, b, c, d):
    xsh = x.shape
    x = x.ravel()
    ix = np.searchsorted(x0[1 : -1], x)
    y = func_spline(x, ix, x0, a, b, c, d)
    y = y.reshape(xsh)
    return y
    
# Appximates function given by (x0, y0) by piece-wise spline or linear
def approx_func(x0, y0, t = 'spline'): # t is spline/linear
    assert x0.ndim == 1 and y0.ndim == 1 and x0.size == y0.size#, (x0.shape, y0.shape)
    n = x0.size - 1
    if t == 'linear':
        k = np.diff(y0) / np.diff(x0)
        return piece_wise_linear, (x0, y0, k, np.zeros((0,), dtype = y0.dtype), np.zeros((0,), dtype = y0.dtype))
    elif t == 'spline':
        a, b, c, d = calc_spline_params(x0, y0)
        return piece_wise_spline, (x0, a, b, c, d)
    else:
        assert False, t

# Main function that computes Euclidian Equi-Distant points based on approximation function
@numba.njit(
    [f'f{ii}[:, :](f{ii}[:], f{ii}[:], f{ii}, b1, b1, f{ii}, f{ii}, f{ii}[:], f{ii}[:], f{ii}[:], f{ii}[:], f{ii}[:])' for ii in (4, 8)],
    cache = True, fastmath = True)
def _resample_inner(x, y, d, is_spline, strict, aerr, rerr, a0, a1, a2, a3, a4):
    rs, r = 0, np.zeros((1 << 10, 2), dtype = y.dtype)
    t0 = np.zeros((1,), dtype = y.dtype)
    i, x0, y0 = 0, x[0], y[0]
    #print(i, x0, y0, np.sin(x0))
    while True:
        if rs >= r.size:
            r = np.concatenate((r, np.zeros(r.shape, dtype = r.dtype))) # Grow array
        r[rs, 0] = x0
        r[rs, 1] = y0
        rs += 1
        if i + 1 >= x.size:
            break
        ie = min(i + 1 + np.searchsorted(x[i + 1:], x0 + d), x.size - 1)
        for ie in range(i + 1 if strict else ie, ie + 1):
            xl = max(x0, x[ie - 1 if strict else i])
            xr = max(x0, x[ie])
            # Do binary search to find next point
            for ii in range(1000):
                if xr - xl <= aerr:
                    break # Already very small delta X interval
                xm = (xl + xr) / 2
                t0[0] = xm
                if is_spline:
                    ym = piece_wise_spline(t0, a0, a1, a2, a3, a4)[0]
                else:
                    ym = piece_wise_linear(t0, a0, a1, a2, a3, a4)[0]
                
                # Compute Euclidian distance
                dx_, dy_ = xm - x0, ym - y0
                dm = np.sqrt(dx_ * dx_ + dy_ * dy_)

                if -rerr <= dm / d - 1. <= rerr:
                    break # We got d with enough precision
                if dm >= d:
                    xr = xm
                else:
                    xl = xm
            else:
                assert False # To many iterations
            if -rerr <= dm / d - 1. <= rerr:
                break # Next point found
        else:
            break # No next point found, we're finished
        i = np.searchsorted(x, xm) - 1
        #print('_0', i, x0, y0, np.sin(x0), dist(x0, xm, y0, ym), dist(x0, xm, np.sin(x0), np.sin(xm)))
        x0, y0 = xm, ym
        #print('_1', i, x0, y0, np.sin(x0), dm)
    return r[:rs]
    
# Resamples (x, y) points using given approximation function type
# so that euclidian distance between each resampled points equals to "d".
# If strict = True then strictly closest (by X) next point at distance "d"
# is chosen, which can imply more computations, when strict = False then
# any found point with distance "d" is taken as next.
def resample_euclid_equidist(
    x, y, d, *,
    aerr = 2 ** -21, rerr = 2 ** -9, approx = 'spline',
    return_approx = False, strict = True,
):
    assert d > 0, d
    dtype = np.dtype(y.dtype).type
    x, y, d, aerr, rerr = [dtype(e) for e in [x, y, d, aerr, rerr]]
    ixs = np.argsort(x)
    x, y = x[ixs], y[ixs]
    f, fargs = approx_func(x, y, approx)
    r = _resample_inner(x, y, d, approx == 'spline', strict, aerr, rerr, *fargs)
    return (r[:, 0], r[:, 1]) + ((), (lambda x: f(x, *fargs),))[return_approx]

def test():
    import matplotlib.pyplot as plt, numpy as np, time
    np.random.seed(0)
    # Input
    n = 50
    x = np.sort(np.random.uniform(0., 10 * np.pi, (n,)))
    y = np.sin(x) * 5 + np.sin(1 + 2.5 * x) * 3 + np.sin(2 + 0.5 * x) * 2
    # Visualize
    for isl, sl in enumerate(['spline', 'linear']):
        # Compute resampled points
        for i in range(3):
            tb = time.time()
            xa, ya, fa = resample_euclid_equidist(x, y, 1.5, approx = sl, return_approx = True)
            print(sl, 'try', i, 'run time', round(time.time() - tb, 4), 'sec', flush = True)
        # Compute spline/linear approx points
        fax = np.linspace(x[0], x[-1], 1000)
        fay = fa(fax)
        # Plotting
        plt.rcParams['figure.figsize'] = (9.6, 5.4)
        for ci, (cx, cy, fn) in enumerate([
            (x, y, 'original'), (fax, fay, f'approx_{sl}'), (xa, ya, 'euclid_euqidist'),
        ]):
            p, = plt.plot(cx, cy)
            p.set_label(fn)
            if ci >= 2:
                plt.scatter(cx, cy, marker = '.', color = p.get_color())
                if False:
                    # Show distances
                    def dist(x0, x1, y0, y1):
                        # Compute Euclidian distance
                        dx, dy = x1 - x0, y1 - y0
                        return np.sqrt(dx * dx + dy * dy)
                    for i in range(cx.size - 1):
                        plt.annotate(
                            round(dist(cx[i], cx[i + 1], cy[i], cy[i + 1]), 2),
                            (cx[i], cy[i]), fontsize = 'xx-small',
                        )
        plt.gca().set_aspect('equal', adjustable = 'box')
        plt.legend()
        plt.show()
        plt.clf()

if __name__ == '__main__':
    test()

下面是结果图。以函数为例y = np.sin(x) * 5 + np.sin(1 + 2.5 * x) * 3 + np.sin(2 + 0.5 * x) * 2采样于50均匀随机点0 <= x <= 10 * pi范围。地块：blue是原函数，用直线点连接，orange是近似函数（样条函数或线性函数），这只是插值函数，它被绘制为数百个点，这就是为什么看起来平滑，green得到的欧几里德等距离点，正是任务的内容，两个绿色小圆圈之间每段的欧几里德长度恰好等于所需距离d。第一个屏幕表示分段三次样条的近似值。第二个屏幕表示完全相同的输入点的分段线性函数的近似值。

Spline:

Linear: