Speeding up updateOccupancy() function
1 vue (au cours des 30 derniers jours)
Afficher commentaires plus anciens
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!
0 commentaires
Réponses (1)
Cameron Stabile
le 8 Juil 2024
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
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
0 commentaires
Voir également
Catégories
En savoir plus sur SLAM dans Help Center et File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!