Optimal camera-robot pose estimation in linear time from points and lines

TL;DR

This paper introduces AOPnP(L), a linear-time algorithm for robot pose estimation that effectively fuses point and line features, achieving theoretical optimality and high accuracy suitable for real-time applications.

Contribution

The paper presents a novel, optimal, linear-time pose estimation algorithm combining points and lines with a two-step estimation process and theoretical guarantees.

Findings

AOPnP(L) achieves mean squared error close to the Cramer-Rao lower bound.

The algorithm operates in linear time, suitable for real-time systems.

Experimental results show superior accuracy in static and dynamic scenarios.

Abstract

Camera pose estimation is a fundamental problem in robotics. This paper focuses on two issues of interest: First, point and line features have complementary advantages, and it is of great value to design a uniform algorithm that can fuse them effectively; Second, with the development of modern front-end techniques, a large number of features can exist in a single image, which presents a potential for highly accurate robot pose estimation. With these observations, we propose AOPnP(L), an optimal linear-time camera-robot pose estimation algorithm from points and lines. Specifically, we represent a line with two distinct points on it and unify the noise model for point and line measurements where noises are added to 2D points in the image. By utilizing Plucker coordinates for line parameterization, we formulate a maximum likelihood (ML) problem for combined point and line measurements. To optimally solve the ML problem, AOPnP(L) adopts a two-step estimation scheme. In the first step, a consistent estimate that can converge to the true pose is devised by virtue of bias elimination. In the second step, a single Gauss-Newton iteration is executed to refine the initial estimate. AOPnP(L) features theoretical optimality in the sense that its mean squared error converges to the Cramer-Rao lower bound. Moreover, it owns a linear time complexity. These properties make it well-suited for precision-demanding and real-time robot pose estimation. Extensive experiments are conducted to validate our theoretical developments and demonstrate the superiority of AOPnP(L) in both static localization and dynamic odometry systems.