VINS源码笔记（一）

这段时间在看VINS，写一下总结。
VINS需要在ROS框架下运行，而自己用的是Mac，不能很好的支持VINS，于是在把代码搞熟之后自己发挥动手能力在Mac下把VINS搭建了一遍，效果如下。

VINS在ros框架下进行开发，主要由3个节点构成：feature_tracker, vins_estimator和pose_graph。对应代码位于同名文件夹下，其中config文件夹主要包含了不同demo的一些参数，camera_model主要包含了关于相机模型的一些算法，主要用于去畸变、投影等等。

VINS的算法功能主要包括前端图像处理、后端非线性优化（初始化、视觉惯性里程计、边缘化）、重定位（回环检测）、全局位姿图优化。其中主要开启了四个线程：图像光流跟踪、后端非线性优化、回环检测、全局位姿图优化。

视觉前端(feature_tracker节点)

对于每一帧新读取的图像，用KLT稀疏光流对特征点进行跟踪（cv::calcOpticalFlowPyrLK），然后用OpenCV的goodFeaturesToTrack()函数检测新的特征点，从而保证每一帧有100到300个特征点。通过设置最小分布距离，检测到的特征点会有一个相对均匀的分布。检测到的2D特征首先会根据相机内参进行去畸变处理，然后投影到归一化相机坐标系下。相邻帧间用cv::findFundamentalMat()函数计算基本矩阵，这一步中函数算法设置为cv::FM_RANSAC，主要目的是根据函数输出的status变量来剔除光流跟踪中的误匹配点。VINS对图像设置了一定的发布频率，默认值为10Hz(euroc demo)。如果当前帧是发布帧，则把当前帧特征点的归一化坐标、图像中的像素坐标、特征点速度发布给vins_estimator节点。

后端非线性优化

初始化

初始化基于松耦合的方式，先将视觉Structure from Motion与IMU预积分分开进行，然后进行Visual-Inertial Alignment计算尺度、重力向量和IMU偏置，然后将滑窗中各帧位姿的z轴旋转至重力向量的方向。

VINS中默认滑动窗口大小为WINDOW_SIZE = 10，实际进行非线性优化时会多一帧，也就是11帧。滑窗中的图像信息和IMU信息保存在all_image_frame变量中，当all_image_frame中的图像数达到WINDOW_SIZE（+1）时，进行初始化。进行初始化有两个条件：

（1）足够的IMU激励，即运动速度有明显变化，VINS中通过计算加速度的方差来计算，如果方差小于0.25，则会给出IMU激励不够的警告。

（2）相机有足够的运动，从而保证特征点有足够的视差，对应relativePose()函数。

IMU预积分:

论文里面通常会把IMU数据和图像数据对的整整齐齐，然而实际情况下图像数据和IMU数据一般不会这么巧同时出现，多多少少会岔开一点，在VINS中，与图像同一时刻的IMU数据是通过插值得到的。

IMU测量模型：
$$\begin{gathered} \widehat{{\text{a}}_t}={\text{a}}_t+{\text{b}}_{a_t}+{\text{R}}_w^t{g}^w+{\text{n}}_a \\ {\hat{\mathbf{\omega }}}_t={\mathbf{\omega }}_t+{\text{b}}_{w_t}+{\text{n}}_w \end{gathered}$$
给定相邻的图像帧$b_k,b_{k+1}$，其连续时间的位置，速度，旋转可以由如下公式给出，积分得到的状态量作为非线性优化的初始值。
$$\begin{gathered} {\text{p}}_{b_{k+1}}^w={\text{p}}_{b_k}^w+{\text{v}}_{b_k}^w\Delta t_k+\int {\int }_{t\in [t_k,t_{k+1}]}({\text{R}}_t^w(\widehat{{\text{a}}_t}-{\text{b}}_{a_t}-{\text{n}}_a)-{\text{g}}^w)d{t}^2, \\ {\text{v}}_{b_{k+1}}^w={\text{v}}_{b_k}^w+{\int }_{t\in [t_k,t_{k+1}]}({\text{R}}_t^w(\widehat{{\text{a}}_t}-{\text{b}}_{a_t}-{\text{n}}_a)-{\text{g}}^w)dt, \\ {\text{q}}_{b_{k+1}}^w={\text{q}}_{b_k}^w\otimes {\int }_{t\in [t_k,t_{k+1}]}\frac{1}{2}\Omega ({\hat{\omega }}_t-{\text{b}}_{w_t}-{\text{n}}_w){\text{q}}_t^{b_k}dt \end{gathered}$$

等式两边乘以${\text{R}}_w^{b_k}$，
R w b k p b k 1 w = R w b k ( p b k w + v b k w Δ t k − 1 2 g w Δ t k 2 ) + α b k + 1 b k , R w b k v b k + 1 w = R w b k ( v b k w − g w Δ t k ) + β b k + 1 b k , q w b k ⊗