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.
Example calls to the SLAMICP library using a MATLAB wrapper ( includded in the library as a precompiled MEX file for Windows®):
>> [ Pi ] = SLAMICP( M, Ti, inlierthreshold, method, maxiter, Pi-1, N, Outlierthreshold );
The parameters used in the calls are:
M, is the point cloud defining the map of the environment.
Ti, is the point cloud defining the i'th LIDAR scan that will be matched with the map M.
Mi, is the map updated with the information included in the scan Ti
Mi-1, is the map before incorporating the new information included in the scan Ti
inlierthreshold, is 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 valid ICP methods are: 'point_to_point' or 'point_to_plane' (faster if the normals N are provided)
maxiter, is the maximum number of iterations allowed during the ICP matching
P, is the current position and orientation of the mobile robot in the map M expressed as: P = ( x, y, θ )
Pi, is the current position of the mobile robot in the map M expressed as: Pi = ( xi, yi, θi )
Pi-1, is the initial guess of Pi, it describes the previous position and orientation of the mobile robot updated with the information of the encoders (if available)
N, are the normals of the map M (use N = [] if the normals are unknown or not used)
Ni, are the normals of the map Mi
Ni-1, are the normals of the map Mi-1 (use Ni-1 = [] if the normals are unknown or not used)
Outlierthreshold, is the threshold distance applied to the matched points to classify a point of tTi as an outlier (does not affect the ICP matching), default = 0.3 units
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)
Oindex, are the index of the points of Ti classified as outliers
Ocoords, are the coordinates of the outliers expressed in the coordinates of M (transformed outliers)
tTi, is the current (i'th) LIDAR scan (Ti) expressed in the coordinates of M (transformed LIDAR scan)
Representation of example input ( Mi-1, Ti ) and output ( tTi, Mi ) 2D point clouds:
Reference LIBICP
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.
Compared 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 SLAMICP function computes the transformation (Pi) that aligns the input point cloud (Ti) 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®.
Example MATLAB calls for method = 'point_to_plane'
% Precomputation of the normals N of a (already built) map M >>N = SLAMICP(M);
% Call to match Ti with Mi-1 and update the map Mi and the normals Ni, faster, ideal for Simultaneous Localization And Mapping (SLAM)
>> [ Pi, niter, md, Oindx, Ocoords, tTi, Mi, Ni ] = SLAMICP( Mi-1, Ti, inlierthreshold, method, maxiter, Pi-1, Ni-1, Outlierthreshold );
% Call to match Ti with Mi-1 and update the map Mi (slowest, the normals are computed on each call)
>> [ Pi, niter, md, Oindx, Ocoords, tTi, Mi ] = SLAMICP( Mi-1, Ti, inlierthreshold, method, maxiter, Pi-1, [], Outlierthreshold );
% Call to match Ti with M without updating the map (faster), ideal for self-localization
>> [ Pi, niter, md, Oindx, Ocoords, tTi ] = SLAMICP( M, Ti, inlierthreshold, method, maxiter, Pi-1, N, Outlierthreshold );
% Call to match Ti with M without updating the map (slower, the normals are computed on each call)
>> [ Pi, niter, md, Oindx, Ocoords, tTi ] = SLAMICP( M, Ti, inlierthreshold, method, maxiter, Pi-1, [], Outlierthreshold );
>>Pi = SLAMICP( M, Ti, inlierthreshold, method, maxiter, Pi-1, N); % fastest, ideal for mobile robot self-localization, Pi = ( xi, yi, θi ) >>Pi = SLAMICP( M, Ti, inlierthreshold, method, maxiter, Pi-1, []); % performing mobile robot self-localization, Pi = ( xi, yi, θi )
% Display the help and usage instructions
>> SLAMICP( );
% Obtain the outliers of tTi (transformed Ti according to Pi ), matching and merging only the outliers of tTi (listed in Ocoords ) with Mi-1 (no ICP iterations) to create Mi >> [ ~, ~, ~, Oindex, Ocoords, tTi, Mi ] = SLAMICP( Mi-1, Ti, inlierthreshold, method, 0, Pi, Outlierthreshold );
% Obtain the outliers of tTi (transformed Ti according to Pi )
>> [ ~, ~, ~, Oindex, Ocoords, tTi ] = SLAMICP( M, Ti, inlierthreshold, method, 0, Pi, Outlierthreshold );
% Obtain tTi (transformed Ti according to Pi ) and merge all points of tTi (all are outliers because Outlierthreshold = 0 ) with Mi-1 (no ICP iterations) to create Mi >> [ ~, ~, ~, Oindex, Ocoords, tTi, Mi ] = SLAMICP( Mi-1, Ti, 0, method, 0, Pi, 0 );
% Obtain tTi (transformed Ti using Pi )
>> [ ~, ~, ~, ~, ~, tTi ] = SLAMICP( M, Ti, 0, method, 0, Pi );
Download
21-12-2023: SLAMICP_V51.rar - Includes a precompiled MEX file for MATLAB under Windows®
The SLAMICP function computes the transformation (Pi) that aligns the input point cloud (Ti) with a reference point cloud (M).
A MATLAB wrapper is provided as precompiled MEX file for Windows®.
Example MATLAB calls
% Call to match Ti with Mi-1 >> [ Pi, niter, md, Oindx, Ocoords, tTi, Mi ] = SLAMICP( Mi-1, Ti, inlierthreshold, method, maxiter, Pi-1, Outlierthreshold ); % slower, ideal for SLAM (create Mi ) >> [ Pi, niter, md, Oindx, Ocoords, tTi ] = SLAMICP( M, Ti, inlierthreshold, method, maxiter, Pi-1, Outlierthreshold ); >> [ Pi, niter, md, Oindx, Ocoords ] = SLAMICP( M, Ti, inlierthreshold, method, maxiter, Pi-1, Outlierthreshold ); >> [ Pi, niter, md ] = SLAMICP( M, Ti, inlierthreshold, method, maxiter, Pi-1, Outlierthreshold ); >> [ Pi, niter] = SLAMICP( M, Ti, inlierthreshold, method, maxiter, Pi-1, Outlierthreshold ); % faster >> [ Pi ] = SLAMICP( M, Ti, inlierthreshold, method, maxiter, Pi-1, Outlierthreshold ); % ideal for mobile robot self-localization, Pi = ( xi, yi, θi ) >> [ Pi ] = SLAMICP( M, Ti, inlierthreshold, method, maxiter, Pi-1 ); % Outlierthreshold = inlierthreshold
% Display the help and usage instructions
>> SLAMICP( );
% Obtain the outliers of tTi (transformed Ti according to Pi ), matching and merging only the outliers of tTi (listed in Ocoords ) with Mi-1 (no ICP iterations) to create Mi >> [ ~, ~, ~, Oindex, Ocoords, tTi, Mi ] = SLAMICP( Mi-1, Ti, inlierthreshold, method, 0, Pi, Outlierthreshold );
% Obtain the outliers of tTi (transformed Ti according to Pi )
>> [ ~, ~, ~, Oindex, Ocoords, tTi ] = SLAMICP( M, Ti, inlierthreshold, method, 0, Pi, Outlierthreshold );
% Obtain tTi (transformed Ti according to Pi ) and merge all points of tTi (all are outliers because Outlierthreshold = 0 ) with Mi-1 (no ICP iterations) to create Mi >> [ ~, ~, ~, Oindex, Ocoords, tTi, Mi ] = SLAMICP( Mi-1, Ti, 0, method, 0, Pi, 0 );
% Obtain tTi (transformed Ti using Pi )
>> [ ~, ~, ~, ~, ~, tTi ] = SLAMICP( M, Ti, 0, method, 0, Pi );
Download
16-05-2023: SLAMICP_V41.rar - First version published online ( includes a precompiled MEX file for MATLAB under Windows® )
The PCMerge function applies a transformation to the current LIDAR scan (Ti) and merges the result with the reference point cloud (Mi-1).
A MATLAB wrapper is provided as precompiled MEX file for Windows®.
Example MATLAB calls
% Create an updated map Mi : 1) transform Ti using Pi = ( xi, yi, θi ) obtained with SLAMICP and 2) merge the result with the original map Mi-1 >> [ Mi, tTi ] = PCMerge( Mi-1, Ti, Pi ); >>Mi = PCMerge( Mi-1, Ti, Pi );
% Create an updated map Mi : 1) transform Ti using Trfit = ( R( θ ), t ) obtained with LIBICP and 2) merge the result with the original map Mi-1 >> [ Mi, tTi ] = PCMerge( Mi-1, Ti, Trfit ); >>Mi = PCMerge( Mi-1, Ti, Trfit );
The ComputeNormals function computes the normals of a point cloud.
