基于直线的最小非线性SLAM相机姿态估计方法

1. 介绍

针对线映射到相机图像，提出Perspective-n-Lines（PnL） 问题，PnL解决的是六自由度位姿估计，将三维空间中的直线正确地投影到二维图像平面上；PnL问题的三种解，即 直接线性变换（DLT）、多项式解和迭代解。
直接线性变换是用奇异值分解（SVD）直接求解重投影方程，这种方法将旋转矩阵中的9个变量和平移向量中的3个变量视为自变量，忽略了变量的非线性正交约束。因此，PnL问题的多项式解将投影函数与正态性约束结合起来形成高阶方程，DLT方法和多项式方法都用至少12个变量的多解冗余表示参数化了刚体运动。
另一种是使用非线性方法迭代以最小化重投影误差，该方法将一条三维直线视为两个端点，这就增加了自由度，因为两个三维点有6个自由度，而一条三维直线有4个自由度。
论文主要创新：
**1）**分析了李群切线空间中直线在6自由度运动矢量上的重投影误差偏导数（雅可比矩阵），为迭代方法更新估计奠定了基础。
**2）**提出了一种新的基于线特征的非线性姿态估计方法，该方法仅适用于3对线的对应关系，框架非常适合于摄像机姿态的连续更新。

2. 非线性优化方法

2.1 迭代法
在给定图像平面上的一组二维直线及其世界坐标系下对应的三维直线lw，找到了一个使重投影误差最小的姿态，通过迭代（高斯牛顿法）最小化距离来估计姿态，该距离是计算出的直线预测与其在真实图像中的测量li之间的残余误差：分析得到高斯牛顿更新函数，其中ε表示为六自由度向量的迭代更新，J表示为运动矢量上重投影的二维直线的偏导数矩阵：
2.2 重投影
重投影的三维直线可分为两步：
1）将世界帧中以Plücker坐标表示的3D线转换为相机帧
2）根据针孔相机的成像模型，将变换后的直线映射到图像上的二维直线上
由于投影图像线只与直线的u向量有关，采用偏导数链式法则得到最终的雅可比矩阵：
2.3 ε雅可比

类似于使用点对应的姿态估计方法中的表达式，我们将雅可比矩阵定义为：
求解ε雅可比矩阵，需使用李群中伴随矩阵：为了简化计算，z来表示摄像机坐标lc中3D线的伴随函数，即投影后线的坐标：由李代数理论得出：融合上面的方程可以得到整个系统的雅可比矩阵，重投影线在6自由度运动矢量上的偏导数：根据所给出的雅克比矩阵，其每一直线对应给出2个约束方程，则需要3条直线就可计算刚性运动。

3 实验

3.1 噪声模拟试验

采用蒙特卡罗方法在三维空间中生成随机线，生成一个随机的刚性运动，将这些线转换为摄像机坐标。最后，利用实验室工业摄像机的摄像机内参矩阵作为虚拟摄像机，将三维直线投影到图像平面上。将相机分辨率设置为VGA（640*480），相机和随机线中心之间的距离设置为2米。在每个图像噪声水平下，分别在2.1像素内的0.1像素区间内重复了100次随机测试。
测试结果：

从图中可以看出误差随着图像噪声的增加而增大。在2个像素的噪声sigma下，平均平移误差约为1cm。最大平移误差小于4cm。论文方法在亚像素图像噪声下的位置误差小于2cm，旋转误差始终小于0.4°，显示出较高的方位估计精度。
对比其他方法
在测试随机线集和刚性变换下，最新发展的LPnL、Bar-LS、DLT-plücker、DLT-combined方法和论文方法在方向和位置估计方面都有更稳健和准确的性能。Ansar和Mirzaei的方法对噪声太敏感，导致输出不稳定。Mirzaei方法是唯一一种使用3对线的方法，但是在存在噪声的情况下，它没有收敛的结果。
论文方法在旋转和平移估计方面都具有与现有方法相当的性能，而只有该论文算法能够处理3个噪声匹配线对。

3.2 实景实验

在模拟实验的基础上，利用该算法对VGG道路数据集进行姿态估计。结果如图所示，估计的姿势具有4cm的位置误差和0.04°的旋转误差，尽管有些端点不严格匹配，但估计的线恢复得非常好。并且由于未匹配的端点仍然位于同一条直线上，因此它们仍然能够很好地重建图像平面上的二维直线。
里程计
采用纯运动BA方法对姿态进行连续估计。红线是分解后的轨迹，蓝线是也测轨迹。在摄像机运动9.19m后，只得到了0.066m的位置误差，考虑到旋转误差，平均旋转误差为0.0045°左右，方向确定准确。