Universitat de Lleida: Laboratori de Robòtica
Inicio   Equipo    Publicaciones    TFG/TFM/Tesis    Proyectos    SLAMICP    Contacto
SLAMICP Library ( Free and Open Source )
Description of the SLAMICP library
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.

    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:

    >> [ Trfit ] = icpMEX( M, Ti, Trinitial_guess, inlierthreshold, method );

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

    Downloadable packages for Linux/Mac/Windows®

  • 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
  • License
    This code is published under the GNU General Public License.

    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):
  • 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.

  • SLAMICP Library
    Inicio   Equipo    Publicaciones    TFG/TFM/Tesis    Proyectos    SLAMICP    Contacto

    © Laboratori de Robòtica, Universitat de Lleida