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:
>> [ Pi, niterations, md, Oindex, Ocoords, tTi, uM ] = SLAMICP( M, Ti, inlierthreshold, method, iterations, Pi-1, Outlierthreshold );
The input parameters are:
M, the point cloud describing the map of the application scenario or a reference LIDAR scan
Ti, the current (i'th) LIDAR scan
inlierthreshold, 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
Pi-1, the initial guess of Pi, usually the last known position and orientation of the mobile robot Pi-1 = ( xi-1, yi-1, θi-1 )
Outlierthreshold, 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:
Pi, the current position of the mobile robot in M expressed as: Pi = ( xi, yi, θi )
niterations, the number of iterations of the ICP matching
md, the mean inlier distance (computed from the distance between the matched (inlier) points)
Oindex, the index of the points of Ti classified as outliers
Ocoords, the coordinates of the outliers expressed in the coordinates of M (transformed outliers)
tTi, the current (i'th) LIDAR scan (Ti) expressed (or transformed) in the coordinates of M
uM, the updated map combining M with (only) the outliers of tTi (listed in Ocoords)
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.
Example calls to the MATLAB wrapper functions (provided as precompiled MEX files for Windows®) with 2D point clouds
% call to match Ti with M >> [ Pi, ni, md, Oindx, Ocoords, tTi, uM ] = SLAMICP( M, Ti, inlierthreshold, method, iter, Pi-1, Outlierthreshold ); % slower, ideal for SLAM (create uM ) >> [ Pi, ni, md, Oindx, Ocoords, tTi ] = SLAMICP( M, Ti, inlierthreshold, method, iter, Pi-1, Outlierthreshold ); >> [ Pi, ni, md, Oindx, Ocoords ] = SLAMICP( M, Ti, inlierthreshold, method, iter, Pi-1, Outlierthreshold ); >> [ Pi, ni, md ] = SLAMICP( M, Ti, inlierthreshold, method, iter, Pi-1, Outlierthreshold ); >> [ Pi, ni] = SLAMICP( M, Ti, inlierthreshold, method, iter, Pi-1, Outlierthreshold ); % faster >> [ Pi ] = SLAMICP( M, Ti, inlierthreshold, method, iter, Pi-1, Outlierthreshold ); % ideal for mobile robot self-localization, Pi = ( xi, yi, θi ) >> [ Pi ] = SLAMICP( M, Ti, inlierthreshold, method, iter, Pi-1 ); % Outlierthreshold = inlierthreshold
% display the help and usage instructions >> [ Pi ] = SLAMICP( );
% obtain the outliers of tTi (transforming Ti according to Pi ), matching and merging only the outliers of tTi (listed in Ocoords ) with M (no ICP iterations) to create uM >> [ ~, ~, ~, Oindex, Ocoords, tTi, uM ] = SLAMICP( M, Ti, inlierthreshold, method, 0, Pi, Outlierthreshold );
% obtain the outliers of tTi (transforming Ti according to Pi ) >> [ ~, ~, ~, Oindex, Ocoords, tTi] = SLAMICP( M, Ti, inlierthreshold, method, 0, Pi, Outlierthreshold );
% obtain tTi (transforming Ti according to Pi ) and merge all points of tTi (all are outliers because Outlierthreshold = 0 ) with M (no ICP iterations) to create uM >> [ ~, ~, ~, Oindex, Ocoords, tTi, uM ] = SLAMICP( M, Ti, 0, method, 0, Pi, 0 );
% obtain tTi (transforming Ti using Pi ) >> [ ~, ~, ~, ~, ~, tTi ] = SLAMICP( M, Ti, 0, method, 0, Pi );
% create tTi (transforming Ti using Pi = ( xi, yi, θi ) obtained with SLAMICP ) and merge with M >> [ uM ] = PCMerge( M, Ti, Pi );
% create tTi (transforming Ti using Trfit = ( R( θ ), t ) obtained with LIBICP ) and merge with M >> [ uM ] = PCMerge( M, Ti, Trfit );
Application Example: Mobile robot trajectory obtained from 2D LIDAR data matched with a 2D Map
Images and videos of applications of the SLAMICP library
Direct download of the video (elevator.mp4, 560MB)
more to come...
Authors
Eduard Clotet & Jordi Palacín
Related papers
Main paper featuring the SLAMICP Library (please add citation if you use the library):