个人注释版本的代码链接如下
https://github.com/linyicheng1/OpenSLAM-Notes/tree/main/VINS-Mono-master
算法分为三个部分,分别在3个独立的ros工程中,分别为 feature_tracker vins_estimator pose_graph
运行算法的launch文件,如euroc.launch中则运行该三个节点,并获取配置文件 config/euroc_config.yaml 路径,传递给各程序
因此,大致的算法思想为feature_tracker模块对图像数据进行处理,得到特征点跟踪信息,vins_estimator为主体程序处理跟踪得到的特征点信息和imu数据,pose_graph则为后端优化以及重定位功能,接下再仔细对每一个模块进行分析
本模块中核心实现为特征点跟踪类,基本流程为获取得到一帧图片数据转换为opencv格式并控制帧率,最后送入FeatureTracker类中跟踪特征点,最后发布特征点数据。核心调用在于95行的
95:trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec());
其中函数readImage的主要流程如下,包含两个较为重要的知识点
主要的执行函数如下
void process();
imu_callback
feature_callback
relocalization_callback
imu_propagate
)predict()
给出了实现,可参考注释学习。本线程是主要处理部分,首先明确如下数据,即输入数据有三个,均使用了队列的数据结构对数据先进先出逐个处理。
queue imu_buf
queue feature_buf
queue relo_buf
主要的流程图如下,核心函数实现类为 Estimator 类
processIMU
进行预积分计算setReloFrame
添加重定位约束processImage
处理图像特征点因此接下来将逐个介绍相关内容
processIMU
预积分计算由预积分类IntegrationBase
实现,该类重载了push_back 函数,相当于将imu数据压入该类后就自动计算预积分的更新量。
setReloFrame
添加重定位约束在函数中并未对数据进行处理,仅仅是保存得到的数据
processImage
处理图像特征点对于图片特征点数据的处理是算法中最为核心且复杂的内容之一,主要的流程图如下
因此核心实现函数如下:
addFeatureCheckParallax
initialStructure
solveOdometry
slideWindow
几个重要的点
对于前面两点,可以参考代码注释和计算机视觉相关的书籍弄懂,这里仅对IMU与视觉数据对齐方式做如下说明
数据对齐的目的在于通过对相机位姿的缩放和平移得到一个与IMU估计一致的估计状态。
主要流程如下
具体初始化流程与论文第五节B中进行了详细的描述
这里并没有做滑动窗口的概率转移相关的计算,单纯的滑动删除一帧
三角化特征点
对于一个特征点在大于两个关键帧内被看到,则能够求得其深度,而多于两帧则采用线性最小二乘的方式进行求解,代码中构造矩阵利用svd分解对其进行求解,具体原理可参考多视图几何一书。
调用ceres库对整体进行优化求解
注意:这里仅优化了相机的位姿而没有优化地图点
构造图优化问题
后处理部分
具体如何求解请参考原论文以及相关资料的理论部分,这里并不打算进行理论部分的说明,后期专门讲理论的部分会补充相关理论。
在滑动窗口内对窗口内的数据进行处理后将数据发布到整体的位姿图上进行最终的整体优化过程。
odometry
imu_odometry
key_pose
extrinsic
keyframe_point
relo_relative_pose
位姿图代码主要运行在两个线程中,其中一个进行回环检测而另外一个进行四自由度的位姿图优化
下面的图能够很好的表达该模块的工作流程。
首先我们进行回环检测得到了一个约束项,然后发送给vins_estimator模块作为一个固定约束,因此整个滑动窗口的值都会被矫正到回环之后的位置,然后在另外一个线程进行位姿图的优化,对之前所有关键帧相互之间的约束加上回环约束得到整个图在回环校准后的位姿。
posegraph.addKeyFrame(keyframe, 1);
detectLoop