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

Question

I have a grid containing some data in polar coordinates, simulating data obtained from a LIDAR for the SLAM problem. Each row in the grid represents the angle, and each column represents a distance. The values contained in the grid store a weighted probability of the occupancy map for a Cartesian world.

After converting to Cartesian Coordinates, I obtain something like this:

This mapping is intended to work in a FastSLAM application, with at least 10 particles. The performance I am obtaining isn't good enough for a reliable application.

I have tried with nested loops, using the scipy.ndimage.geometric_transform library and accessing directly the grid with pre-computed coordinates.

In those examples, I am working with a 800x800 grid.

Nested loops: aprox 300ms

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 library (Described here): aprox 2500ms (Gives a smoother result since it interpolates the empty cells)

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 indexes: 80ms

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

I am not very used to python and Numpy, and I feel I am skipping an easy and fast way to solve this problem. Are there any other alternatives to solve that?

Many thanks to you all!

Answer 1

I came across a piece of code that seems to behave x10 times faster (8ms):

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