function [np, ni, md, oli, ol, tT, uM, uN] = SLAMICP(M, T, inlierTh, method, maxIter, initialPos, Normals, outlierTh)
% SLAMICP Performs Scan Matching using ICP.
%
%   USAGE:
%     1. See full documentation: 
%        SLAMICP();
%
%     1. Compute Normals only:
%        N = SLAMICP(M)
%
%     2. Perform ICP:
%        [np, ni, md, oli, ol, tT, uM, uN] = SLAMICP(M, T, inlierTh, method, maxIter, initialPos, Normals, outlierTh)
%
%   INPUTS:
%     M                 : (N x 2) or (N x 3) Model point cloud (Reference).
%     T                 : (N x 2) or (N x 3) Template point cloud (To be aligned).
%     inlierTh          : Scalar. Max distance to consider a point an inlier (-1 = all).
%     method            : String. 'point_to_point' or 'point_to_plane'.
%     maxIter           : Scalar. Maximum number of iterations.
%     initialPos        : Initial guess vector.
%                           [2D]: [x, y, theta]
%                           [3D]: [x, y, z, Yaw, Pitch, Roll]
%                         Rotation Order: Z(Yaw) -> Y(Pitch) -> X(Roll).
%     Normals           : (N x Dim) Normals of M. Use [] if unknown or need to be recalculated.
%     outlierTh         : (Optional) Scalar. Threshold for detecting outliers in output.
%                           Defaults to 'inlierTh' if omitted.
%
%   OUTPUTS:
%     np  : Calculated Position [x, y, z, Yaw, Pitch, Roll].
%     ni  : Number of iterations performed.
%     md  : Mean distance of inliers.
%     oli : Indices of outliers.
%     ol  : Coordinates of outliers.
%     tT  : Transformed Template cloud.
%     uM  : Updated Map (Model + incorporated outliers).
%     uN  : Updated Normals from all points of M.
%
%   COMPILE:
%     To compile, run the "make.m" script.