Question

我正在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)。

Answer 1

您的问题可以改写为：对于每个坐标，如果下一个坐标紧接在其后，则将其删除。

我假设“移动方向不会改变”是指：如果您要通过前两个点画一条线，那么第三个点将在第一个点的第二个点之后，位于同一条线上。

因此，[(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)

第一个和最后一个坐标始终存在，对于其他示例，此示例生成一组三元组，彼此之间的坐标位于中间，然后检查每个三元组是否完美对齐-如果这样，则不包括坐标，否则它确实包含它。

Answer 2

给定三个点[(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))

Python中的直线网格单元路径平滑

2 个答案: