% 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 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.

% Create a figure and axis to display the progress
figure('Color','k');
mainAx = axes();

% Use the first scan as Model PC (M)
M = scanData{1}.scan;

% Visualize the PC
scatterPlot = showMap([], M, [], mainAx);

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

    % Run ICP to calculate the new position of the LIDAR on the Map, ep
    [initialPos, ~, ~, outliersIndx, ~, ~, 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.
    % outliersIndx = int32 vector. Contains the index of the outilers in T.
    % M = NxnDims matrix. Updated Map (Model + incorporated outliers).
    % Normals = NxnDims matrix. Updated Normals from all points of M.

    % Update the map building PC
    scatterPlot = showMap(scatterPlot, M, outliersIndx, mainAx);
end



function sp = showMap(sp, xyz, oi, ax)
    numPoints = length(xyz);

    % Get the limits of the map
    maxX = max(xyz(:,1));
    minX = min(xyz(:,1));

    maxY = max(xyz(:,2));
    minY = min(xyz(:,2));
    
    maxZ = max(xyz(:,3));
    minZ = min(xyz(:,3));

    % Adjust color gradient and size of all points in M
    cData = zeros(numPoints,3);
    cData(:,1) = (xyz(:,1)-minX)/(maxX-minX);
    cData(:,2) = (xyz(:,2)-minY)/(maxY-minY);
    cData(:,3) = (xyz(:,3)-minZ)/(maxZ-minZ);
    
    
    sizeData = ones(1,numPoints)*1;

    % Higlight new points in M using larger, red dots
    if ~isempty(oi)
        numOutliers = length(oi);
        addedPointsIndx = numPoints-numOutliers;

        cData(addedPointsIndx:end,1) = 1;
        cData(addedPointsIndx:end,2) = 0;
        cData(addedPointsIndx:end,3) = 0;
        sizeData(addedPointsIndx:end) = 10;
    end
    
    % If the plot is not initialized, initialize it. Otherwise, update de
    % data
    if isempty(sp)
        sp = scatter3(ax, xyz(:,1),xyz(:,2),xyz(:,3),5,'filled');
        ax.Color = 'k';
        ax.XColor = 'w';ax.YColor = 'w'; ax.ZColor = 'w';
        xlabel(ax, 'X (mm)'); ylabel(ax, 'Y (mm)'); zlabel(ax, 'Z (mm)');
        axis(ax, 'equal');
    else
        sp.XData = xyz(:,1); sp.YData = xyz(:,2); sp.ZData = xyz(:,3);  
    end

    % Update color and size of points in scatter
    sp.CData = cData;
    sp.SizeData = sizeData;

    % Process all GUI callbacks
    drawnow;
end
