MSCKF浅析

MSCKF_VIO浅析

MSCKF 是明尼苏达州大学Mourikis 等人提出的一种基于EKF 的VIO 紧耦合的SLAM 框架。该框架主要解决了使用传统EFK方式解决SLAM问题时，状态向量中特征点维度爆炸的问题。MSCKF的创新点在于未将路标点加入到状态向量中，而是将多个相机状态扩维到状态向量中，多个相机对路标点的观测作为一种约束，整合到EKF更新过程中。

S_MSCKF是宾夕法尼亚大学Vijay Kumar 实验室开源的双目版本MSCKF 算法。代码简洁明了，数据结构设计的简单易用，质量比较高！相比于基于优化的VIO算法，MSCKF精度相当（可以看S_MSCKF的对比试验），但是计算量较小。

我注释过的代码见here。

1、Image Processor

此node是MSCKF视觉前端，进行双目光流跟踪。

1.1、简要流程

本node接收原始双目图像以及IMU原始测量：

借助message_filters::TimeSynchronizer<>同步接收双目图像，第一帧图像后，不断添加imu消息到imu_msg_buffer；
建立图像金字塔；
若是第一帧图像，先提取第一帧图像中左眼的fast角点；之后做双目匹配（以双眼外参为右眼中的角点提供初值，进行LK光流跟踪；将落在图像之外的点标记为outliers，根据点到极线（极线通过双目间外参得到）的距离再标记一波outliers。）；去除双目特征点中的outliers（左右眼上特征点的数量相同！）；4x5方格的均匀化处理。
前后帧特征跟踪：前后帧跟踪会用IMU积分的相对旋转预测特征点在当前帧的位置作为初值，左图特征点前后帧跟踪，得到当前帧左图特征点，当前帧左右图跟踪（双目匹配），得到当前帧右图特征点，左右图分别做前后帧two Point RANSAC剔除外点；4x5方格的均匀化处理。
添加新特征点：新提取左眼fast角点；左眼4x5方格均匀化处理；双目匹配，去除外点。

1.2、重点函数

/**
 * @brief      Applies two point ransac algorithm to mark the inliers in the input set.
 *
 * @param[in]  pts1                 previous set of points.
 * @param[in]  pts2                 current set of points.              
 * @param[in]  R_p_c                a rotation matrix takes a vector in the previous camera frame to the current camera frame.
 * @param[in]  intrinsics           intrinsics of the camera.
 * @param[in]  distortion_model     distortion model of the camera.
 * @param[in]  distortion_coeffs    distortion coeffs.
 * @param[in]  inlier_error         acceptable error to be considered as an inlier.
 * @param[in]  success_probability  the required probability of success. Ransac算法的置信度
 * @param[out] inlier_markers       1 for inliers and 0 for outliers.
 */
void ImageProcessor::twoPointRansac(
    const vector<Point2f> &pts1, const vector<Point2f> &pts2,
    const cv::Matx33f &R_p_c, const cv::Vec4d &intrinsics,
    const std::string &distortion_model,
    const cv::Vec4d &distortion_coeffs,
    const double &inlier_error,
    const double &success_probability,
    vector<int> &inlier_markers);

twoPointRansac()外点去除方法没有手撸过（一般是通过cv::findEssentialMat(...)或者cv::solvePnPRansac(...)集成的Ransac算法实现）。

Ransac算法原理：

随机抽样一致性（RANSAC）算法，可以在一组包含“外点”的数据集中，采用不断迭代的方法，寻找最优参数模型，不符合最优模型的点，被定义为“外点”。以OpenCV求解单应矩阵为例子，算法流程为：

随机从数据集中随机抽出4个样本数据 (此4个样本之间不能共线)，计算出变换矩阵H，记为模型M；
计算数据集中所有数据与模型M的投影误差，若误差小于阈值（需要设定），加入内点集 I ；
如果当前内点集 I 元素个数大于最优内点集 I_best , 则更新 I_best = I，同时更新迭代次数k ;
如果迭代次数大于k，则退出 ; 否则迭代次数加1，并重复上述步骤；
然后，用所有假设的局内点去重新估计模型（譬如使用最小二乘法），因为它仅仅被初始的假设局内点估计过；
最后，通过估计局内点与模型的错误率来评估模型。

迭代次数的更新公式为：
$$
k = \frac{log(1-p)}{log(1-w^m)}
$$
其中，$p$是置信度，一般取为0.995；$w$是由模型M计算出来的内点的比例；$m$是计算模型所需要的最少样本数。这个公式说明，内点的比例越高，需要的迭代次数就越少。

上述过程被重复执行的过程中，每次产生的模型要么因为局内点太少而被舍弃，要么因为比现有的模型更好而被选用。

其实核心就是随机性和假设性。随机性用于减少计算了，循环次数k就是利用正确数据出现的概率。所谓的假设性，就是说随机抽出来的数据都认为是正确的，并以此去计算其他点，获得其他满足变换关系的点，然后利用投票机制，选出获票最多的那一个变换。

代码中并非恒定将$t_z$设定为1，而是根据方程（7）的系数矩阵，将系数矩阵按列向量求解一范数，范数最小的列对应的$t$设定为1，所以代码中分为了三种情况。

2、MsckfVio Filter

2.1、核心要义

这部分是Msckf的核心，网上相关的参考资料比较丰富，例如Gavin Gao的推导，以及知乎MSCKF那些事。我丰富了Gavin Gao的推导以及纠正了一些错误。在此之前，给出KF和EKF的常规五步公式：

MSCKF核心思路下面这个PDF写的比较详尽，具体的Jacobian推导MSCKF那些事和代码一致。

2.1、TODO

能观性分析
松耦合的回环检测，利用ov_secondary松耦合实现（回环结果不会反馈给滤波器）。

GWH Blog