VINS-Mono 代码解析四、闭环检测和优化 (偏理论)

一、理论部分

1.1 怎么知道发生了闭环

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

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)。

1.2 闭环中的几个坐标系

1.3 闭环优化

二、程序

2.1 函数入口

2.2 闭环检测：addKeyFrame()

2.3 闭环优化：optimize4DoF

2.4 闭环优化的因子图

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