main.m

% SLAM Algorithm assignment - Stephen Nwankwo - 2018/242916


% Load Laser Scan Data from File
load('offlineSlamData.mat');
% -------------------------------------


% Run SLAM Algorithm, Construct Optimized Map and Plot Trajectory of the Robot
maxLidarRange = 8;
mapResolution = 20;
slamAlg = lidarSLAM(mapResolution, maxLidarRange);
slamAlg.LoopClosureThreshold = 210;  
slamAlg.LoopClosureSearchRadius = 8;
% -------------------------------------

% Observe the Map Building Process with Initial 10 Scans
for i=1:10
    [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i});
    if isScanAccepted
        fprintf('Added scan %d \n', i);
    end
end
figure;
show(slamAlg);
title({'Map of the Environment','Pose Graph for Initial 10 Scans'});
% -------------------------------------

% Observe the Effect of Loop Closures and the Optimization Process
firstTimeLCDetected = false;
figure;
for i=10:length(scans)
    [isScanAccepted, loopClosureInfo, optimizationInfo] = addScan(slamAlg, scans{i});
    if ~isScanAccepted
        continue;
    end
    % visualize the first detected loop closure, if you want to see the
    % complete map building process, remove the if condition below
    if optimizationInfo.IsPerformed && ~firstTimeLCDetected
        show(slamAlg, 'Poses', 'off');
        hold on;
        show(slamAlg.PoseGraph); 
        hold off;
        firstTimeLCDetected = true;
        drawnow
    end
end
title('First loop closure');
% -------------------------------------

% Visualize the Constructed Map and Trajectory of the Robot
figure
show(slamAlg);
title({'Final Built Map of the Environment', 'Trajectory of the Robot'});
% -------------------------------------

% Build Occupancy Grid Map
[scans, optimizedPoses]  = scansAndPoses(slamAlg);
map = buildMap(scans, optimizedPoses, mapResolution, maxLidarRange);
figure; 
show(map);
hold on
show(slamAlg.PoseGraph, 'IDs', 'off');
hold off
title('Occupancy Grid Map Built Using Lidar SLAM');
% -------------------------------------

% Copyright 2019 The MathWorks, Inc.