vins的重定位模块主要包含回环检测,回环候选帧之间的特征匹配,紧耦合重定位三个部分.
a.回环检测:
采用BRIEF描述子的DBOW2词袋进行闭环检测,对新来的关键帧重新检测500个角点进行闭环检测同时对所有角点进行BRIEF描述,然后计算当前帧与词袋的相似度,与关键帧数据库中所有帧进行对比,通过闭环一致性检测得到闭环候选帧.(回环检测只对关键帧进行,当一帧被加入到关键帧中时才进行回环检测)
b.特征匹配:
检测到回环时,通过BRIEF描述子匹配找到对应关系,建立局部滑动窗口与回环候选帧之间的连接.直接描述子匹配可能会有大量的外点,所以进行两步几何外点剔除操作:2d-2dRANSAC基本矩阵检验(程序中并没有使用);3d-2dRANSAC的PNP检验.若内点超过一定阈值,就将该候选帧视为正确的循环检测并执行重定位.
c.紧耦合重定位:
重定位使当前滑窗与过去的位姿图对齐.将回环帧的位姿作为常量,利用所有IMU测量值,局部视觉测量和从回环中提取的特征对应值共同优化滑动窗口.
当检测到当前帧与之前帧(记为第v帧)有闭环时,将第v帧的位姿和相关特征点作为视觉约束项加入到后端非线性优化的整体目标函数中,用滑窗优化来更精确计算第v帧的位姿.根据优化得到的相对位姿关系,对滑窗内所有帧的位姿进行调整,完成快速重定位.这里新加入的优化项中,集合为回环帧中匹配的特征点,(l,v)意思是在回环帧v中观测到的第l个特征点,(q,p)为第v帧的位姿.优化后将当前帧重新定位到原世界坐标系中.
d.闭环优化: