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.

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 are:

*M*, is the point cloud defining the map of the environment.
*T*_{i}, is the point cloud defining the **i**'th LIDAR scan that will be matched with the map *M*.
*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 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*, *θ* )
*P*_{i}, is the current position of the mobile robot in the map *M* expressed as: *P*_{i} = ( *x*_{i}, *y*_{i}, *θ*_{i} )
*P*_{i-1}, is the initial guess of *P*_{i}, 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)
*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 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 (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)
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 outliers)
*tT*_{i}, is the current (i'th) LIDAR scan (*T*_{i}) expressed in the coordinates of *M* (transformed LIDAR scan)
Representation of example input (

*M*_{i-1},

*T*_{i} ) and output (

*tT*_{i},

*M*_{i} ) 2D point clouds:

Based on the **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 original

**LIBICP** 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®.

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 *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 mobile robot self-localization using a map *M* and its normals *N*, *P*_{i} = ( *x*_{i}, *y*_{i}, *θ*_{i} )
**>>** *P*_{i} = SLAMICP(

*M*,

*T*_{i}, inlier

_{threshold}, method, maxiter,

*P*_{i-1}, []);

% performing mobile robot self-localization, *P*_{i} = ( *x*_{i}, *y*_{i}, *θ*_{i} )
% 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} );

Download

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

↧ PCMerge V1.2 (additional performance function)

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®.

Example **MATLAB** calls

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

Download

07-12-2023: **PCMerge_MEX_V12.rar**

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

↧ ComputeNormals V1.2 (additional performance function)

The

**SLAMICP** function computes the transformation (

*P*_{i}) that aligns the input point cloud (

*T*_{i}) with a reference point cloud (

*M*).

A

**MATLAB** wrapper is provided as precompiled MEX file for Windows®.

Example **MATLAB** calls

% Call to match *T*_{i} with *M*_{i-1}
**>>** [

*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} );

% slower, ideal for SLAM (create *M*_{i} )
**>>** [

*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}, Outlier

_{threshold} );

**>>** [

*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}, Outlier

_{threshold} );

% faster
**>>** [

*P*_{i} ] = SLAMICP(

*M*,

*T*_{i}, inlier

_{threshold}, method, maxiter,

*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, maxiter,

*P*_{i-1} );

% Outlier_{threshold} = inlier_{threshold}
% 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} );

Download

16-05-2023: **SLAMICP_V41.rar** - First version published online ( *includes a precompiled ***MEX** file for **MATLAB** under **Windows**® )

Additional packages required to compile the **SLAMICP** and **LIBICP** library from scratch

**Boost libraries** - Used by the k-d tree search

**CMake** - Requiered to control the software compilation

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 Retrospective Analysis of Indoor CO2 Measurements Obtained with a Mobile Robot during the COVID-19 Pandemic**,
*Sensors* **2024**, *24*, 3102.
**Path Planning of a Mobile Delivery Robot Operating in a Multi-Story Building Based on a Predefined Navigation Tree**,
*Sensors* **2023**, *23*, 8795.

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

Application Examples

• Mobile robot trajectory obtained matching 2D LIDAR data with a 2D Map:

• Creation of a 2D Map with the

**SLAMICP** library

• Path Planning for a Multi-story mobile robot:

**https://youtu.be/IjQ5V-fAVek**
• Multi-story mobile robot navigation based on 2D LIDAR data and a 2D Map:

**https://youtu.be/q7XyZmrdGHk**

© Laboratori de Robòtica, Universitat de Lleida