我的目标是在Python中插入点对。我使用了以下方法:
def interpolate(obs,radius, n_steps):
ox = obs[:,0].tolist() # obstacle x position list [m]
oy = obs[:,1].tolist() # obstacle y position list [m]
interpol = []
for i in range(1,len(ox)):
prev_ = np.array([ox[i-1],oy[i-1]])
next_ = np.array([ox[i],oy[i]])
dist= radius[i-1]+radius[i]
for j in range(n_steps+1):
dr= dist/n_steps
dir_= next_ - prev_
dir_ = dir_ / np.linalg.norm(dir_)
next_point= prev_ + dr*j*dir_
interpol.append([next_point[0],next_point[1]])
return interpol
这是两个2D场景。由于某种原因,我得到的结果不完整,如下所示: pair of points
有什么主意我想念的吗? 提前致谢, 布鲁诺