  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.
