The SLAMICP library performs Iterative Closest Point matching focused on mobile robot self-localization and map creation based on LIDAR data.

The Iterative Closest Point (ICP) is a matching technique used to determine the transformation matrix that minimizes the distance between point clouds.

The MATLAB wrapper (includded in the library) that calls the ICP matching procedure implemented in the SLAMIPC library is:

>> [ P_i, niterations, md, O_index, O_coords, tT_i, uM ] = SLAMICP( M, T_i, inlier_threshold, method, iterations, P_i-1, Outlier_threshold );

The input parameters are:

M, the point cloud describing the map of the application scenario or a reference LIDAR scan

T_i, the current (i'th) LIDAR scan

inlier_threshold, the threshold distance applied to the matched points to classify a point as an inlier (used by the ICP matching), default = 0.3 units

method, the ICP method: 'point_to_point' or 'point_to_plane'

iterations, maximum number of iterations during the ICP matching

P_i-1, the initial guess of P_i, usually the last known position and orientation of the mobile robot P_i-1 = ( x_i-1, y_i-1, θ_i-1 )

Outlier_threshold, the threshold distance applied to the matched points to classify a point as an outlier (not used in the ICP matching), default = 0.3 units

The parameters returned are:

P_i, the current position of the mobile robot in M expressed as: P_i = ( x_i, y_i, θ_i )

niterations, the number of iterations of the ICP matching

md, the mean inlier distance (computed from the distance between the matched (inlier) points)

O_index, the index of the points of T_i classified as outliers

O_coords, the coordinates of the outliers expressed in the coordinates of M (transformed outliers)

tT_i, the current (i'th) LIDAR scan (T_i) expressed (or transformed) in the coordinates of M

uM, the updated map combining M with (only) the outliers of tT_i (listed in O_coords)

Representation of example input ( M, T ) and output ( tT, uM ) 2D point clouds:


The SLAMICP library is based on the 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.
Copared with the LIBICP, the SLAMICP returns the number of iterations, the mean inlier distance, the outliers, the transformed point cloud and the updated map.

The original LIBICP is avialable 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.

16-05-2023: SLAMICP V4.1 Download - First version published online ( including a precompiled MEX file for MATLAB under Windows® )

16-05-2023: PCMerge V1.0 Download - First version published online ( including a precompiled MEX file for MATLAB under Windows® )

Github: pending...
FileExchange: pending...

---

Additional packages required to compile SLAMICP from scratch:

Boost libraries - Used by the k-d tree search

CMake - Requiered to control the software compilation

---

Previous versions:

24-04-2023: PCMerge V1.0 - Internal beta version

21-04-2023: SLAMICP V4.0 - Internal beta version

This code is published under the GNU General Public License.

% call to match T_i with M
>> [ P_i, ni, md, O_indx, O_coords, tT_i, uM ] = SLAMICP( M, T_i, inlier_threshold, method, iter, P_i-1, Outlier_threshold );   % slower, ideal for SLAM (create uM )
>> [ P_i, ni, md, O_indx, O_coords, tT_i ] = SLAMICP( M, T_i, inlier_threshold, method, iter, P_i-1, Outlier_threshold );
>> [ P_i, ni, md, O_indx, O_coords ] = SLAMICP( M, T_i, inlier_threshold, method, iter, P_i-1, Outlier_threshold );
>> [ P_i, ni, md ] = SLAMICP( M, T_i, inlier_threshold, method, iter, P_i-1, Outlier_threshold );
>> [ P_i, ni] = SLAMICP( M, T_i, inlier_threshold, method, iter, P_i-1, Outlier_threshold );   % faster
>> [ P_i ] = SLAMICP( M, T_i, inlier_threshold, method, iter, P_i-1, Outlier_threshold );      % ideal for mobile robot self-localization, P_i = ( x_i, y_i, θ_i )
>> [ P_i ] = SLAMICP( M, T_i, inlier_threshold, method, iter, P_i-1 );      % Outlier_threshold = inlier_threshold

% display the help and usage instructions
>> [ P_i ] = SLAMICP( );

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

% obtain the outliers of tT_i (transforming 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 (transforming T_i according to P_i ) and merge all points of tT_i (all are outliers because Outlier_threshold = 0 ) with M (no ICP iterations) to create uM
>> [ ~, ~, ~, O_index, O_coords, tT_i, uM ] = SLAMICP( M, T_i, 0, method, 0, P_i, 0 );

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


% create tT_i (transforming T_i using P_i = ( x_i, y_i, θ_i ) obtained with SLAMICP ) and merge with M
>> [ uM ] = PCMerge( M, T_i, P_i );

% create tT_i (transforming T_i using Tr_fit = ( R( θ ), t ) obtained with LIBICP ) and merge with M
>> [ uM ] = PCMerge( M, T_i, Tr_fit );

	Direct download of the video (elevator.mp4, 560MB)	more to come...

Eduard Clotet & Jordi Palacín

Main paper featuring the SLAMICP Library (please add citation if you use the library):

SLAMICP Library: Accelerating Obstacle Detection in Mobile Robot Navigation via Outlier Monitoring following ICP Localization, Sensors 2023, 23, 6841.

Other related papers:

A Procedure for Taking a Remotely Controlled Elevator with an Autonomous Mobile Robot Based on 2D LIDAR, Sensors 2023, 23, 6089.