[英]Is there a fast Numpy algorithm for mapping a Polar grid into a Cartesian grid?
I have a grid containing some data in polar coordinates, simulating data obtained from a LIDAR for the SLAM problem. 我有一个包含一些极坐标数据的网格,用于模拟从LIDAR获得的SLAM问题数据。 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. 此映射旨在在至少有10个粒子的FastSLAM应用程序中工作。 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. 我已经尝试过使用scipy.ndimage.geometric_transform库和使用预先计算的坐标直接访问网格的嵌套循环。
In those examples, I am working with a 800x800 grid. 在这些示例中,我正在使用800x800网格。
Nested loops: aprox 300ms 嵌套循环: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) Scipy库( 在此处描述 ):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)
Pre-computed indexes: 80ms 预先计算的索引: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. 我对python和Numpy不太熟悉,我感觉自己正在跳过一种简单快速的方法来解决此问题。 Are there any other alternatives to solve that?
还有其他解决方案吗?
Many thanks to you all! 非常感谢大家!
I came across a piece of code that seems to behave x10 times faster (8ms): 我遇到了一段代码,其运行速度似乎快了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,:]
声明:本站的技术帖子网页,遵循CC BY-SA 4.0协议,如果您需要转载,请注明本站网址或者原文地址。任何问题请咨询:yoyou2525@163.com.