Scan Matching-based 2D Lidar Odometry
A simple scan matching-based 2D lidar odometry ROS node written as my EL5000 Advanced Mathematics course final project. The scan matching is solved using the iterative closest point (ICP) algorithm, implemented in C++ using Eigen.
Date: 2020
Role: Everything
Keywords: Lidar Odometry, Scan Matching, Iterative Closest Point
- Programming Languages: C++, Octave
- Libraries: Eigen
- Tools and Frameworks: ROS
- Algorithms: Iterative Closest Point (ICP)
The scan matching-based 2D lidar odometry ROS node was written as the final project for my EL5000 Advanced Mathematics course in 2020. The ROS node is written in C++ using the Eigen library and implements the iterative closest point (ICP) algorithm. As a mathematics course final project, this project is written mainly to demonstrate how singular value decomposition (SVD) can be used to solve the point cloud alignment problem and is not meant to be an implementation suitable for real-world use. A proof of concept implementation of the algorithm is implemented in Octave and illustrated below.
The correspondences between the two point clouds are made using naive nearest neighbor searches. All correspondence pairs are weighted equally and no outlier rejection is performed. The translation vector between the point clouds is obtained by calculating the position difference between the centroids of both point clouds. The rotation matrix is extracted using SVD. To accelerate computations, all input point clouds are subsampled beforehand.
The ICP C++ implementation is then wrapped in a ROS node which subscribes to an input 2D laser scan topic and publishes the resulting calculated odometry. The implementation is tested on a set of sample 2D lidar data taken in Gazebo and is evaluated against the ground truth trajectory using the evo odometry evaluation package. The following figures show the estimated and ground truth trajectories as well as the position and orientation comparison at every timestep.
