2007单目 vs 2017双目
MSCKF从单目扩展到双目的改动：
前端主要功能：
1、双目特征跟踪
1 ) 左图前后帧跟踪：对上一帧左图特征点，在当前帧左图进行跟踪，获得当前帧左图的跟踪特征点
2 ) 左右图跟踪：对当前帧左图特征点，在当前帧右图进行跟踪，获得当前帧右图的跟踪特征点
3 ) 左右图前后帧RANSAC：分别对左右图，进行前后帧2-points RANSAC，剔除外点。只保留左右目都通过RANSAC的特征点。
2、新双目特征提取
1 ) 提取左图特征：在当前帧左图上，构建mask（记录已有特征在图像上的位置）对图像剩下区域提取FAST角点，将新的角点投射到网格中，如果网格中特征点满了则删除该新角点。
2 ) 左右图跟踪：将当前帧左图新提取的特征点，对应到当前帧右图，获得当前帧右图的跟踪特征点。
3、特征均匀化采样
为使特征点在图像上均匀分布，将图像分成了4*5个网格(grid)，每个网格中最多4个特征点，这样避免了特征点集中在图像上的某一小块。
注意：左右图的特征点成对出现，对于在右图中跟踪失败的点，在左图特征点会被删除。
特征跟踪流程中的跟踪主要有两种：前后帧跟踪和左右图跟踪，二者都是调用LK光流。
区别在于：
（1）前后帧跟踪用了IMU估计相机旋转来预测特征点初值
（2）左右图跟踪用的左右相机外参，左右图跟踪还加了极线约束，所以匹配准确性更高。
（使用IMU和相机位姿以及特征点进行位姿估计）
算法步骤：
其状态向量包括：IMU状态和N个历史相机状态，imu误差状态向量如下：
2007年发表的msckf（15维）：
2017年发表的msckf（21维）：
除旋转量以外，其他误差项都是直接相加(真实值=估计值+误差值)
IMU Propagation过程：利用两帧图像之间的所有IMU观测数据（加速度 am和角速度wm），对MSCKF的状态向量和协方差进行迭代预测。它相当于EKF中的预测过程。
不考虑噪声项时，imu测量值为：
IMU误差状态可表示为：（连续模型微分方程）
15x15的F，15x12的G表达式如下：
IMU误差状态转移方程：（离散模型）
转换关系：
直接对状态估计量进行迭代预测（而非误差状态向量）；
协方差是根据误差状态的运动模型进行迭代；
每一帧的imu数据预测量包括位置、速度、旋转：p,v,q。ba和bg保持不变。
1）姿态预测：
当|w|>1e-5时：
当|w|<1e-5时：
2）位置预测：
速度采用的4阶Runge-Kutta积分：
3）速度预测：
常用的三种数值积分：Euler积分、Mid-Point积分、Runge-Kutta积分(4阶)。它们的精度依次从低到高、计算量依次从小到大。令 [k2]的权重为1，其他为0，RK4就退化为Mid-Point法；令 [k1]的权重为1，其他为0，RK4就退化为Euler积分。
s-msckf中IMU状态 协方差Pii（21x21）:
总状态向量的协方差 P （21 + 6N）：
每一个imu测量值到达时都需进行：imu状态传播、imu状态协方差更新。
MSCKF的状态向量：
包括IMU状态和N个相机状态
每来一帧图像后，先根据IMU Propogation对IMU状态进行预测，然后根据相机外参计算当前帧的相机位姿，将最新相机状态加入到状态向量中并扩展协方差矩阵，这就是状态扩增（State Augmentation）
通过当前的imu位姿和imu-cam外参，计算当前相机的位置p与姿态q：
将新的相机状态（新增6维）加入到状态向量中。
原状态协方差矩阵为（21+6N）x（21+6N），现在新增6维；
2017：（imu状态21维）
新增相机状态对原状态向量之间的Jacobian【6x（21+6N）】，由相机位姿计算公式对 21+6N 维状态量求偏导。
新加入的相机状态与历史相机状态无关，只与IMU状态有关。
2007：（imu状态15维）
MSCKF中，IMU Propagation只改变IMU状态向量和其对应的协方差，与相机无关；而Measurement Updata的观测模型是残差相对于相机状态的观测模型，与IMU状态没有直接关联。状态扩增就相当于相机和IMU状态之间的桥梁，通过关联协方差 Pic 描述相机和IMU状态之间的关系，每一个相机状态都与IMU状态形成关联，这样在观测更新相机状态的同时，会间接更新IMU状态。
MSCKF观测更新的原理：一个静止的特征点被多个相机位姿观测到，从而对观测到该特征点的多个相机产生约束（即多个相机对该特征点的观测射线应该汇聚在一点）。
MSCKF以此为出发点，先根据多个观测三角化特征点的空间坐标，然后定义特征点到各相机的重投影误差（残差 [r]）作为观测量，并推导残差与状态向量之间的线性化关系，即残差观测模型:
其中：H为残差对误差状态向量的jacobian。
选取Cn为相机参考位姿，则 fj 在不同图像上的观测值可通过相机的相对位姿关系，转换到其他相机坐标系（在归一化平面的观测）：
设fj在C1，C2，…，Cn相机下的观测分别为 z1,…zn，最小二乘优化问题：
得到世界坐标系下特征点 f j 的3d坐标：
单个特征点对单个相机有一个残差模型，残差定义为：
通过上一步，已知：世界坐标系下特征点 fj的3d位置、相机i位置的位姿估计（由imu状态得到）
-> 将特征点 f j 从世界坐标系，转到相机 i 位置的相机坐标系：
-> 特征点 f j 在相机 i 位置的2d投影（归一化平面的像素坐标 ）满足下式：
-> 将所有观测到 特征点f j 的相机的残差聚集起来，i属于1~n
将所有特征点的观测模型合并得到完整的观测模型， j属于1~m
1）单个特征点对单个相机（双目相机）的观测模型
根据残差公式，残差 r（2x1）与3部分有关：
（1）与相机的状态有关
（2）与特征点的位置误差有关
（3）与图像噪声有关
单相机：残差（2x1）
双相机：残差（4x1）
jacobian详细：
2）单个特征点对所有相机的观测模型合并
将单个特征点对所有相机的观测模型按行堆叠，得到单个特征点的观测模型：
2007 15维imu状态：
有M个相机位置都观测到了特征点 f j;
单相机：残差（2x M x1）
根据上式，残差 r 除了与状态向量有关，还与 特征点的3d坐标有关，与理想的观测模型不一致。
所以需要将特征点从观测模型中移除
移除方式是观测模型两边乘以【 Hfj 的左零空间】
左零空间的定义：
观测模型两边同时乘以 [VT]，得到只与状态向量相关，与特征点坐标无关的观测模型：
为什么左零空间消除特征项这一步是在将单个特征点的所有重投影模型聚合之后？原因有几点：
单个特征点对单个相机的重投影模型中 [公式]的维度为 [公式]，左零空间不存在，因为 [公式]的解不存在，当 [公式]的行数大于 [公式]的列数时，才存在左零空间。所以特征点至少要被两个相机观测到 [公式]才可以用于观测更新。
对于单个特征点来说，到所有相机的残差模型中都包含 [公式]项，所以可以合并后用左零空间消除；而不同特征点的观测模型中 [公式]项不一样，所以要在不同特征点的观测模型合并之间进行消除。
3）所有特征点的观测模型合并
对所有特征点的残差模型进行合并，得到整体残差模型：
当Hx的行数>列数时，进行QR分解，减少Hx的行数：
所以最终用于EKF更新的观测模型为：
EKF更新公式：
与常规EKF更新公式唯一的区别在状态向量更新量直接是 [Kr]
协方差更新公式使用的对称正定式。
协方差更新有三种等价形式：
1 常规，数值稳定性较差，计算出的协方差不能保证是对称正定阵。
2 对称形式，计算出的协方差仍是对称阵。
3 对称正定形式，计算出的协方差是仍是对称正定阵。
剔除离群点包含上述(3)(4)(5)三个步骤：在上一帧与当前帧的cam0、cam1之间不断循环。
prev frames cam0 ----------> cam1
| |
|ransac |ransac
| stereo match |
curr frames cam0 ----------> cam1
1) 当前图像 cam0 与 cam1 进行双目匹配.
2) 上一帧图像与当前图像的 cam0 进行RANSAC .
3) 上一帧图像与当前图像的 cam1 进行RANSAC .
对于步骤3，不再需要图像间的跟踪，将双目匹配结果直接用于RANSAC。
通过以上步骤，特征跟踪得到了当前左右两图像的特征点。
4. 当前图像提取新特征点addNewFeatures()
（1）在左图上提取新特征：
detect(cur_img, new_features)
mask:避免提取已经存在的特征
（2）双目匹配，获取右图像上的新特征
measurementUpdate在两种情况下触发：
1 特征跟丢了需要移除特征时（removeLostFeatures）
2 相机状态数量达到最大限制需要剔除掉相机状态时（pruneCamStateBuffer）
使用误差状态的优点