% Load example scans
load scanData.mat scanData

% Get the number of scans in the dataset
numScans = length(scanData);

% Initialize SLAMICP parameters
inlierTh = 3000;	        % Scalar. Max distance to consider a point an inlier (-1 = all).
method = 'point_to_plane';  % String. Matching strategy String 'point_to_point' or 'point_to_plane'.
maxIter = 3;                % Scalar. Maximum number of iterations.
initialPos = zeros(1, 6);   % Initial position guess vector. [x, y, z, Yaw, Pitch, Roll]
                            %   Rotation Order: Z(Yaw) -> Y(Pitch) -> X(Roll).
Normals = [];               % Normals of M. Use [] if unknown.
outlierTh = 30;             % Threshold for detecting outliers of M in output.

% Use the first scan as initial Map
M = scanData{1}.scan;

% initialize a variable for the scatter plot
scatterPlot = [];

% Main process loop
for scanID = 2:numScans
    % Load the next scan as Template
    T = scanData{scanID}.scan;

    % Run ICP to calculate the new position of the LIDAR on the Map, ep
    [initialPos, ~, ~, ~, ~, ~, M, Normals] = SLAMICP(M, T, inlierTh, method, maxIter, initialPos, Normals, outlierTh);

    % initialPos = [x, y, z, Yaw, Pitch, Roll]. Updated position of the
    %   robot, which is the initialPos of the robot for the next
    %   iteration.
    % M = NxnDims matrix. Updated Map (Model + incorporated outliers).
    % Normals = NxnDims matrix. Updated Normals from all points of M.

    disp(['Building map... (scan ', num2str(scanID),'/',num2str(numScans),')']);
end

% Create a figure and axis to display the result
figure
pc = pointCloud(M);
pcshow(pc);