  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.
