Сглаживание траектории ячейки прямой линии в Python

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

Учитывая три точки [(x1, y1), (x2, y2), (x3, y3)], средняя точка (x2, y2) может быть удалена, если наклоны последовательных отрезков совпадают. Т.е., если (y2 - y1) / (x2 - x1) == (y3 - y2) / (x3 - x2). Чтобы избежать возможности деления на ноль и устранить любую ошибку округления, условие устранения средней точки можно умножить на (x3 - x2) * (y2 - y1) == (x2 - x1) * (y3 - y2).

Допустим, у вас есть пустой массив в качестве пути:

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-векторами строк, а не одномерными массивами.

Результат

 [[1 1]
  [1 3]
  [2 3]
  [2 5]]

Вот ссылка IDEOne с демонстрацией: https://ideone.com/rykLCz

Если вам нужно преобразовать обратно в список:

smoothed = list(map(tuple, smoothed))

Answer 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)

Первая и последняя координаты всегда находятся, в остальном этот пример генерирует набор троек с координатами друг друга в середине, а затем проверяет для каждой из них, идеально ли они выстраиваются - если это так, он не включает координата, в противном случае она включается.