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

相机位姿估计是机器人领域中的一个基本问题。本文关注两个问题：首先，点和线特征具有互补优势，因此设计一个可以有效融合它们的统一算法非常有价值；其次，随着现代前端技术的发展，单张图像中可能存在大量特征点和特征线，这为高精度的机器人位姿估计提供了潜力。基于这些观察，我们提出了一种名为AOPnP(L)的算法，它是一种从点和线估计相机-机器人位姿的最优线性时间算法。具体来说，我们用线上的两个不同点表示一条直线，并统一了点和线测量的噪声模型，其中噪声被添加到图像中的2D点。通过利用Plucker坐标对线进行参数化，我们为组合点和线测量制定了最大似然（ML）问题。为了最优地解决ML问题，AOPnP(L)采用了两步估计方案。在第一步中，通过消除偏差，设计出一个可以收敛到真实位姿的一致估计。在第二步中，执行单个高斯-牛顿迭代来优化初始估计。AOPnP(L)具有理论上的最优性，其均方误差收敛于Cramer-Rao下界。此外，它具有线性时间复杂度。这些特性使它非常适合于对精度有要求且需要实时机器人位姿估计的应用。我们进行了大量实验证明了我们的理论成果，并展示了AOPnP(L)在静态定位和动态里程计系统中的优越性。

本文旨在解决机器人视觉中相机位姿估计的问题。具体而言，该论文试图解决两个问题：如何有效地融合点特征和线特征，以及如何利用现代前端技术中大量的特征来实现高精度的机器人位姿估计。

本文提出了一种名为AOPnP(L)的算法，可以从点和线中进行最优的线性时间相机-机器人位姿估计。该算法采用Plucker坐标对线进行参数化，并将点和线的噪声模型统一起来。通过消除偏差，利用两步估计方案来解决最大似然问题，并使用单个高斯-牛顿迭代来优化初始估计。该算法具有理论上的最优性和线性时间复杂度。

本文的亮点包括：算法具有理论上的最优性和线性时间复杂度；在静态定位和动态里程计系统中都表现出优越性能；作者通过实验验证了算法的有效性，并提供了开源代码。

在机器人视觉领域中，还有一些相关研究，例如：《ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras》、《Visual SLAM: A Survey from 2010 to 2016》等。