Globally optimal consensus maximization for robust visual inertial localization in point and line map

TL;DR

This paper introduces a globally optimal, robust method for visual inertial localization that efficiently handles high outlier percentages by decoupling rotation and translation estimation, achieving faster and more reliable results.

Contribution

It proposes a novel two-stage global optimization approach using TIMs, enabling deterministic convergence and exponential speedup over existing methods.

Findings

Accurately estimates pose with up to 90% outliers

Achieves exponential speedup compared to existing BnB methods

Provides deterministic global convergence without initial guess dependence

Abstract

Map based visual inertial localization is a crucial step to reduce the drift in state estimation of mobile robots. The underlying problem for localization is to estimate the pose from a set of 3D-2D feature correspondences, of which the main challenge is the presence of outliers, especially in changing environment. In this paper, we propose a robust solution based on efficient global optimization of the consensus maximization problem, which is insensitive to high percentage of outliers. We first introduce translation invariant measurements (TIMs) for both points and lines to decouple the consensus maximization problem into rotation and translation subproblems, allowing for a two-stage solver with reduced solution dimensions. Then we show that (i) the rotation can be calculated by minimizing TIMs using only 1-dimensional branch-and-bound (BnB), (ii) the translation can be found by running 1-dimensional search for three times with prioritized progressive voting. Compared with the popular randomized solver, our solver achieves deterministic global convergence without depending on an initial value. While compared with existing BnB based methods, ours is exponentially faster. Finally, by evaluating the performance on both simulation and real-world datasets, our approach gives accurate pose even when there are 90\% outliers (only 2 inliers).