是否有用于将极坐标网格映射到笛卡尔网格的快速 Numpy 算法？

Question

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

转换为笛卡尔坐标后，我得到了这样的东西：

此映射适用于 FastSLAM 应用程序，至少有 10 个粒子。我获得的性能对于可靠的应用程序来说还不够好。

我尝试使用嵌套循环，使用 scipy.ndimage.geometric_transform 库并直接访问具有 pre-computed 坐标的网格。

在这些示例中，我使用的是 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）：大约 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)

Pre-computed 索引：80 毫秒

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,:]

是否有用于将极坐标网格映射到笛卡尔网格的快速 Numpy 算法？

Is there a fast Numpy algorithm for mapping a Polar grid into a Cartesian grid?

python

numpy

slam