最近因为项目需求，对MSCKF_vio的论文和代码进行了一番研读，现将学习过程记下。
MSCKF_vio是一种基于多状态约束卡尔曼滤波器的双目视觉里程计。其中多状态约束是指将多帧图像的相机位姿加入卡尔曼状态向量中，在进行卡尔曼增益之前通过多帧图像之间的约束进行最小二乘优化来估计特征点的空间位置，然后根据优化后的特征点空间位置来约束状态向量。其中，多帧图像保存在一个按时间排序的滑动窗口序列中，跟踪多个特征点在多帧图像中的坐标，从而建立各帧图像位姿之间的约束。另一个约束是：同一时刻的相机位姿与IMU位姿之间是一个已知的约束，这个约束用在状态增广中。在这两种约束下，卡尔曼滤波能够获得一个较好的估计。该方法的优点在于鲁棒性高，计算复杂度低，而缺点是由于其后端是卡尔曼滤波，无法用到全局的信息进行全局的优化，没有回环检测。
代码中主要有三个文件比较重要：src/msckf_vio.cpp, src/image_processor.cpp ,include/msckf_vio/feature.hpp。其中src/msckf_vio.cpp是系统的主体，滤波器的所有步骤都在此文件中进行，包括状态预测，状态增广，测量更新等，是后端优化部分。src/image_processor.cpp是视觉前端，主要作用是跟踪特征点，去除outliers，其中用了三种方法去跟踪特征点：LK光流跟踪，双目匹配，two-point RANSAC。include/msckf_vio/feature.hpp主要是通过多帧图像之间的约束进行最小二乘优化计算各个特征点的空间位置，这个空间位置在msckf测量更新的时候要用到(计算测量雅可比矩阵H，计算残差)。
现在，先从前端部分开始，主要讲src/image_processor.cpp中整个特征点跟踪流程和各个函数的作用，以及two-point RANSAC算法的实现。
视觉前端
首先来看看ImageProcessor::stereoCallback(），该函数是双目图像的回调函数，直接对每帧图像原始数据进行处理。在该函数中，主要进行了如下步骤：
createImagePyramids()
建立图像的金字塔，这个用来作为之后的LK光流跟踪的输入数据。
initializeFirstFrame()
初始化第一帧：用FAST描述子检测特征点，然后通过双目匹配去除outliers。
trackFeatures()
这是前端部分的核心模块，通过三个过程进行特征点追踪：LK光流跟踪，双目匹配，two-point RANSAC。
addNewFeatures()
Fast检测特征点，通过双目匹配去除outliers，并将特征点根据坐标分类到图像网格中，在每个网格中特征点按响应值排序。
主要看看trackFeatures()函数的实现。
trackFeatures() 函数的实现流程
trackFeatures() 函数主要实现特征点的追踪，过滤误差较大，不稳地的outliers。追踪方法有三种：LK光流跟踪，双目匹配，two-point RANSAC。现逐个分析每个trackFeatures() 中主要函数的作用和原理。
integrateImuData()
用两帧图像之间的IMU数据，通过积分计算两帧图像的相对旋转矩阵cam0_R_p_c，cam1_R_p_c。这部分比较简单，直接对角速度进行积分获得旋转矢量，然后用罗德里格斯公式转化成旋转矩阵。
predictFeatureTracking()
通过integrateImuData() 函数预测的cam_0_R_c计算前一帧图像特征点在当前帧图像的对应坐标的初值。
由对极约束，对应前后两帧的特征点p1， p2 有：
s1p1 = KP s2p2 = K(RP + t)
由于前后两帧相差时间很短，忽略深度的差异，得到：
p1 = KP p2 = K(RP + t)
只考虑旋转，有：
p2 = K * R * K_inverse * p1
其中R即是IMU直接积分计算的前后两帧之间的相对旋转矩阵cam_0_R_c。
calcOpticalFlowPyrLK()
该函数是opencv中的函数：根据图像金字塔跟踪两帧间的特征点，跟踪失败的会将标志位track_inliers设为0，该函数需要设置第二帧图像中特征点坐标的初始值。
stereoMatch()
双目匹配，先用光流跟踪获取cam1对应于cam0的特征点坐标。然后通过对极约束原理，设x1，x2为特征点的归一化平面坐标，则有：
x2_T * E * x1 = 0
其中本质矩阵 E = t^ * R， t^为t向量对应的反斜对称矩阵，t和R分别为双目摄像头之间的平移向量和旋转矩阵，由初始标定时获得。
将特征点像素坐标通过undistortPoints()函数转化成归一化平面坐标x1，x2，最后计算x2_T * E * x1 的误差大小，误差大于一定阈值则标记为outliers。这里x2，x1相当于观测值（根据原始图像数据通过LK光流跟踪获得），用双目之间的约束（双目之间的相对位姿R和t）去验证光流跟踪的准确性，从而达到去除outliers的效果。
twoPointRansac()
通过two-point RANSAC方法去除outliers，这里稍后重点讲一下。
RANSAC算法基本思想描述:
①考虑一个最小抽样集的势为n的模型(n为初始化模型参数所需的最小样本数)和一个样本集P，集合P的样本数#§>n，从P中随机抽取包含n个样本的P的子集S初始化模型M；
②余集SC=P\S中与模型M的误差小于某一设定阈值t的样本集以及S构成S*。S认为是内点集，它们构成S的一致集(Consensus Set)；
③若#(S)≥N，认为得到正确的模型参数，并利用集S*(内点inliers)采用最小二乘等方法重新计算新的模型M*；重新随机抽取新的S，重复以上过程。
④在完成一定的抽样次数后，若未找到一致集则算法失败，否则选取抽样后得到的最大一致集判断内外点，算法结束。
现在看看MSCKF_vio中的具体步骤：
假设空间点P在两个相机中对应的像素点分别为p1， p2，则根据对极约束有：
其中 R，t分别为相机1到相机2的旋转和平移。
设 :
则:
即：
展开之后得到：
（1）
根据RANSAC原理，任取前后两帧匹配点，有：
（2）
其中，Ax=[ y1-y2 y3-y4]
那么通过将其中一项移至右边，可得到以下三个公式：
因此，问题回到矩阵方程求解： Ax=b。
取Ax ，Ay， Az中最小的一个，并且令其对应的平移t = 1。通过矩阵求逆来求解另外两个t， 例如假设Ax 最小，则令tx为1，则可得：
然后将求得的t作为模型带入式(1), 计算各个匹配点的坐标通过该模型计算的值作为误差（理论值为0，实际可能有误差），如果误差满足一定条件，则将匹配点设置为inliers。
最后会得到一个内点集，使用内点集中所有的内点（而不是两个点），带入公式(2)，用最小二乘法重新计算新的模型，并使用该模型计算最终的误差总和error_sum。（该步骤只是根据选出的内点计算新的误差，作为多次迭代模型好坏的评价标准）
通过重复上面整个过程，每次迭代先任选两个点初始化模型，用初始化模型去除误差较大的外点，然后用剩余的内点集构建新的模型，计算当前内点集的误差总和error_sum。在多次迭代中选取误差总和error_sum最小的一次内点集，即是最大一致性内点集，从而达到去除外点的效果。
下一篇：MSCKF_vio的主体内容，核心主要在于状态增广， 测量更新