Speeding up updateOccupancy() function

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!

Answer 1

[Cameron Stabile]

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