VINS-Mono 代码解析四、闭环检测和优化

闭环检测
VINS 是采用 BRIEF 描述子的 DBoW2 词袋进行闭环检测,因为前端识别的 Harris 角点数量通常只有 70 个(VINS-Mobile)
,对于闭环检测远远不够,因此会对新来的 KeyFrame 即后端非线性优化刚处理完的关键帧,再重新检测出 500 个 FAST 角点进行闭环检测用,

同时对所有新老角点进行 BRIEF 描述(对应代码 BriefExtractor::operator)。然后,计算当前帧与词袋的相似度分数,并与关键帧数据库中所有帧进行对比，

并进行闭环一致性检测,获得闭环的候选帧(TemplatedLoopDetector::detectLoop)。当检测到闭环后,我们利用 BRIEF 描述子对闭环对的老帧的 500 个 FAST 角点,

和当前帧中的 70 个 Harris 角点进行邻域匹配(KeyFrame::searchInAera),然后对匹配点对利用求基础矩阵对异常点进行 RANSAC 排除(KeyFrame::PnPRANSAC)。

当匹配点超过阈值我 们 则 认 为 该 候 选 帧 是 一 个 正 确 的 闭 环 帧 ( 对 应 代 码KeyFrame::findConnectionWithOldFrame)。

重定位
当检测到当前帧与之前帧(记为第 v 帧)有闭环时,我们将第 v 帧的位姿和相关特征点作为视觉约束项,加到后端非线性优化的整体目标函数中,

并固定第 v 帧的参数,来优化滑窗内的其他参数。这样的目标函数可写为:

这样,我们根据当前帧的闭环帧对滑动窗口内的位姿进行优化。当下一帧来时,一般也会与数据库中的某一帧为闭环帧,

因此可以用多个闭环帧对滑窗内位姿进行多重约束优化,即对应上图中的第 4 步。

闭环关键帧数据库
上面说的关键帧数据库,是当滑窗内的关键帧被移出滑窗时才会添加进数据库中(注意这种策略貌似在最新的 VINS-Mono 已放)
,这样可以保证添加进来的关键帧位姿基本正确,因为已经经过前面的重定位。当然,即使不正确也无所谓,因为会在闭环优化时进行修正。
数据库中的帧对于下一步的闭环优化会提供两种边:

a. 序列边(Sequential edge):  是指通过 VIO 计算的两帧之间的相对位姿

b. 闭环边(Loop edge):是指检测到闭环的两帧。

当运行时间越来越长,数据库将变得越来越大,导致闭环检测和闭环优化的耗时也将越来越长。

虽然前面已经仅保留每帧图像的位姿和特征点描述子,已扔掉原始图像,但是当运行几小时后仍将无法实时。

因此,我们将根据分布密度对数据库进行下采样,保留那些与周围 帧 的 位 置 和 角 度 不 同 的 , 而 丢 弃 集 中 在 一 起 的 帧 ( 代 码 对 应KeyFrameDatabase::downsample)。

 

闭环优化

当从滑窗内滑出的帧与数据库中的帧为闭环帧时,则对数据库的所有帧进行闭环优化。因为前面已经跟重力对齐,因此根据重力方向可以观测出俯仰θ和翻滚φ角度,即 pitch 和 roll
可观。因此闭环优化时,我们仅优化位置 x,y,z 和偏航角 yaw 这四个自由度。那么,第 i 帧和第 j 帧的残差可写成:

实际程序逻辑

VINS 的三个版本中(VINS-Mobile、VINS-Mono 第一版、VINS-Mono171229 版)的闭环逻辑不完全相同,或者说差别很大,

最后在 Android 采用的是在 VINS-Mono171229 版基础上添加了 VINS-Mobile 的部分。下面我们对闭环与后端优化的关系再进行详细说明。

上图中,蓝线为正常的闭环优化流程,即通过后端的非线性优化来更新滑窗内所有相机的位姿。紫线为闭环检测模块,当后端优化完成后,会将滑窗内的次新帧进行闭环检测,即
首先提取新角点并进行描述,然后与数据库进行检索,寻找闭环帧,并将该帧添加到数据库中。红线为快速重定位模块,当检测到闭环帧后,会将闭环约束添加到后端的整体目标函数
中进行非线性优化,得到第 i 帧(注意这里的帧为闭环帧中的老帧)经过滑窗优化后的位姿,这时可以直接根据计算结果修正滑窗内所有相机的位姿,但是因为此处计算的T i←i_opt 并不
准 确 , 因 此 我 们 会 在 检 测 到 闭 环 成 功 后 , 进 行 PoseGraph 的 四 自 由 度 优 化32PoseGraph::optimize4DoF(),来计算T i←i_opt ,并将此值(r_drift、t_drift)传回给后端优化线
程,来更新滑窗内的相机位姿 Estimator::update_loop_correction()。