Speeding up updateOccupancy() function

1 次查看（过去 30 天）
Josh
Josh 2024-7-2
回答： Cameron Stabile 2024-7-8
I am trying to write a Mapping module where I ingest LIDAR data from a simulated sensor and create a 2D occupancy map using the Update2DMap function:
function [map] = Update2DMap(map, pose, points, hitProbUpdate, passProbUpdate)
% The map is then updated based on the where the ray intersects the
% world
tic
sensorPos = [pose(1) pose(2)];
for i = 1 : size(points, 1)
point = points(i,:);
% Calculate points which a LIDAR ray passes through and the end
% point
[endPoints, midPoints] = raycast(map, sensorPos, point);
% Update the points in the map where the ray has passed through as
% unoccupied therefore reduce their occupancy value
updateOccupancy(map, midPoints, passProbUpdate, 'grid');
% End point indicates it has hit an obstacle therefore the
% occupancy values should increase
updateOccupancy(map, endPoints, hitProbUpdate, 'grid');
end
toc % Currently takes ~3 seconds to complete this loop
end
Where:
  • map - 2D occupancy map being updated
  • pose - pose vector (x, y, theta)
  • points - n x 2 position vector
  • hitProbUpdate - probabilty change when an obstacle is detected
  • passProbUpdate - probabilty change when an obstacle is not detected
I am currently processing a position vector which dimensins ~9000 x 3 and this function is my bottleneck where the tic/toc pair show to complete the for-loop takes around 2-3 seconds which is very slow when plotting.
I've tried using parfor loops and seeing if there are ways to use my GPU but neither produces any significant speed increase.
I'm relatively new to using the Navigation/Mapping Toolbox so I wanted to reach out to the community in case anyone has advice on how to speed up my updating function.
Many thanks!

请先登录，再回答此问题。

回答（1 个）

Cameron Stabile
Cameron Stabile 2024-7-8
Ran in:
Hi Josh,
My understanding is that you are trying to integrate a batch of rays into an occupancyMap and are looking to improve your performance. I believe insertRay might be the answer here. Based on your inputs, the following syntax should offer better performance and a more concise API:
% Problem setup
map = occupancyMap(1000,1000);
hitUpdate = 0.9;
freeUpdate = 0.2;
invModel = [freeUpdate hitUpdate];
% Integrate clouds
for i = 1:10
pose = [500 + (rand(1,2)-0.5)*100 rand*2*pi];
pts = (rand(9000,2)-.5)*100 + pose(1:2);
tic;
insertRay(map,pose(:,1:2),pts,invModel);
t = toc
show(map,FastUpdate=1);
end
t = 0.3503
t = 0.2982
t = 0.2970
t = 0.2933
t = 0.2963
t = 0.2832
t = 0.2839
t = 0.2827
t = 0.2814
t = 0.2857
Note that the overall performance of this code will depend on your machine, the length of rays, and the resolution of the map, but using insertRay to process the pointcloud in batches will likely help.
Best,
Cameron

类别

在 Help Center 和 File Exchange 中查找有关 Mapping 的更多信息

标签

产品


版本

R2024a

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by