Improving the Iterative Closest Point Algorithm using Lie Algebra

TL;DR

This paper enhances the ICP algorithm by integrating a Lie algebra-based angular penalty term, improving registration accuracy with inertial data and reducing drift in point cloud mapping.

Contribution

Introduces a novel Lie algebra-based angular penalty term for ICP that eliminates parameter tuning and effectively incorporates IMU orientation data.

Findings

Consistent performance on real-world and KITTI datasets

Suppresses outlier effects from IMU measurements

Facilitates optimal fusion of geometric and inertial data

Abstract

Mapping algorithms that rely on registering point clouds inevitably suffer from local drift, both in localization and in the built map. Applications that require accurate maps, such as environmental monitoring, benefit from additional sensor modalities that reduce such drift. In our work, we target the family of mappers based on the Iterative Closest Point (ICP) algorithm which use additional orientation sources such as the Inertial Measurement Unit (IMU). We introduce a new angular penalty term derived from Lie algebra. Our formulation avoids the need for tuning arbitrary parameters. Orientation covariance is used instead, and the resulting error term fits into the ICP cost function minimization problem. Experiments performed on our own real-world data and on the KITTI dataset show consistent behavior while suppressing the effect of outlying IMU measurements. We further discuss promising experiments, which should lead to optimal combination of all error terms in the ICP cost function minimization problem, allowing us to smoothly combine the geometric and inertial information provided by robot sensors.