2-Entity RANSAC for robust visual localization in changing environment论文笔记

论文链接：https://arxiv.org/pdf/1903.03967.pdf

文章主要内容

本文指出当前的视觉定位方法由于地图或者图片序列在外观上的巨大变化，仍然会受到季节天气等环境因素的影响，进一步指明问题的原因在于外点数，即错误的特征匹配所占比例。在借助惯性测量的方法下，针对3D-2D的定位方式，文章提出了2-entity RANSAC(random sample consensus)方法，即使用两点匹配或者一点和一条线的匹配方式，在降低了所需特征点匹配数目的同时提高了系统对外点的鲁棒性。

文章主要贡献

1、提出一种通用框架，来推导位姿估计的封闭形式解， 它可以处理点和线特征，即2点匹配或1点匹配和1线匹配。
2、通过将最小封闭形式解嵌入到RANSAC框架中，提出了2-Entity RANSAC方法, 并在RANSAC中提出了三种选择不同类型特征匹配进行位姿估计的采样策略。
3、分析了随机抽样系统中不同抽样策略的成功概率，并提出了一种根据环境特征自适应选择策略的选择机制。
4、提出的方法在综合数据和多个真实世界数据上进行评估，实验结果验证了该方法在季节性和光照变化的视觉定位中的有效性和高效性。

算法分析

首先介绍一下欧拉角

其中，pitch称为俯仰角，yaw称为偏航角，roll称为翻滚角。
首先，定义世界参考系$W_m$，相机坐标系为$C_q$，相机坐标系到世界坐标系下的位姿变换表示为$T_{W_m{C}_q}$，$\tilde{\beta }$和$\tilde{\gamma }$分别表示俯仰角和翻滚角，$T_{W_m{C}_q}$可以表示为：
$$T_{W_m{C}_q}=[R_z(\alpha )R_y(\tilde{\beta })R_x(\tilde{\gamma })|(T_1{T}_2{T}_3{)}^T].$$
其中，$\alpha$表示偏航角，$(T_1,T_2,T_3)$表示带估计的三个变换。

如上图所示，相机的光学中心为$C^0$，地图点为$P_1^0$，$P_1^0$在图像上对应的投影点为$D_1^0$，三点共线，该直线表示为 { C 0 , D 1 0 , R C q W m P 1 0 + t C q W m } L \{C^0,D_1^0,R_{C_qW_m}P_1^0+t_{C_qW_m }\}_L {C0,D10​,RCq​Wm​​P10​+tCq​Wm​​}L​，通过$C^0$和$D_1^0$求解直线方程，第三个点可以被替代，形成了两个独立约束条件。另一方面，相机的光学中心为$C^0$，地图点线段$L_0^2{L}_0^3$以及它在图像平面上的的投影线段$D_0^2{D}_0^3$，该图像平面$\pi$可以表示为 { C 0 , D 2 0 , D 3 0 , R C q W m L 2 0 + t C q W m } P \{C^0,D_2^0,D_3^0,R_{C_qW_m}L_2^0+t_{C_qW_m }\}_P {C0,D20​,D30​,RCq​Wm​​L20​+tCq​Wm​​}P​和 { C 0 , D 2 0 , D 3 0 , R C q W m L 3 0 + t C q W m } P \{C^0,D_2^0,D_3^0,R_{C_qW_m}L_3^0+t_{C_qW_m }\}_P {C0,D20​,D30​,RCq​Wm​​L30​+tCq​Wm​​}P​。同理，通过$C^0$,$D_2^0$和$D_3^0$求解直线方程，地图点线段的两个端点可以被替代，形成了另外两个独立的约束条件。因此，两对非退化的匹配便足够解决4DoF的定位问题。下面分三种情况进行讨论：
1、一点一线
考虑一个3D-2D点匹配和一个3D-2D线匹配，问题可以描述为1P1L问题，考虑到被检测的图像特征点的2D坐标，可以根据校准的内参计算相应的3D投影射线，同样的过程可以运用到线段的两个端点处获得两条3D投影射线，3D投影射线在相机参考帧$C_q$中进行表示，对应的特征点的3D坐标在世界参考帧$W_m$中进行表示，为了简化方程，我们针对相机和地图分别引入中间参考帧$C_1$和$W_1$。
针对$C_1$的选择，如下图所示：

在初始的相机参考帧$C_q$中，原点就是相机的中心点$C^0$，关于2D点的相机投影射线由它的归一化方向矢量$\overrightarrow{d_1}$进行表示，2D直线的两个端点对应投影射线分别由它的归一化方向矢量$\overrightarrow{d_2}$和$\overrightarrow{d_3}$进行表示。
在中间参考帧$C_1$中，关于2D点的投影射线可以表示为$C{D}_1$，关于2D线的投影射线可以表示为$C{D}_2$和$C{D}_3$。中间参考帧需要满足下列条件：
1）新的相机中心$C$为$(0,0,-1)$
2）直线$L_2{L}_3$的一个端点对应的投影射线$C{D}_2$在$z$轴上，使得$D_2$的坐标成为(0,0,0)。
3）直线$L_2{L}_3$的另一个端点对应的投影射线$C{D}_3$在$xz$平面上，并且$D_3$点是$x$轴和投影射线的交点。
4）特征点对应的投影射线上的$D_1$点在$xy$平面上。
经过变换之后，得到如些结果：

然后初始相机帧下的点可以通过下面的公式计算：

变换矩阵$T_{c_1{c}_q}$可以通过$(C^0,D_2^0,D_3^0)$到$(C,D_2,D_3)$的变换求得。从而，$D_1$点可以表示为$(a_1,b_1,0)$。
针对$W_1$的选择：
世界参照帧的变换是一种将3D点变换到$W_1$原点的变换：

因此，在$W_1$中：

这里注意这些点已知。
下面介绍$C_1$和$W_1$之间的位姿估计：
记旋转和平移矩阵分别为$R$和$t$，$R\triangleq R_{C_1{W}_1},t\triangleq t_{c_1{w}_1}$。根据 { C , D 1 , R P 1 + t } L \{C,D_1,RP_1+t\}_L {C,D1​,RP1​+t}L​的共线性，可得以下等式：

根据 { C , D 2 , D 3 , R L 2 + t } P \{C,D_2,D_3,RL_2+t\}_P {C,D2​,D3​,RL2​+t}P​的共平面性：

根据 { C , D 2 , D 3 , R L 3 + t } P \{C,D_2,D_3,RL_3+t\}_P {C,D2​,D3​,RL3​+t}P​的共平面性：

其中，$R_{mn}$表示$R$的第$m$行$n$列，$T_m$表示$t$的第$m$个元素。
同时，注意到矩阵$R$由$\alpha$唯一确定。结合上诉等式，可以求解$T_{C_q{W}_m}$：

2、两点
在这一部分，我们给出两对3D-2D的点匹配，记为2P。在这中情境下，中间的相机参考帧和原始帧保持一致，因此在$C_q$和$C_1$中的点可记为：

其中，$a_1,a_2,b_1,b_2$是已知参数，可以通过内参和归一化的深度求得。对世界参考帧的变换是将两个3D坐标点的一个变换到中间世界参考帧的原点，因此，在$W_1$中，有：

仿照1P1L的标记方式，根据 { C , D 1 , R P 1 + t } L \{C,D_1,RP_1+t\}_L {C,D1​,RP1​+t}L​的共线性，可得以下等式：

根据 { C , D 2 , R P 2 + t } L \{C,D_2,RP_2+t\}_L {C,D2​,RP2​+t}L​的共线性，可得以下等式：

结合上诉公式，可以求解$T_{C_1{W}_1}$，进一步可以求得定位结果：

3、两条线
根据论文推导，此方法不可行。

模型选择

记$p$为匹配点的数目，$l$为匹配线的数目，$\lambda$为点匹配的内点率（正确匹配率），$\gamma$为线匹配的内点数：

其中，$m,n$分别表示匹配点的内点数，匹配线的内点数。然后基于RANSAC的不同采样方式成功的概率如下：

$mixed$表示将$1_p{1}_l$和$2p$两种方式结合。进一步可以得到：

其中$a=\frac{1-\lambda }{p-1}$。
通常情况下满足$p-1\gg 1-lambda$，这意味着$a$是一个接近0的非常小的正数。因此，可以得到下面两个式子：


然后可以根据特定的场景选择不同的采样方式。

实验结果

所提出的方法在合成和真实数据上进行了评估，以证明其在视觉定位方面优于其他基线方法。我们首先通过模拟实验来说明当图像特征有噪声时的准确性，对不准确的俯仰和滚转角的敏感性，以及对异常值的鲁棒性。然后在真实数据上，通过成功率比较，验证了所提方法和模型选择机制的有效性。
1 合成数据的结果

我们在Cube中生成了一些3D点和线，并针对不同的相机姿态计算了2D投影，以获得3D-2D特征匹配。对于每种方法，执行100次RANSAC迭代。最终识别的内点被发送到非线性最小化器以获得高精度。 在优化中，有两种可能的方式:4DoF或6DoF优化。4自由度优化是指我们固定惯性测量单元提供的俯仰和滚转角，只优化姿态中的其他四个变量。当需要优化的内点程序很少时，这可能很有用。

模拟实验中评估的最小解是EPnP 、P3P 、2P1L ，我们提出的解包括1P1L-6自由度、1P1L-4自由度、2P1L自由度，2P1L自由度、混合6自由度和混合4自由度也在针对异常值的鲁棒性实验中进行了评估。我们利用平移和旋转误差来评估相对于地面真实情况的估计结果。

1)精确度:为了量化不同最小解的精确度，我们为2D投影添加了具有零均值和可变标准差的高斯噪声，并在四个级别中改变了特征匹配的数量:10、5、4、3(10的情况意味着场景中有10个点匹配和10个线匹配)。结果如图3所示。我们可以发现，当特征匹配足够充分时(见案例10)，随着噪声标准偏差的增加，所提出的方法可以获得与其他基线方法相同的精度。 当特征匹配的数量减少时，与2P1L、EPnP和P3P相比，我们的两实体方法的优势变得更大。人们可以注意到，当特征匹配的数量减少到4个时，我们的方法的误差略大于10个匹配的情况，而其他的则严重增长。

2)灵敏度:在惯性传感器的帮助下，我们通过利用传感器提供的俯仰和滚转角，将定位问题的程度降低到4。因此，有必要研究这两个角度的质量对最终精度的影响。我们在俯仰角和滚转角上加入了均值为零、标准差可变的高斯噪声，并在10°、5°、3°三个特征匹配水平上研究了所提方法的性能。 结果如下图所示。

当有足够的特征匹配时(见10种情况)，所提出的方法可以在俯仰角和滚转角上容忍几乎25度的噪声。 根据经验，在实际应用中，俯仰角和滚转角的噪声要小得多，因此俯仰角和滚转角的噪声对精度的影响应该是有限的。 我们还注意到，在这种情况下，6自由度优化优于4自由度优化。然而随着特征匹配数量的减少，6自由度和4自由度方法之间的差异变小。 这是合理的，因为当内点数较少时，优化中由附加自由度引起的误差可能大于由噪声姿态估计引起的误差。 因此如果长期存在非常有限的可靠特征匹配变化发生时，4自由度优化是一个很好的选择。

3)鲁棒性:为了验证所提出方法的鲁棒性，我们设计了一些实验，在不同的内点数水平(10，5，4，3)下，将外点率从0变化到80%。 我们通过添加外点来获得外点率，外点是通过错误地关联原始数据中的特征而产生的。 当平移误差小于10%，旋转误差小于5度时，则认为定位成功。我们对每种方法进行了200次试验，以平均成功率。 结果见图5。 不出所料，当外点率增加时，所提出的两实体RANSAC优于其他最小解，当内点数减少时，这一点更加明显。 注意，当外点率为80%时，通过足够的内点，所提出的方法可以达到90%以上的成功率。

2 真实数据的结果

对于真实世界的实验，使用YQ-Dataset，其中三个数据集是在2017年夏天的三天内收集的，表示为2017-0823、2017-0827、2017-0828，另一个数据集是在2018年冬天收集的，表示为2018-0129。 我们选择2017-0823数据集来构建3D地图，并利用其他三个数据集来评估不同天气或季节下的定位性能。 地面真实相对姿态是通过对齐同步的3D激光雷达扫描提供的。 评估的方法是P3P的EPnP，我们的方法包括1P1L-4/6DoF，2P4/6DoF，混合-4/6DoF。为了获得查询图像和地图之间的3D-2D特征匹配，我们采用了以下步骤:

1）使用视觉惯性SLAM软件获得相机姿态和地图中的3D-2D点匹配[30]
2）运行Line3D++算法以获取地图中的3D-2D线匹配
3）对于查询会话，我们基于LibVISO2的描述符获得3D-2D点/线匹配LBD，然后传送到RANSAC
4）对于查询图像的俯仰和滚转角，我们直接使用IMU测得的对应值

我们首先使用不同的方法计算查询会话中所有关键帧中正确定位的查询图像，并在四个平移和旋转误差阈值下以百分比表示定位成功率，这可以在选项卡中看到6自由度优化方法的性能仍然优于4自由度优化方法。 此外，我们RANSAC比P3P和EPnP的RANSAC更好，后者要求更多的匹配次数。所提出的2P方法和混合方法明显优于EPnP方法和P3P方法，这表明了所提出的方法在处理不同天气和季节时的鲁棒性。注意1P1L没有基于点要素的方法好，其中的原因是地图环境中道路两侧有很多树，所以点要素远比线要素丰富。

为了在1P1L和2P方法之间进行公平的比较，我们从地图上手动挑选了一些结构化和非结构化的片段，其中一些图像如图6所示。 在非结构化数据段中选择了大约100个位置，在结构化数据段中选择了50个位置。 选项卡中显示了所选细分市场上采用多种方法进行本地化的成功率。 结果表明，2P方法在非结构化场景中表现最好，1P1L在结构化场景中的表现与2P相似，但在非结构化场景中的表现明显差于2P，证实了环境中的特征分布对方法选择有不可避免的影响。 此外从2018-0129结构化场景的结果中，我们可以发现，当外点百分比由于季节的变化而增长时，情况越好线特征匹配的鲁棒性提高了1P1L方法的性能。 从理论上推导出的混合6自由度，与2P和1P1L相比，在所有领域都具有相对稳定的性能。

为了进一步验证模型选择机制的正确性，我们在2017-0828数据集上做了一个额外的遗漏实验。 根据2017-0827和2018-0129数据集在2017-0823地图上的定位结果，利用2P-6自由度估计姿态获得的点和线特征的内点率，自动为整个数据集标注结构化和非结构化线段。如果线的内点率高于点，则在该段中使用1P1L，否则使用2P之后，我们在不同的定位误差阈值上再次统计了整个2017-0828数据集的成功率，并计算了每种方法的曲线下面积，如下表所示。结果表明，通过选择机制，性能得到了预期的进一步提升。