Improving the Iterative Closest Point Algorithm using Lie Algebra
This work addresses drift reduction in mapping for applications like environmental monitoring, but it is incremental as it builds on existing ICP-based methods with IMU integration.
The paper tackles local drift in point cloud registration for mapping by introducing a new angular penalty term derived from Lie algebra into the ICP algorithm, which avoids arbitrary parameter tuning and suppresses outlier IMU measurements, as shown in experiments on real-world and KITTI datasets.
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.