vins 解读_VINS-mono详细解读

VINS-mono详细解读

极品巧克力

前言

Vins-mono是香港科技大学开源的一个VIO算法，https://github.com/HKUST-Aerial-Robotics/VINS-Mono，是用紧耦合方法实现的，通过单目+IMU恢复出尺度，效果非常棒。

感谢他们开源，我从中学到了非常多的知识。源码总共有15000多行，我在通读完程序之后，结合参考文献，把程序背后的算法理论都推导了一遍，总结成了本文，与各位分享。

本文目标读者：对vins-mono有一定了解的SLAM算法工程师。由于程序里有非常多的实现细节，建议读者在读本文前，先读一遍vins-mono的程序。

1.特征点跟踪

首先用cv::goodFeaturesToTrack在第一帧图像上面找最强的150个特征点，非极大值抑制半径为30。新的特征点都有自己的新的对应的id。然后在下一帧过来时，对这些特征点用光流法进行跟踪，在下一帧上找匹配点。然后对前后帧中这些匹配点进行校正。先对特征点进行畸变校正，再投影到以原点为球心，半径为1的球面上，再延伸到深度归一化平面上，获得最终校正后的位置。对于每对匹配点，基于校正后的位置，用F矩阵加ransac来筛选。然后再在匹配上的特征点之外的区域，用cv::goodFeaturesToTrack搜索最强的新的特征点，把特征点数量补上150个。

最后，把剩下的这些特征点，把图像点投影回深度归一化平面上，再畸变校正，再投影到球面上，再延伸到深度归一化平面上，得到校正后的位置。把校正后的位置发送出去。

特征点跟踪和匹配，就是前一帧到这一帧的，一帧帧继承下去。或者生成新的特征点。

2.初始化

2.1外参中的旋转

用机器人手眼标定的方法，计算出外参中的旋转。

其中

是陀螺仪预积分得到的，

是用8点法对前后帧对应的特征点进行计算得到的。详细见《Monocular Visual-Inertial State Estimation With Online Initialization and Camera-IMU Extrinsic Calibration》。

2.2 SFM

先在关键帧窗口里面，找到第l帧，第l帧与最后一帧有足够的像素位移，并且能用8点法算出旋转和位移。以l帧的姿态为世界坐标系。先从l帧开始与最后一帧进行三角定位，再用pnp估计出下一帧的位姿，下一帧再与最后一帧三角定位得出更多的三维点。重复到倒数第二帧。从l帧开始往第一帧，逐渐帧pnp，再与第l帧进行三角定位得到更多的三维点。每帧pnp时的位姿初值都用上一个关键帧的的位姿。剩下的那些还没有被三角定位的特征点，通过它被观察到的第一帧和最后一帧进行三角定位。

固定住l帧的位置和姿态，固定住最后一帧的位置。因为这时候的图像位姿和点的位置都不太准，所以用ceres统一一起优化图像位姿和三维点位置，优化重投影误差。优化的测量值是，特征点在每帧中被观察到的位置，可以转成重投影误差约束。有关的自变量是，每帧图像的位姿，特征点的三维坐标。

优化完成之后，即用ceres优化出这些关键帧的位姿和地图点后，再用pnp算出在这段时间区域内的所有图像的位姿。每个图像的计算都用下一个关键帧的位姿来当pnp的初值。

程序里面没有求雅克比，而是用自动求导的方法。

2.3 计算陀螺仪的偏移

在2.1中已经根据连续图像的相对旋转算出相机和IMU间的外参旋转了，现在要再根据上一节2.2中的SFM算出来的各帧图像的相对旋转来计算出陀螺仪的偏移。

就是根据前后帧之间的根据陀螺仪预积分出来的旋转

与基于SFM图像算出来的旋转转换到IMU坐标系的相对旋转

之间的向量差的两倍。

在程序里面，每次算出的图像的姿态

，都会转换成

。然后在计算相对IMU的姿态时，就用

。

这里是采用了近似计算的方法。其实就是把角度的残差转换成了角轴差的形式。详见《从角轴到四元数微分方程》和《on-manifold详细解读》。

这个

关于陀螺仪的偏移求导，得到雅克比矩阵。然后再根据高斯牛顿法算出陀螺仪偏移。

算出陀螺仪的偏移以后，对于每一幅图像，利用缓存的每一个IMU数据，重新计算这一段时间的每帧图像对应的预积分，雅克比矩阵和协方差矩阵。

雅克比矩阵的初值是单位阵，协方差矩阵的初值为零。雅克比矩阵每次都乘以一个状态转移矩阵。协方差矩阵每次都左右乘状态转移矩阵，再加上一个噪声矩阵，噪声矩阵就是加速度和陀螺仪噪声所形成的噪声协方差矩阵。

2.4速度，重力和尺度对齐

从2.3之后，认为陀螺仪是准确的，每帧图像的姿态也都是准确的。

速度，重力和尺度对齐，其实就是，每一帧对后一帧的位置，速度的预测值，与当前值的误差。当前值

的位置，是SFM位置

通过外参

计算过来，给的。与这有关的自变量是每帧的速度，重力，尺度。速度，重力，尺度，给的初值都是零。

以第一帧和第二帧为例。

因为速度，重力尺度的初值全部都是零，所以第一步，这里残差的计算可以简化成如下。

但雅克比的计算，还是得用原来的表达式算。

虽然给的初值是零࿰