MsckfVio Estimator/Filter 纸上运行

0.主要数据结构

本文简要分析MsckfVio Estimator/Filter细节，例如数据管理、矩阵维度等。具体理论逻辑结合参考这篇文章。

前端数据接收：

state_server:

map_server:

1.前端图像帧模拟

2.第一帧图像到来

状态增广 + 协方差矩阵增广：

疑问：求导时，$t_{c2w}$ 不是使用的误差状态，而是使用的状态求导，为什么？

第二三帧图想到来，没有特征点跟丢，无需三角化处理，但是依然要增广相机状态：

3.第四帧图像到来

特征点 f3,f4,f5,f6跟踪三帧，第四帧跟丢，进行三角化：

  // All camera poses should be modified such that it takes a
  // vector from the first camera frame in the buffer to this
  // camera frame.  // ! 求 R_{c_02c_i}
  Eigen::Isometry3d T_c0_w = cam_poses[0];
  for (auto& pose : cam_poses)
    pose = pose.inverse() * T_c0_w;

三角化完成后就开始计算相机测量模型：

线性化过程求偏导，是观测量 $z$ 对所有状态量求偏导：