【VINS-Mono 初始化】Monocular Visual-Inertial State Estimator(一)

本文为原创博客，本文地址为：https://blog.csdn.net/q_z_r_s/article/details/100941451

机器感知 一个专注于SLAM、三维重建、机器视觉等相关技术文章分享的公众号

1. 测量值预处理

1.1 视觉前端处理

特征跟踪通过KLT光流法跟踪，为了避免特征点越跟越少，同时辅助的在每帧上在提取一些特征点，保证特征点数量在100-300之间。图像特征点首先去畸变，然后使用RANSAC方法计算基础矩阵F来剔除异常点，最后把这些特征点投影到单位球上。

关键帧的选择也是在这一步进行的，关键帧选择有两个准则：1. 当前帧距离上一个关键帧的平均视差；2. 跟踪质量评价，可视特征数量和能够跟踪特征数量的比值。

1.2 IMU预积分

IMU测量方程如下：

左值表示测量值，$b_{a_t}$和$b_{{\omega }_t}$表示加性测量偏置噪声，$n_{\omega }$和$n_a$表示加性测量高斯噪声。其中$b_{a_t}$和$b_{{\omega }_t}$满足如下特性：

总之，这两类噪声量都认为是测量不准确的根源，其中偏置噪声是需要进行估计的状态变量之一。为了能够及时的处理IMU测量得到的数据，这里使用了一个小trick，就是所谓的预积分，预积分部分公式如下：

可以看出，两帧之间的IMU测量数据积分是以上一帧的载体坐标系为参考系进行的，且积分项中并不需要知道当前帧的PVQ信息，唯一需要的只有IMU当前的偏置状态。

当积分过程中偏置估计值发生变化时，就需要对偏置变化带来的影响进行校正，这里分两种情况：

a. 当偏置变化不大时，直接对式(3)进行关于偏置状态的线性化：

然后根据偏置增量，修正预积分获得的增量，避免预积分重新传播，节省了计算量。

b. 当偏置变化较大时，这是就没办法了，只能对预积分进行重新传播了，没啥办法~~~

2. 系统初始化

2.1 Vision-Only SfM in Sliding Window

初始化开始于基于视觉的SfM，以此来估计相机位姿及特征点位置，为了限制计算量，这里仅使用一定数量的帧进行，即所谓的Sliding Window。首先，检查最新帧和以前的帧(程序中先从第一帧开始找，因为与最新帧里的更远，视差更大)之间的特征对应关系，如果有足够多的特征匹配及充足的视差，使用5(对)点法(程序中使用的是cv::findFundamentalMat，然后使用cv::recoverPose)恢复这两帧之间旋转和带有尺度的平移，然后就是根据某些东西对平移进行缩放，然后三角化特征点，接下来就是3D-2D，即PnP(cv::solvePnP)解算位姿了，最后对滑窗中所有的帧及3D点进行最小化重投影误差的全局BA优化，优化位姿和3D点。由于现在还没法确定世界坐标系，因此设置第一帧为SfM的参考坐标系，所有帧的位姿、3D点位置都是相对于第一帧来表示的。

2.2 Visual-Inertial Alignment

2.2.1 Gyroscope Bias Calibration

首先是矫正陀螺仪偏置量，方法很简单，就是整个IMU积分得到的各帧位姿与其前一帧位姿的变化量与IMU预积分得到的两帧间位姿，理论上，如果没有任何误差，它们的乘积应该为单位阵，所以就以两种增量间的乘积关系为优化目标，代价函数如下：

因为是要优化陀螺仪偏置，所以我们需要对两帧间的四元数表示的姿态预积分进行关于陀螺仪偏置量的扰动，如式(2)第二式所示。然后就可以使用比如GN、L-M、DogLeg等方法最小化代价函数了，由此可得到一个初始的陀螺仪偏置量。接下来要做的就是根据刚刚得到的这个偏置量(这里认为整个滑窗中的陀螺仪偏置都是一个值，即它们有相同的偏置)，对所有的IMU预积分进行重新计算。

2.2.2 Velocity, Gravity Vector, and Metric Scale Initialization

接下来就是初始化导航中很重要的状态了，即速度、重力矢量、尺度，状态变量如下：

这里初始化的重力矢量是第一帧坐标系下观测到的重力矢量，再根据导航坐标系下的重力矢量，即(0,0,g)，即可得到第一帧与导航坐标系间的位姿关系。接下来就是找到视觉约束和IMU约束间的等价关系，这样就可以建立误差方程，优化我们想优化的变量了，现在能用的有PVQ，这些在可以同时从视觉和IMU中获得，与初始化陀螺仪偏置类似，同样是使用预积分来做，具体如下：


左值可通过预积分得到，右值可通过视觉得到；又因为我们想知道速度、尺度、重力矢量，因此我们必须把它们引入到式中，故使用简单的运动学公式即可得到预积分与待求变量之间的等式关系。接下来就是将式(9)写成状态方程的形式，将式(6)中第二个方程带入式(9)中，将$p_{c_x}^{c_0}$替换成$p_{c_x}^{c_0}$，然后根据式(8)提取矩阵，结果如下：


其中，每个残差行数为6，由于一次涉及到两帧，所以列数为10(3(速度)+3(速度)+3(重力矢量)+1(尺度))。有了式(10)建立的预积分与视觉里程计之间的约束之后，接下来就是通过最小化两者之间的残差来获得我们所需要的状态变量了，优化目标如下：

经过这一步之后，我们就可以得到所有帧在第一帧下的速度、重力矢量及尺度因子了。

2.2.3 Gravity Refinement

接下来就是微调重力矢量，由于重力大小是已知的，所以实际重力只有两个自由度，故这里只需在重力的正切空间上进行2自由度的扰动即可，使用的方程还是式(9)进行的，只不过只对重力KaTeX parse error: Expected group after '_' at position 2: g_̲^{c_0}进行扰动，同时将上一步得到的KaTeX parse error: Expected group after '_' at position 2: g_̲^{c_0}不在作为状态变量，而是带入方程中作为初始值，取而代之，$\delta g$成为状态变量，再次解方程之后就可得到重力矢量的更新量了。

2.2.4 Completing Initialization

经过上一步，我们就有了在第一帧$c_0$下的重力矢量KaTeX parse error: Expected group after '_' at position 2: g_̲^{c_0}，再根据真正的重力矢量即可得到第一帧与世界坐标系之间的相对旋转矩阵了，可用施密特正交化来做，代码中使用的是：Eigen::Quaterniond::FromTwoVectors来计算的。

至此，初始化部分就完成了，接下来就是所谓的紧耦合VIO部分了。