  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.
