我正在ROS中构建一个导航节点。 我是ROS和Python(2.7)的新手。 我有一个网格单元格，从中获得两个单元格之间的最短路径(单元格坐标)(从开始到目标)。
我的问题是： 什么是快速优雅的方式来抚平我的道路？
平滑路径意味着如果我的移动方向在两个单元格之间没有改变，我可以省略这些单元格之间的所有单元格。
我的意思是，例如，如果我的路径是(x,y):[(1,1),(1,2),(1,3),(2,3),(2,4),(2,5)]
我的顺利路径应该是：[(1,1),(1,3),(2,3),(2,5)]
请注意，例如在(1,1)和(1,3)之间，方向不会改变，因此我们删除(1,2).
您的问题可以改写为：对于每个坐标，如果下一个坐标紧随其后的下一个坐标，请将其删除。
我假设"移动方向不会改变"意味着：如果你要画一条穿过前两点的线，那么第三条线将在同一条线上，从第一个点开始的第二个点后面。
所以，[(1,1), (2,2), (3,3)]变得[(1,1), (3,3)]，[(1,1), (3,2), (5,3)]变得[(1,1), (5,3)]。
path = [(1, 1), (1, 2), (1, 3), (2, 3), (2, 4), (2, 5)]
def behind(x, y, z):
# if the first two points are on a vertical line
if x[0] == y[0]:
# if the third point is one the same vertical line?
if y[0] == z[0]:
# then they still need to be in the correct order
return x[1] > y[1] > z[1] or x[1] < y[1] < z[1]
else:
return False
elif y[0] == z[0]:
return False
# none of the points are directly above each other, calculate and compare direction
return (x[1] - y[1]) / (x[0] - y[0]) == (y[1] - z[1]) / (y[0] - z[0])
triples = list(zip(path[:-2], path[1:-1], path[2:]))
smooth_path = path[:1] + [
y for x, y, z in triples
if not behind(x, y, z)
] + path[-1:]
print(smooth_path)
第一个和最后一个坐标始终在，对于其余坐标，此示例生成一组三元组，其中每个坐标位于中间，然后检查它们是否完美排列 - 如果是，则不包括坐标，否则包含它。
给定三个点[(x1, y1), (x2, y2), (x3, y3)]，如果连续线段的斜率相同，则可以删除中间点(x2, y2)。 即，如果(y2 - y1) / (x2 - x1) == (y3 - y2) / (x3 - x2).为了避免除以零并消除任何舍入误差的可能性，消除中间点的条件可以交叉乘以(x3 - x2) * (y2 - y1) == (x2 - x1) * (y3 - y2)。
假设您有一个 numpy 数组作为您的路径：
path = [(1, 1), (1, 2), (1, 3), (2, 3), (2, 4), (2, 5)]
path = np.array(path)
可以按如下方式计算保留掩码：
delta = np.diff(path, axis=0)
prod = delta[:-1, :] * delta[1:, ::-1]
diff是(x2 - x1), (y2 - y1)的两列向量。 然后prod变成包含消除条件分量的两列向量。你可以制作它的面具：
mask = (prod[:, 0] != prod[:, 1])
并敷上面膜：
smoothed = np.concatenate((
path[np.newaxis, 0, :],
path[1:-1, :][mask, :],
path[np.newaxis, -1, :]), axis=0)
串联是必需的，因为掩码包含len(path) - 2元素，如您所料。端点保证存在于输出中。因此，您最终必须连接第一行、屏蔽部分和最后一行。将np.newaxis插入索引可确保行切片以 2D 行向量而不是 1D 数组的形式出现。
结果是
[[1 1]
[1 3]
[2 3]
[2 5]]
下面是一个带有演示的 IDEOne 链接：https://ideone.com/rykLCz
如果需要转换回列表：
smoothed = list(map(tuple, smoothed))