Gravity-constrained point cloud registration

TL;DR

This paper introduces a gravity-constrained ICP-based front-end for lidar SLAM that leverages gravity measurements to improve pose estimation, reducing localization drift and aiding loop closure in large-scale environments.

Contribution

The novel front-end exploits gravity to constrain pose estimation in lidar SLAM, enhancing accuracy and supporting faster global map convergence.

Findings

Reduces localization drift by 30% compared to standard ICP

Supports loop closure identification in SLAM

Validated in large-scale outdoor and subterranean environments

Abstract

Visual and lidar Simultaneous Localization and Mapping (SLAM) algorithms benefit from the Inertial Measurement Unit (IMU) modality. The high-rate inertial data complement the other lower-rate modalities. Moreover, in the absence of constant acceleration, the gravity vector makes two attitude angles out of three observable in the global coordinate frame. In visual odometry, this is already being used to reduce the 6-Degrees Of Freedom (DOF) pose estimation problem to 4-DOF. In lidar SLAM, the gravity measurements are often used as a penalty in the back-end global map optimization to prevent map deformations. In this work, we propose an Iterative Closest Point (ICP)-based front-end which exploits the observable DOF and provides pose estimates aligned with the gravity vector. We believe that this front-end has the potential to support the loop closure identification, thus speeding up convergences of global map optimizations. The presented approach has been extensively tested in large-scale outdoor environments as well as in the Subterranean Challenge organized by Defense Advanced Research Projects Agency (DARPA). We show that it can reduce the localization drift by 30% when compared to the standard 6-DOF ICP. Moreover, the code is readily available to the community as a part of the libpointmatcher library.