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!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! 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! mapping, lidar, occupancy, speed MATLAB Answers — New Questions

​