  This paper presents a visual-inertial odometry-enhanced geometrically stable
Iterative Closest Point (ICP) algorithm for accurate mapping using aerial
robots. The proposed method employs a visual-inertial odometry framework in
order to provide robust priors to the ICP step and calculate the overlap among
point clouds derived from an onboard time-of-flight depth sensor. Within the
overlapping parts of the point clouds, the method samples points such that the
distribution of normals among them is as large as possible. As different
geometries and sensor trajectories will influence the performance of the
alignment process, evaluation of the expected geometric stability of the ICP
step is conducted. It is only when this test is successful that the matching,
outlier rejection, and minimization of the error metric ICP steps are conducted
and the new relative translation and rotational components are estimated,
otherwise the system relies on the visual-inertial odometry transformation
estimates. The proposed strategy was evaluated within handheld, automated and
fully autonomous exploration and mapping missions using a small aerial robot
and was shown to provide robust results of superior quality at an affordable
increase of the computational load.
