Существует ли быстрый алгоритм Numpy для отображения полярной сетки в декартову сетку? - PullRequest
Новая
       33

Существует ли быстрый алгоритм Numpy для отображения полярной сетки в декартову сетку?

0 голосов
/ 30 января 2019

У меня есть сетка, содержащая некоторые данные в полярных координатах, имитирующие данные, полученные из LIDAR для задачи SLAM.Каждая строка в сетке представляет угол, а каждый столбец представляет расстояние.Значения, содержащиеся в сетке, хранят взвешенную вероятность карты занятости для декартового мира.

Polar coordiantes

После преобразования в декартовые координаты я получаю что-то вроде этого:

Polar coordiantes

Это отображение предназначено для работы в приложении FastSLAM, содержащем не менее 10 частиц.Производительность, которую я получаю, недостаточно хороша для надежного приложения.

Я пытался использовать вложенные циклы, используя библиотеку scipy.ndimage.geometric_transform и получая прямой доступ к сетке с предварительно вычисленными координатами.

В этих примерах я работаю с сеткой 800x800.

Вложенные циклы: приблизительно 300 мс 

i = 0
for scan in scans:
    hit = scan < laser.range_max
    if hit:
        d = np.linspace(scan + wall_size, 0, num=int((scan+ wall_size)/cell_size))
    else:
        d = np.linspace(scan, 0, num=int(scan/cell_size))

    for distance in distances:
        x = int(pos[0] + d * math.cos(angle[i]+pos[2]))
        y = int(pos[1] + d * math.sin(angle[i]+pos[2]))
        if distance > scan:
            grid_cart[y][x] = grid_cart[y][x] + hit_weight
        else:
            grid_cart[y][x] = grid_cart[y][x] + miss_weight

    i = i + 1

Библиотека Scipy ( Описано здесь ): приблизительно 2500 мс(Дает более плавный результат, поскольку он интерполирует пустые ячейки) 

grid_cart = S.ndimage.geometric_transform(weight_mat, polar2cartesian, 
    order=0,
    output_shape = (weight_mat.shape[0] * 2, weight_mat.shape[0] * 2),
    extra_keywords = {'inputshape':weight_mat.shape,
        'origin':(weight_mat.shape[0], weight_mat.shape[0])})

def polar2cartesian(outcoords, inputshape, origin):
    """Coordinate transform for converting a polar array to Cartesian coordinates. 
    inputshape is a tuple containing the shape of the polar array. origin is a
    tuple containing the x and y indices of where the origin should be in the
    output array."""

    xindex, yindex = outcoords
    x0, y0 = origin
    x = xindex - x0
    y = yindex - y0

    r = np.sqrt(x**2 + y**2)
    theta = np.arctan2(y, x)
    theta_index = np.round((theta + np.pi) * inputshape[1] / (2 * np.pi))

    return (r,theta_index)

Предварительно вычисленные индексы: 80 мс 

for i in range(0, 144000): 
    gird_cart[ys[i]][xs[i]] = grid_polar_1d[i]

Я не очень привык к Python и Numpy, и я чувствую, что япропускаю простой и быстрый способ решить эту проблему.Есть ли другие варианты решения этой проблемы?

Большое спасибо всем вам!

1 Ответ

0 голосов
/ 31 января 2019

Я обнаружил фрагмент кода, который, кажется, работает в 10 раз быстрее (8 мс): 

angle_resolution = 1
range_max = 400

a, r = np.mgrid[0:int(360/angle_resolution),0:range_max]

x = (range_max + r * np.cos(a*(2*math.pi)/360.0)).astype(int)
y = (range_max + r * np.sin(a*(2*math.pi)/360.0)).astype(int)

for i in range(0, int(360/angle_resolution)): 
    cart_grid[y[i,:],x[i,:]] = polar_grid[i,:]
...