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.

翻译：相机位姿估计是机器人学中的基础问题。本文重点关注两个具有实际意义的问题：首先，点特征与线特征具有互补优势，设计能够有效融合二者的统一算法具有重要价值；其次，随着现代前端技术的发展，单幅图像中可存在大量特征，这为实现高精度机器人位姿估计提供了潜力。基于这些观察，我们提出了AOPnP(L)——一种基于点与线特征的线性时间最优相机-机器人位姿估计算法。具体而言，我们采用直线上两个不重合的点来表示直线，并统一了点与线测量的噪声模型，其中噪声被添加到图像中的二维点坐标。通过采用普吕克坐标进行直线参数化，我们构建了点线融合测量的最大似然估计问题。为最优求解该最大似然问题，AOPnP(L)采用两步估计策略：第一步通过偏差消除技术设计出能够收敛至真实位姿的一致性估计量；第二步执行单次高斯-牛顿迭代以优化初始估计。AOPnP(L)具有理论最优性，其均方误差收敛于克拉美-罗下界，同时具备线性时间复杂度。这些特性使其非常适用于高精度与实时性要求的机器人位姿估计任务。我们通过大量实验验证了理论推导，并证明了AOPnP(L)在静态定位与动态里程计系统中的优越性能。