1

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

极坐标

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

极坐标

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

我尝试使用嵌套循环，使用 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 库（在此处描述）：aprox 2500ms（因为它插入了空单元格，所以得到更平滑的结果）

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，我觉得我正在跳过一种简单快捷的方法来解决这个问题。有没有其他方法可以解决这个问题？

非常感谢大家！

4

1 回答 1

0

我遇到了一段似乎表现得快 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,:]
于 2019-01-30T21:10:14.507 回答