mp2p_icp repository

Repository Summary

Checkout URI
VCS Type git
VCS Version master
Last Updated 2023-11-30
CI status No Continuous Integration
Tags No category tags.
Contributing Help Wanted (0)
Good First Issues (0)
Pull Requests to Review (0)


Name Version
mp2p_icp 0.2.2


CircleCI Docs

Distro Build dev Build releases Stable version
ROS 1 Noetic (u20.04) (ROS 1 obsolete) Build Status Version
ROS 2 Humble (u22.04) (See root MOLA) Build Status Version
ROS 2 Iron (u22.04) (See root MOLA) Build Status Version
ROS 2 Rolling (u22.04) (See root MOLA) Build Status Version


A repertory of multi primitive-to-primitive (MP2P) ICP algorithms in C++.

License: New BSD 3-Clause

Docs: - Main library documentation - C++ API reference

The OLAE-ICP method is described in this technical report:

Jose-Luis Blanco-Claraco. "OLAE-ICP: Robust and fast alignment of geometric
features with the optimal linear attitude estimator", Arxiv 2019.



The project provides these C++ libraries: * mp2p_icp_map: Provides the mp2p_icp::metric_map_t generic metric map container. * mp2p_icp: With ICP algorithms. It depends on mp2p_icp_map. * mp2p_icp_filters: With point cloud filtering and manipulation algorithms. It depends on mp2p_icp_map.

This project provides: * mp2p_icp::metric_map_t: A generic data type to store raw or processed point clouds, e.g. segmented, discrete extracted features. Note that filtering point clouds is intentionally left outside of the scope of this library. See MOLA for possible implementations. * mp2p_icp::ICP_Base: A uniform API for matching those generic point clouds. * Implementations/wrappers of different ICP algorithms under such uniform API. * The library exposes both, complete iterative ICP algorithms, and the underlying optimal transformation estimators which are run at each ICP iteration.

mp2p_icp is used in the MOLA lidar-inertial odometry system.

Implemented Optimal Transformation methods

  • optimal_tf_olae(): A novel algorithm that can recover the optimal attitude from a set of point-to-point, line-to-line, and plane-to-plane pairings.
  • optimal_tf_horn(): Classic Horn's closed-form optimal quaternion solution. Relies on the implementation in <mrpt/tfest/se3.h>.
  • optimal_gauss_newton(): Simple non-linear optimizer to find the SE(3) optimal transformation for these pairings: point-to-point, point-to-plane.

Implemented ICP methods

  • ICP_OLAE: ICP for point clouds, planes, and lines. Uses optimal_tf_olae().
  • ICP_Horn_MultiCloud: Align point clouds layers, using classic Horn's closed-form optimal quaternion solution.



  • A C++17 compiler. Tested with gcc-7, MSVC 2017.
  • Eigen3
  • CMake >= 3.4
  • MRPT >=2.11.3 (If you need to use an older version of MRPT, use mp2p_icp<=0.2.2)

Install all the dependencies in Ubuntu with:

# MRPT >=2.4.0, for now from this PPA (or build from sources if preferred):
sudo add-apt-repository ppa:joseluisblancoc/mrpt
sudo apt update
sudo apt install libmrpt-dev

# Rest of dependencies:
sudo apt install build-essential cmake libeigen3-dev


cmake -H. -Bbuild
cd build
cmake --build .   # or make

Run the tests

make test

Run the demos

# 2D icp with point-to-point pairings only:
build/bin/mp2p-icp-run \
  --input-local demos/ \
  --input-global demos/ \
  -c demos/icp-settings-2d-lidar-example-point2point.yaml \

# Inspect the debug log:

# 2D icp with point-to-line pairings:
build/bin/mp2p-icp-run \
  --input-local demos/ \
  --input-global demos/ \
  -c demos/icp-settings-2d-lidar-example-point2line.yaml \

# Inspect the debug log:

# 3D icp with external library wrapper
build/bin/mp2p-icp-run \
  --input-local demos/ \
  --input-global demos/ \
  -c demos/icp-settings-example-libpointmatcher.yaml \

# Inspect the debug log:


No found.