如何解决重新采样轨迹以在每个采样中具有相等的欧几里德距离
假设我们有一个x,y点列表:
x = [0,0]
y = [0,10,100]
点之间的欧几里得距离现在是[10,90]。
我正在寻找一个接受x,y和sample_rate的函数,并且可以输出相等的距离点。例如:
x = [0,100]
resample_distance = 10
resampler(x,y,resample_distance)
# Outputs:
# [0,20,30,40,50,60,70,80,90,100]
# [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.]
使用任何第三方库(例如numpy,scipy等)都可以。
解决方法
我实施了下一个解决方案。
所有提高效率的功能均由支持Numba / Just-in-Time技术的Ahead-of-Time编译器/优化器进行编译。每当启动Python代码时,Numba都会自动将所有由@numba.njit
装饰器功能标记的代码自动转换为纯C/C++代码,然后将C ++代码编译为机器代码。在此类函数中,没有与Python进行交互,仅在内部使用了低级的快速结构。因此,Numba通常能够将几乎任何代码的速度平均提高50x
-200x
倍,非常快!这样的Python编译代码通常达到的速度非常接近纯C / C ++中手动实现的相同算法的速度。要使用Numba,只需安装两个下一个python软件包:python -m pip install numpy numba
。
下一步在我的代码中完成:
- 输入函数由两个一维数组
x
和y
表示。 - 然后,输入函数(点)由两种分段函数之一-1)Approximated 2){{3}来Interpolated(或称为Linear)。 }。
- 线性分段逼近函数仅连接点
(x0,y0)
的给定数组,以便两个连续点(x1,y1)
和(x2,y2)
的对通过直线(段)连接。 - 三次样条曲线是平滑逼近函数的更高级方法,它连接所有点
(x0,y0)
,从而使每对点(x1,y2)
都由{{3 }},使其通过这两个端点,再加上公共点内相邻线段的一阶和二阶导数相等,所有这些都确保函数看起来非常平滑和美观,并且在计算机图形学中非常流行用于自然/逼真的逼近/功能/表面的可视化。 - 然后使用非常快的Cubic Spline逐一搜索此插值函数上的点,以使每个连续两个点之间的cubic polynomial完全等于期望值(算法提供)值
d
。 - 以上只是计算部分。其余步骤将可视化部分,并使用
matplotlib
库绘制图。地块的详细说明在地块之前的代码之后。
要使用此已实现的欧几里得等距重采样算法,您只需import
我的下一个脚本模块并执行xr,yr = module_name.resample_euclid_equidist(x,y,dist)
,其中输入和输出x
和{{1} }都是具有浮点数的一维numpy数组,这将返回以y
欧几里德距离重新采样的输入点。更多用法示例位于我的代码的dist
函数中。第一次运行非常慢(可能需要test()
秒),该运行只是编译运行,我所有的代码都会自动预编译为C / C ++,然后是机器代码,下次运行非常快,尤其是重采样功能本身只需要几毫秒。同样,要仅使用代码的计算部分,您需要安装15
,并且要运行包括测试和可视化的整个代码(只需运行我的脚本),您只需安装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}[:])' for ii in (4,8)],cache = True,inline = 'always')
def piece_wise_linear(x,k,dummy0,dummy1):
xsh = x.shape
x = x.ravel()
ix = np.searchsorted(x0[1 : -1],x)
y = func_linear(x,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,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,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,inline = 'always')
def func_spline(x,a,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}[:],inline = 'always')
def piece_wise_spline(x,d):
xsh = x.shape
x = x.ravel()
ix = np.searchsorted(x0[1 : -1],x)
y = func_spline(x,d)
y = y.reshape(xsh)
return y
# Appximates function given by (x0,y0) by piece-wise spline or linear
def approx_func(x0,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,np.zeros((0,dtype = y0.dtype),dtype = y0.dtype))
elif t == 'spline':
a,d = calc_spline_params(x0,y0)
return piece_wise_spline,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},b1,fastmath = True)
def _resample_inner(x,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,y0 = 0,x[0],y[0]
#print(i,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,a4)[0]
else:
ym = piece_wise_linear(t0,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,np.sin(x0),dist(x0,xm,ym),np.sin(xm)))
x0,y0 = xm,ym
#print('_1',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,*,aerr = 2 ** -21,rerr = 2 ** -9,approx = 'spline',return_approx = False,strict = True,):
assert d > 0,d
dtype = np.dtype(y.dtype).type
x,rerr = [dtype(e) for e in [x,rerr]]
ixs = np.argsort(x)
x,y = x[ixs],y[ixs]
f,fargs = approx_func(x,approx)
r = _resample_inner(x,approx == 'spline',*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,1.5,approx = sl,return_approx = True)
print(sl,'try','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,'original'),(fax,fay,f'approx_{sl}'),(xa,'euclid_euqidist'),]):
p,= plt.plot(cx,cy)
p.set_label(fn)
if ci >= 2:
plt.scatter(cx,marker = '.',color = p.get_color())
if False:
# Show distances
def dist(x0,x1,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]),(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
是逼近函数(样条曲线或线性曲线),这只是内插函数,它被绘制成数百个点,这就是为什么看起来很平滑的原因,{{ 1}}是得到的Euclidian-Equal-Distance点,这恰好是任务所在,两个绿色小圆圈之间的每个线段的Euclidian长度正好等于所需的距离orange
。第一个屏幕代表分段三次样条曲线的近似值。第二个屏幕代表对于完全相同的输入点的分段线性函数的近似值。
样条线:
线性:
,正如许多评论所指出的那样,您需要更加具体地说明如何处理歧义案件。在您的示例x = [0,0]; y = [0,10,100]
中,值是10的倍数,它们整齐地加起来。但是,当值不整齐地累加时,您需要确定自己如何处理情况。
我编写了一个函数,该函数可以从第一个点开始,在两个点之间相互返回给定距离(resample_distance
)的所有点的x和y值。也许这对您有用,以此为基础。
import numpy as np
from matplotlib import pyplot as plt
def resampler_2_points(p1,p2,resample_distance,include_endpoint=False):
# get the distacne between the points
distance_p1p2 = np.sqrt( np.sum( (p2 - p1)**2 ) )
# check for invalid cases
if resample_distance > distance_p1p2:
print("Resample distance larger than distance between points")
return None
elif distance_p1p2 == 0:
print("Distance between the two points is 0")
return None
# if all is okay
else:
# get the stepsize of x and y coordinates
stepsize_x,stepsize_y = (p2 - p1) * (resample_distance / distance_p1p2)
# handle the case when a 'stepsize' along and axis equals 0
if stepsize_x == 0:
y = np.arange(p1[1],p2[1],stepsize_y)
x = np.zeros(len(y)) + p1[0]
elif stepsize_y == 0:
x = np.arange(p1[0],p2[0],stepsize_x)
y = np.zeros(len(x)) + p1[1]
# all other cases
else:
x = np.arange(p1[0],stepsize_x)
y = np.arange(p1[1],stepsize_y)
# optionally append endpoint to final list
if include_endpoint:
x = np.append(x,p2[0])
y = np.append(y,p2[1])
# retrun the x and y coordinates in two arrays
return x,y
下面是此函数与输出图一起使用的示例。
# set values (x and y coordiantes) for 2 points,and an resample distance
p1 = np.array([2,3])
p2 = np.array([20,15])
resample_distance = 4
x,y = resampler_2_points(p1,include_endpoint=False)
plt.plot(x,'o--r',label="Sampled points")
plt.scatter([p1[0],p2[0]],[p1[1],p2[1]],s=100,c='b',label="Input points")
plt.ylim((0,25))
plt.xlim((0,25))
plt.legend()
plt.show()
版权声明:本文内容由互联网用户自发贡献,该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 dio@foxmail.com 举报,一经查实,本站将立刻删除。