Metric scale and angle estimation in monocular visual odometry with multiple distance sensors

Abstract

In this paper, a novel approach is presented to estimate the metric scale (MSC) and roll and pitch angles of a platform by using distance sensors in a monocular visual odometry setup. A state-of-the-art visual odometry algorithm Semi-Direct Visual Odometry (SVO) [1] is used to obtain sparse three dimensional (3D) point cloud which is then matched with the measurements obtained from the distance sensors for the estimation process. Metric scale with Kalman (MSCwK) filter approach is presented where the metric scale parameter is modeled as a Gaussian random variable and updated with a Kalman filter to improve robustness and accuracy. Maximum Likelihood (ML) method is presented to include multiple distance sensors for a better metric scale estimation. The estimation of the roll and pitch angles for the camera platform is considered. This is achieved with respect to the ground plane using at least three distance sensors placed in a specific geometry to overcome ambiguity and obtain a unique solution. Proposed approach can handle terrain irregularities and does not have drift. Several simulations are performed and the performances of the proposed approaches are compared with the previous works and SVO. The experiments also include real data to show the practical relevance. It is shown that the proposed approaches improve both the metric scale and roll and pitch angles significantly.