是否有快速的Numpy算法将Polar网格映射到笛卡尔网格？

Question

我有一个网格，其中包含一些极坐标数据，模拟从LIDAR获得的SLAM问题数据。网格中的每一行代表角度，每一列代表距离。网格中包含的值存储了笛卡尔世界占用地图的加权概率。

转换为笛卡尔坐标后，我得到如下信息：

此映射旨在在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库（Described here）：aprox 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)

预计算索引：80ms

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

我不太习惯python和numpy，我觉得我跳过了解决此问题的简便方法。还有其他解决方案吗？

非常感谢大家！

Answer 1

我遇到了一段似乎快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,:]