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一致，但是双目残差项多了两项。还未看懂，谢谢惠顾！