Scale Correct Monocular Visual Odometry Using a LiDAR Altimeter

Abstract

The inherent scale ambiguity in monocular vision is a well known issue that forces the integration of other sensory sources to obtain metric references. However, 2D or 3D LiDARs and RGB-D sensors, while guaranteeing metrological accuracy, impose a non negligible burden both in terms of computational load and power requirements limiting the feasibility of being implemented on small exploration vehicles. This paper presents a scale aware monocular Visual Odometry framework that fuses range data from a laser altimeter in order to recover and maintain a correct metric scale. The proposed Visual Odometry method consists of a keyframe based tracking and mapping algorithm using optical flow where range data serves as a scale constraint on a keyframe to keyframe basis. An optimization backend based on iSAM2 is employed in order to refine the trajectory and map estimates eliminating the scale drift without the need of performing loop closures. We demonstrate that our algorithm can obtain very similar performances to state of the art stereo visual SLAM and RGB-D methods.