VINS-Mono源码解析（一）系统框架
VINS-Mono源码解析（一）系统框架
1. VINS-Mono 简介
VINS-Mono是HKUST的Shen Shaojie团队开源的一套Visual-Inertial融合定位算法. 介绍见(https://github.com/HKUST-Aerial-Robotics/VINS-Mono), 论文里面也有.
此外,他们还开源了ios版本(https://github.com/HKUST-Aerial-Robotics/VINS-Mobile), 有兴趣可以去下载源码跑一跑, 好像已经有国外团队基于这套开源代码开发了一些AR的App.
VINS-Mono是目前开源SLAM里面效果非常不错的Visual和Inertial融合SLAM算法, 通过Sliding Windows优化进行视觉和IMU的紧耦合实现VIO, 此外还加了DBoW做Loop Closure, 算是一套完整的SLAM系统. 该算法的前端比较简单,就Harris角点加LK光流跟踪, Loop Closure也挺常规, 算法的厉害之处在于后端Visual Inertial融合优化部分, 也就是VIO部分, 这是VINS算法的核心,也是后面VINS-Mono代码阅读的重点.
目前视觉和IMU融合分为松耦合和紧耦合, 松耦合的性能要逊色于紧耦合. 紧耦合的主流融合框架有基于滤波的方法(MSCKF)和基于优化的方法(VINS-Mono).
VINS-Mono在ROS下开发的, 因此可以很方便的编译和使用，跟着readme安装就可以跑起来, so easy.
2. VINS-Mono系统框架
VINS-Mono的系统框架如下图
主要有以下几个部分:
1. Measurement Preprocessing(观测预处理):对图像提feature做feature tracking,输出tracked feature list, 对IMU做预积分,输出两帧图像间的IMU积分结果.
这里应该还有个很重要的步骤, IMU和图像的数据同步, VINS-Mono代码中貌似没有体现, 不知道Mobile版本中是否有.
2. Initialization: vision-only SfM用纯视觉估计相机运动和特征深度, 视觉得到一个相对运动, IMU预积分得到一个相对运动, 二者做alignment, 从而标定出尺度, 重力加速地, 速度, 和bias.
3. Local BA: 后端融合优化, 状态向量包括: sliding window中的IMU states + 相机外参 + feature点的深度. loss包括: marginalization提供的prior + IMU residuals + visual residuals. 优化完之后, 得到各个时刻相机的位姿, 实现VIO.
4. Loop detection + Global Pose Graph Optimization: VIO毕竟有累积误差, 所以跟常规vSLAM一样, 加入闭环检测, 然后优化一个pose graph, 值得注意的是这里是只优化4 DoF, 因为scale, roll 和 pitch是可观的, 误差只会在(x,y,z)和yaw四个维度上累积.
3. VINS-Mono的ROS架构
VINS-Mono主要包含两个节点: 前端节点feature_tracker_node和后端节点estimator_node. 前端节点处理Measurement Preprocessing中的Feature Detection and Tracking, 其他几个部分(IMU preintegration, initialization, LocalBA, Loop closure)都是在estimator_node中处理.
运行样例程序:
运行feature_tracker节点和estimator节点, 订阅图像和IMU数据, 发布位姿, 3D特征点等消息给RVIZ显示
roslaunch vins_estimator euroc.launch运行RVIZ
roslaunch vins_estimator vins_rviz.launch读取rosbag, 发布图像和IMU数据给VINS节点
rosbag play YOUR_PATH_TO_DATASET/- feature_tracker_node中就一个主线程, 接收图像信息,调用img_callback()函数进行处理.
- estimator_node节点中开了3个子线程, 分别是:process,loop_detection和pose_graph. 其中
- process()线程处理VIO后端, 包括(IMU preintegration, initialization, LocalBA)
- loop_detection()线程处理闭环检测
- pose_graph()线程分别处理全局优化
如果不需要做loop closure, 可以把LOOP_CLOUSRE置0, 则后面两个线程不会创建.