MiniVO: Minimalistic Range Enhanced Monocular System for Scale Correct Pose Estimation

Monocular vision systems allow compact and resource-constrained vehicles to perform online state estimation through Structure from Motion approaches. However, it is needed to integrate additional information to obtain a metric scale for the estimated trajectory. We propose a minimalistic approach based on the sensor fusion between a monocular camera and a 1D LiDAR (LIght Detection And Ranging) rangefinder. A classical visual front-end, in charge of tracking the camera pose with respect to a map of landmarks, is paired with an optimization back-end where depth measurements constrain the distance between cameras and 3D points. The fusion methodology is based on a keyframe-wise search of correspondences between visual landmarks and projections of altimeter points. The differences between triangulated landmark depths and associated altimeter measurements are minimized using the iSAM2 incremental smoother. We test our algorithm in a variety of datasets comprising an outdoor scenario with accurate D-GPS ground truth evaluating both tracking accuracy and computational effort. Results show that our algorithm provides comparable performances to scale-aware RGB-D vision systems and obtains relative trajectory errors close to 0.5%. Furthermore, we propose and validate an extrinsic calibration pipeline to refer range measurements in the camera reference system which differs from common LiDAR-camera calibration algorithms due to the 5 Degrees of Freedom (DoF) nature of the single-point rangefinder and camera system.