The SLAMICP library is a Free and Open Source library that performs Iterative Closest Point matching for mobile robot Self-Localization and Map creation based on LiDAR data.

The general assumption is that the position of a mobile robot (or a mobile platform) in an given environment is defined by the position of the LiDAR.

The Iterative Closest Point (ICP) is a matching technique used to determine the transformation matrix that minimizes the distance between two point clouds: a Map (M) and a LiDAR scan (T).

Example calls to the SLAMICP library using a MATLAB wrapper ( includded in the library as a precompiled MEX file for Windows®):

>> [ P_i, niter, md, O_indx, O_coords, tT_i, M_i, N_i ] = SLAMICP( M_i-1, T_i, inlier_threshold, method, maxiter, P_i-1, N_i-1, Outlier_threshold );

>> [ P_i ] = SLAMICP( M, T_i, inlier_threshold, method, maxiter, P_i-1, N, Outlier_threshold );

The parameters used in the calls must be all defined as 'double' and are:

• M, is the point cloud defining the map of the environment ( recomendation: avoid points with (x = 0) & (y = 0) & (z = 0) in M ).
  • 2D-Map: M must be a k x 2 matrix = [ x₁ y₁; x₂ y₂; ...; x_k y_k ]
  • 3D-Map: M must be a k x 3 matrix = [ x₁ y₁ z₁; x₂ y₂ z₂; ...; x_k y_k z_k ]
• T_i, is the point cloud defining the i'th LIDAR scan that will be matched with the map M ( recomendation: avoid points with (x = 0) & (y = 0) & (z = 0) in T_i ).
  • 2D-Map: T_i must be a q x 2 matrix = [ x₁ y₁; x₂ y₂; ...; x_q y_q ]
  • 3D-Map: T_i must be a q x 3 matrix = [ x₁ y₁ z₁; x₂ y₂ z₂; ...; x_k y_q z_q ]


• M_i, is the map updated with the information included in the scan T_i
• M_i-1, is the map before incorporating the new information included in the scan T_i


• inlier_threshold, is the threshold distance applied to the matched points to classify a point as an inlier (used by the ICP matching), default = 0.3 (ideal when units are in meters).
• method, the valid ICP methods are: 'point_to_point' or 'point_to_plane' (much faster if the normals N of the map are also provided in the call)
• maxiter, is the maximum number of iterations allowed during the ICP matching


• P, is the current position and orientation of the LiDAR in the map M
  • 2D-Map: P must be a 3 x 1 matrix = [ x y θ ]
  • 3D-Map: P must be a 6 x 1 matrix = [ x y z yaw pitch roll ]


• P_i, is the current position of the LiDAR in the map M estimated from T_i
• P_i-1, is the initial guess of P_i (previous position of the LiDAR in the map), if available, it can include an estimattion of the motion of the LiDAR gathered from platform encoders or from IMU.


• N, are the normals of the map M (use N = [ ] if the normals are unknown or not used)
• N_i, are the normals of the map M_i
• N_i-1, are the normals of the map M_i-1 (use N_i-1 = [ ] if the normals are initially unknown or not used)


• Outlier_threshold, is the threshold distance applied to the matched points to classify a point of tT_i as an outlier (this parameter does not affect the ICP matching), default = 0.3 (ideal when units are in meters)
• niter, is the number of iterations performed during ICP matching
• md, is the mean inlier distance (computed from the distance between the matched (inlier) points)


• O_index, are the index of the points of T_i classified as outliers
• O_coords, are the coordinates of the outliers expressed in the coordinates of M (transformed version of the outliers to match with the map M)


• tT_i, is the current (i'th) LiDAR scan (T_i) expressed in the coordinates of M (transformed version of the LiDAR scan T_i to match with the map M)

Representation of example input ( M_i-1, T_i ) and output ( tT_i, M_i ) 2D point clouds:


The SLAMICP library is based on LIBICP (LIBrary for Iterative Closest Point fitting) which is a cross-platfrom C++ library with MATLAB wrappers for fitting 2d or 3d point clouds with respect to each other. Currently it implements the SVD-based point-to-point algorithm as well as the linearized point-to-plane algorithm. It also supports outlier rejection and is accelerated by the use of k-d trees as well as a coarse matching stage using only a subset of all points.
Compared with LIBICP, the SLAMICP library returns the number of iterations, the mean inlier distance, the outliers, the transformed point cloud and the updated map.

The original LIBICP library is available in: https://www.cvlibs.net/software/libicp/

When compiled, the MATLAB wrapper of the LIBICP library can be called using:

>> [ Tr_fit ] = icpMEX( M, T_i, Tr_{initial_guess}, inlier_threshold, method );

Where Tr = ( R( θ ), t ) is the transformation matrix that must be applied to T to match M.

The SLAMICP function computes the transformation (P_i) that aligns the input point cloud (T_i) with a reference point cloud (M).
This version is faster when using the method 'point_to_plane' when the normals (N) are precomputed.
A MATLAB wrapper is provided as precompiled MEX file for Windows®.

% Precomputation of the normals N of a (already built) map M
>> N = SLAMICP(M);

% Call to match T_i with M_i-1 and update the map M_i and the normals N_i, faster, ideal for Simultaneous Localization And Mapping (SLAM)
>> [ P_i, niter, md, O_indx, O_coords, tT_i, M_i, N_i ] = SLAMICP( M_i-1, T_i, inlier_threshold, method, maxiter, P_i-1, N_i-1, Outlier_threshold );

% Call to match T_i with M_i-1 and update the map M_i (slowest, the normals are computed on each call)
>> [ P_i, niter, md, O_indx, O_coords, tT_i, M_i ] = SLAMICP( M_i-1, T_i, inlier_threshold, method, maxiter, P_i-1, [ ], Outlier_threshold );

% Call to match T_i with M without updating the map (faster), ideal for self-localization
>> [ P_i, niter, md, O_indx, O_coords, tT_i ] = SLAMICP( M, T_i, inlier_threshold, method, maxiter, P_i-1, N, Outlier_threshold );

% Call to match T_i with M without updating the map (slower, the normals are computed on each call)
>> [ P_i, niter, md, O_indx, O_coords, tT_i ] = SLAMICP( M, T_i, inlier_threshold, method, maxiter, P_i-1, [ ], Outlier_threshold );

>> [ P_i, niter, md, O_indx, O_coords ] = SLAMICP( M, T_i, inlier_threshold, method, maxiter, P_i-1, N, Outlier_threshold );   % faster
>> [ P_i, niter, md, O_indx, O_coords ] = SLAMICP( M, T_i, inlier_threshold, method, maxiter, P_i-1, [ ], Outlier_threshold );

>> [ P_i, niter, md ] = SLAMICP( M, T_i, inlier_threshold, method, maxiter, P_i-1, N, Outlier_threshold );   % faster
>> [ P_i, niter, md ] = SLAMICP( M, T_i, inlier_threshold, method, maxiter, P_i-1, [ ], Outlier_threshold );

>> [ P_i, niter ] = SLAMICP( M, T_i, inlier_threshold, method, maxiter, P_i-1, N);   % faster
>> [ P_i, niter ] = SLAMICP( M, T_i, inlier_threshold, method, maxiter, P_i-1, [ ]);

>> P_i = SLAMICP( M, T_i, inlier_threshold, method, maxiter, P_i-1, N);      % fastest, ideal for LiDAR or mobile robot self-localization using a map M and its normals N
>> P_i = SLAMICP( M, T_i, inlier_threshold, method, maxiter, P_i-1, [ ]);      % performing LiDAR or mobile robot self-localization

% Display the help and usage instructions
>> SLAMICP( );

% Obtain the outliers of tT_i (transformed T_i according to P_i ), matching and merging only the outliers of tT_i (listed in O_coords ) with M_i-1 (no ICP iterations) to create M_i
>> [ ~, ~, ~, O_index, O_coords, tT_i, M_i ] = SLAMICP( M_i-1, T_i, inlier_threshold, method, 0, P_i, Outlier_threshold );

% Obtain the outliers of tT_i (transformed T_i according to P_i )
>> [ ~, ~, ~, O_index, O_coords, tT_i ] = SLAMICP( M, T_i, inlier_threshold, method, 0, P_i, Outlier_threshold );

% Obtain tT_i (transformed T_i according to P_i ) and merge all points of tT_i (all are outliers because Outlier_threshold = 0 ) with M_i-1 (no ICP iterations) to create M_i
>> [ ~, ~, ~, O_index, O_coords, tT_i, M_i ] = SLAMICP( M_i-1, T_i, 0, method, 0, P_i, 0 );

% Obtain tT_i (transformed T_i using P_i )
>> [ ~, ~, ~, ~, ~, tT_i ] = SLAMICP( M, T_i, 0, method, 0, P_i );

09-02-2026: SLAMICP_V53.rar - Includes a precompiled MEX file for MATLAB under Windows®

21-12-2023: SLAMICP_V51.rar - Includes a precompiled MEX file for MATLAB under Windows®

This example loads some raw scans gathered from a UTM-30LX 2D LiDAR during a small displacement, match the scans, and creates a 2D Map of the visited area.

% Load example data
load scans_2D iteration

% Get number of scans
numIterations = length(iteration);

% Initialize SLAMICP parameters
inlierTh = 0.3; % Scalar. Max distance to consider a point an inlier (-1 = all)
initialPos = [0,0,0]; % Initial position guess vector. [x, y, yaw]
method = 'point_to_plane'; % String. Matching strategy String 'point_to_point' or 'point_to_plane'
maxIter = 20; % Scalar. Maximum number of iterations
Normals = [ ]; % Normals of M. Use [ ] if unknown
outlierTh = 0.1; % Threshold for detecting outliers of M in output

% Initialize M
M = iteration{1}.scan;

% Initialize plots
figure();
map1Ax = axes;

% Initialize empty objects
mapObj = [ ];

% Main loop
for itID = 2:1:numIterations
     % Get a new scan
     T = iteration{itID}.scan;

     % Run SLAMPIC
     [initialPos, ~, ~, ~, outliersIndx, ~, M, Normals] = SLAMICP(M, T, inlierTh, method, maxIter, initialPos, Normals, outlierTh);

     % Update map
     mapObj = updateMap(map1Ax, mapObj, M, outliersIndx);

     % Process GUI callbacks
     drawnow;

     % Show progress
     disp(['Iteration ',num2str(itID),'/',num2str(numIterations)]);
end

function mapObj = updateMap(mainAx, mapObj, points, ol)
   if isempty(mapObj)
     mapObj.map = plot(mainAx, points(:,1), points(:,2),'b.');
     hold(mainAx,'on');

     if isempty(ol)
       mapObj.ol = plot(mainAx, nan, nan,'r.');
     else
       mapObj.ol = plot(mainAx, ol(:,1), ol(:,2),'r.', 'MarkerSize',30);
     end

     hold(mainAx,'off');
     xlabel(mainAx,'x (m)');
     ylabel(mainAx,'y (m)');
     axis(mainAx, 'equal');
     grid(mainAx,'on')
     grid('minor')
   else
     mapObj.map.XData = points(:,1);
     mapObj.map.YData = points(:,2);

     if isempty(ol)
       mapObj.ol.XData = nan;
       mapObj.ol.YData = nan;
     else
       mapObj.ol.XData = ol(:,1);
       mapObj.ol.YData = ol(:,2);
     end
   end
end

03-02-2026: example2d.rar (the map is updated live during the matching)

Figures

This example loads some raw scans gathered from a Mid-360 3D LiDAR during a small displacement, match the scans, and creates a 3D Map of the room.

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

% 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);

03-02-2026: example1.rar (the map is updated at the end of the matching)

03-02-2026: example2_live_update.rar (the map is updated live during the matching)

Figures

Boost libraries - Used by the k-d tree search

CMake - Requiered to control the software compilation

Free and Open Source. The code of the SLAMICP library is published under the GNU General Public License.

The PCMerge function applies a transformation to the current LIDAR scan (T_i) and merges the result with the reference point cloud (M_i-1).
A MATLAB wrapper is provided as precompiled MEX file for Windows®.

% Create an updated map M_i : 1) transform T_i using P_i = ( x_i, y_i, θ_i ) obtained with SLAMICP and 2) merge the result with the original map M_i-1
>> [ M_i, tT_i ] = PCMerge( M_i-1, T_i, P_i );
>> M_i = PCMerge( M_i-1, T_i, P_i );

% Create an updated map M_i : 1) transform T_i using Tr_fit = ( R( θ ), t ) obtained with LIBICP and 2) merge the result with the original map M_i-1
>> [ M_i, tT_i ] = PCMerge( M_i-1, T_i, Tr_fit );
>> M_i = PCMerge( M_i-1, T_i, Tr_fit );

07-12-2023: PCMerge_MEX_V12.rar

16-05-2023: PCMerge_MEX_V11.rar - First version published online

The ComputeNormals function computes the normals of a point cloud.
A MATLAB wrapper is provided as precompiled MEX file for Windows®.

% Optimized computation of the normals N (red arrows) of a map M (blue dots)
>> N = ComputeNormals(M, Neighbours);
>> N = ComputeNormals(M);   % Neighbours = 10 by default

Download ComputeNormals

07-12-2023: ComputeNormals_V12.rar

29-05-2023: ComputeNormals_V10.rar - First version published online