VINS-Mono 融合导航定位流程

type

status

date

slug

summary

融合导航定位流程

参考链接

VINS 论文推导及代码解析参考文档：

kdocs.cn

https://kdocs.cn/l/cstZk0llyV0n

VIO系列 | 视觉与惯性传感器如何融合？来研读VINS-Mono论文与代码_AI算法修炼营的博客-CSDN博客

点击上方" AI算法修炼营 "，选择加星标或"置顶" 标题以下，全是干货一、本系列概述本系列旨在对前一阶段学习 vins-mono开源框架的总结。结合暑假秋招之前报名的深蓝学院的《从零开始手写VIO》课程，本系列文章将从VIO原理以及开源代码分析两部分进行详细介绍。提升代码能力最好的办法就是从头到尾撸开源框架，这种体验是单纯推导公式体会不到的。论文地址：https://arxiv.org/pdf/1708.03852.pdf 源代码地址：https://github.com/HKUST-Aerial-Robotics/VINS-Mono 为什么要用VIO（视觉惯性里程计）？ 1）单纯视觉：缺点: 尺度不确定性、单目纯旋转无法估计、快速运动易丢失、受图像遮挡运动物体干扰。优点：不产生漂移、直接测量旋转与平移。 2）单纯IMU ：缺点：零偏导致漂移、低精度IMU积分位姿发散优点：快速响应、可估计绝对尺度、角速度估计准确。 3）结合视觉+IMU：可用视觉弥补IMU的零偏，减少IMU由于零偏导致的发散和累计误差，IMU可为视觉提供快速响应的定位。融合方案： VINS-Mono贡献：二、本系列文章目录本系列共分为12部分，对应系统的4大部分：图像和IMU预处理、初始化、后端滑动窗口优化、闭环检测和优化。三、VINS-Mono框架与代码总览 Vins-mono是香港科技大学开源的一个VIO算法，用紧耦合的方法，通过单目+IMU恢复出尺度，效果非常棒。 VINS的功能模块可包括五个部分：数据预处理、初始化、后端非线性优化、闭环检测及闭环优化。代码中主要开启了四个线程，分别是：前端图像跟踪、后端非线性优化（其中初始化和IMU预积分在这个线程中）、闭环检测、闭环优化。 3 后端滑动窗口优化：将视觉约束、IMU约束和闭环约束放在一个大的目标函数中进行非线性优化，求解滑动窗内所有帧的PVQ、bias 4 闭环检测和优化：

https://blog.csdn.net/sinat_17456165/article/details/106760620

引言：

融合导航定位即融合多种传感器来实现导航定位的功能，从而提升导航定位的精度和鲁棒性。而在融合导航定位中我选择VIO进行实现，VIO（visual-inertial odometry）即视觉惯性里程计，有时也叫视觉惯性系统（VINS，visual-inertial system），是融合相机和IMU数据实现SLAM的算法，根据融合框架的区别又分为紧耦合和松耦合，松耦合中视觉运动估计和惯导运动估计系统是两个独立的模块，将每个模块的输出结果进行融合，而紧耦合则是使用两个传感器的原始数据共同估计一组变量，传感器噪声也是相互影响的，紧耦合算法上比较复杂，但充分利用了传感器数据，可以实现更好的效果，是目前研究的重点。

下列流程主要是对于VINS-Mono。

流程

概述

Vins-mono是香港科技大学开源的一个VIO算法，用紧耦合的方法，通过单目+IMU恢复出尺度，效果非常棒。

VINS的功能模块可包括五个部分：数据预处理、初始化、后端非线性优化、闭环检测及闭环优化。代码中主要开启了四个线程，分别是：前端图像跟踪、后端非线性优化（其中初始化和IMU预积分在这个线程中）、闭环检测、闭环优化。

算法的流程如下：

1.特征点跟踪

首先在第一帧图像上面找特征点，然后在下一帧过来时，对这些特征点用光流法进行跟踪，在下一帧上找匹配点。然后对前后帧中这些匹配点进行校正。先对特征点进行畸变校正，再投影到以原点为球心，半径为1的球面上，再延伸到深度归一化平面上，获得最终校正后的位置。对于每对匹配点，基于校正后的位置，用F矩阵加ransac来筛选。然后再在匹配上的特征点之外的区域，用cv::goodFeaturesToTrack搜索最强的新的特征点，对特征点数量进行补充。

最后，把剩下的这些特征点，把图像点投影回深度归一化平面上，再畸变校正，再投影到球面上，再延伸到深度归一化平面上，得到校正后的位置。把校正后的位置发送出去。

特征点跟踪和匹配，就是前一帧到这一帧的，一帧帧继承下去。或者生成新的特征点。

2.初始化

2.1 外参中的旋转

用机器人手眼标定的方法，计算出外参中的旋转。

2.2 SFM

先在关键帧窗口里面，找到第一帧，第一帧与最后一帧有足够的像素位移，并且能用8点法算出旋转和位移。以一帧的姿态为世界坐标系。先从一帧开始与最后一帧进行三角定位，再用pnp估计出下一帧的位姿，下一帧再与最后一帧三角定位得出更多的三维点。重复到倒数第二帧。从一帧开始往第一帧，逐渐帧pnp，再与第l帧进行三角定位得到更多的三维点。每帧pnp时的位姿初值都用上一个关键帧的的位姿。剩下的那些还没有被三角定位的特征点，通过它被观察到的第一帧和最后一帧进行三角定位。

2.3 计算陀螺仪的偏移

在2.1中已经根据连续图像的相对旋转算出相机和IMU间的外参旋转了，现在要再根据上一节2.2中的SFM算出来的各帧图像的相对旋转来计算出陀螺仪的偏移。

2.4 速度，重力和尺度对齐

从2.3之后，认为陀螺仪是准确的，每帧图像的姿态也都是准确的。

速度，重力和尺度对齐，其实就是，每一帧对后一帧的位置，速度的预测值，与当前值的误差进行统一。

2.5 进一步优化重力

在2.4计算出来一个相对准确的值的基础上，还要再加个约束，即重力的模为9.8。

2.6 优化地图

用奇异值分解的方法，对每一个特征点都优化重投影误差最小化，优化出它在被观察到的第一帧的相机坐标系下的位置，然后只把深度拿过来用。

与2.2SFM时算出的地图点的区别在于，2.2SFM时算出的地图点是ceres优化出来的关键帧及其对应的地图点。而这里，是固定这段时间的所有的关键帧，这些关键帧的姿态在2.3后期算出陀螺仪偏移后，已经调整过了。

3.正常的跟踪

每新进来一张图片，上面有跟踪出来的特征点。

对于f_manager中的feature列表中的那些还没有深度的特征点，用奇异值分解计算出一个它的坐标，使得它在它被观察到的每帧图像上的重投影误差最小。它的坐标用在它被观察到的第一帧图像的相机坐标系的深度表示。因为它还有它在被观察到的每帧图像上的归一化坐标。

然后用cere来优化。结合各个关键帧的位姿，各个相机的外参，边缘化的信息，与预积分的误差，每个特征点的重投影误差，回环闭环误差。进行优化。

3.1 判断是否是关键帧

3.2 创建地图点

3.3 ceres优化

Ceres优化包含了，与先验值的残差，与预积分的残差，最小化重投影误差，边缘化，回环优化。

4.回环检测

这里的回环检测，是每3个关键帧检测一帧，相当于是跳两帧。这跟回环检测的速度，和实际关键帧生成的速度，对比有关。因为回环检测的速度总是慢于关键帧生成的速度，所以为了保持回环检测的关键帧不落后于时间，只能跳帧检测。ORBSLAM里面也是这样，但ORBSLAM里面的回环检测判断标准是，一段时间内的关键帧都能匹配上回环，所以ORBSLAM的策略是拿一段时间的关键帧来进行检测。ORBSLAM的回环检测程序，Sleep一段时间，在Sleep的这段时间，收集关键帧，然后开始工作，只针对收集的这些关键帧。工作时，不收集新的关键帧，都跳过。处理完这些关键帧后，又Sleep。

因为回环检测的速度总是慢于关键帧生成的速度，所以为了保持回环检测的关键帧不落后于时间，只能跳帧检测。