vins-fusion代码解读[一] vio主体

SLAM新手,欢迎讨论。

港科大vins-fusion代码解读[一]

vins-fusion与vins-mono代码结构有很大相似性。这次先看看vins_estimator节点内的内容。

1.程序入口:

(1)vins_estimator:: rosNodeTest.cpp

main()函数中:

a.读取配置文件参数 readParameter()
b.订阅了四个话题,分别是imu消息,两个相机图像,和feature_tracker所提供的跟踪光流点
c.同时开启了一个新线程sync_process。线程的作用:如果图像buffer里面有数据的话,读入数据并且添加到estimator中。为什么不在相机图像的回调函数就input,我的理解在于对于双目的话,能够检测同步问题,能够将同样时间戳的两帧图片同时放入estimator中。所以对于Imu以及feature直接在回调函数中进行添加。

Estimator estimator;

然而整个程序并不是从main()中开始的,而从Estimator estimator;开启。
类的初始化函数Estimator(),由于Estimator类成员内部有两个比较重要的自定义类成员:
(1)Feature_Tracker featuretracker;(以前vins-mono这部分是作为一个独立的Node存在):
用来对原始图像进行畸变校正,特征点采集,光流跟踪
(2)FeatureManager f_manager;
用来对滑动窗口内所有特征点的管理。
简单设置了一些参数后,系统进入main()。

接着main()与Estimator estimator两者开始发生联系:
main()中estimator.setParameter()开启了滑动窗口估计的一个新线程
由于我们在配置文件中 多线程MULTIPLE_THREAD设置为1,因此当setParameter()时候,就开启了一个Estimator类内的新线程:processMeasurements();

processMeasurements();

处理各种buffer里面的东西,当featureBuf不等于空时,开始进行以下处理(为什么是featureBuf,因为当有图像buffer数据的时候,才会有featuretracker.push(make_pair(t,featureFrame)),即有图像数据后,程序才发给跟踪器叫他产生feature,因此当featureBuf不等于空,所有的buffer,包括imu,图像,都不为空):
processIMU(): 对IMU进行预积分
processIMage():对图像进行处理
pub VIO的各种话题,包括里程计信息,tf变换,相机姿态,点云信息,并且发布关键帧。

processIMage()

1。通过特征点的视差(按照vins-mono的说法,是基于imu旋转补偿后特征点的视差)来判断是否是关键帧,并对应边缘化的flag;
2。判断相机到IMU的外参(R,T)是否有校正,没校正用手眼标定法进行标定,具体体现在CalibrationExRotation里面,注意这里面只有标定旋转矩阵,没有标定平移矩阵,按照沈老师讲座上说的,外参中主要是R对系统比较敏感,如果偏差一两度,系统很容易就蹦。
3。判断是否有进行初始化:
(1)如果初始化已经完成,则就optimization(),用ceres_solver进行滑窗内(11帧)进行非线性优化的求解。按照vinsmono论文中,主要有三项,边缘化残差, imu残差,相机重投影残差。不过其实在代码中,又加入新的残差项,有相机Imu之间同步时间差的残差项。另外在处理图像的这个函数中,同时有一个failureDetection(),满足一定条件下系统认为系统已经挂掉了,例如非线性求解器中的解有大跳动,求解出相机IMU的外参矩阵或IMU偏移等等,系统挂掉就清空状态,重新初始化。
(2)如果没有初始化,则要进行相应的初始化工作。

初始化

初始化分为三种情况:
(1)单目加imu:
相机Imu外参标定好了之后,时间过了0.1秒,就开始进行初始化工作,函数表现在initialStructure()中
(2)双目加imu
双目pnp求解出滑窗内所有相机姿态,三角化特征点空间位置。得到这些之后,再进行陀螺仪漂移的估计。体现在solveGyroscopeBias()中。更新得到新的陀螺仪漂移Bgs,对之前预积分得到的结果进行更新。预积分的好处就在于你得到新的Bgs,不需要又重新再积分一遍,可以通过Bgs对位姿,速度的一阶导数,进行线性近似,得到新的Bgs求解出MU的最终结果。
(3)双目
双目pnp求解出滑窗内所有相机姿态,三角化特征点空间位置。

上面这些都是需要满足一定条件,比如单目加imu中,初始化需要imu至少两轴的激励足够大,并且跟踪到的特征点也需要满足一定的条件。

initialStructure()

1.第一步就是要检测IMU的可观性,即IMU的激励如果不够的话,会造成尺度的不客观。
2.接着使用sfm,pnp来求解出滑窗内所有帧的姿态,此时姿态是在尺度s下面的,即没有尺度信息。(单目没有办法得到尺度信息)
3.接着visualInitialAlign(),用来做视觉与IMU之间的融合,直观上讲,把纯视觉得到的结果拉拉扯扯一下,扯到IMU的尺度上来,并且又进行求解陀螺仪的bias,solveGyroscopeBias().同时LinearIMUAlign()求解得到尺度s,同时又进行RefineGravity(),对重力加速度的方向的大小进行求解。最后将第一帧相机的位姿调整到与重力加速度对齐的方向上来,即第一帧相机的z轴与重力加速度平行。这个就是代码中注释的//change state,下面做的东西

optimization()

google中ceres的问题,待估计的参数包括滑窗内所有帧的位姿,速度,加速度的漂移,陀螺仪的漂移(前面三项体现在para_speedBias里面,只一个9自由度的向量),以及特征点的深度(这一项是让整个非线性优化维度变得很高的主要原因,但是矩阵稀疏,有方便的求解方法)。如果相机到IMU的外参在配置文件没有足够的勇气设置成0,这两项R,T也会作为参数进行估计,另外如果使用比较low的自制VIsensor没有做到相机IMU同步,这两个传感器的时间差td也会作为一个参数来进行非线性优化(配置文件estimator_td如果你没有勇气设置成0的话,这一项也会进行估计)。单目的残差项与VINS-mono一致,但是双目残差项多了两项。还未看懂,谢谢惠顾!

你可能感兴趣的:(SLAM,ROS)