s-msckf代码笔记（一）

s-msckf主要由image_processor跟msckf_vio两个节点组成。

Image_processor中主要的算法位于ImageProcessor::stereoCallback()。

一 前端图像处理

image_processor::stereoCallback()接收双目图像数据，首先构建图像金字塔，用于光流跟踪、双目匹配，保存在curr_cam1_pyramid_，curr_cam1_pyramid_中，然后判断是否为第一帧。

1 视觉初始化

接收到的图像是第一帧，进行初始化initializeFirstFrame()。
initializeFirstFrame()：
(1)把左图像均匀划分成若干个格子，默认参数是把左图像划分成4x4个格子。
(2)检测左图像中的fast特征。
(3)立体匹配stereoMatch()，保留匹配成功的点。（待详细）
(4)把左右相机匹配上的点cam0_inliers和cam1_inliers分配到预先划分的格子grid_new_features里，grid_new_features涉及的数据类型是：

struct FeatureMetaData {
    FeatureIDType id;
    float response;
    int lifetime;//连续跟踪次数
    cv::Point2f cam0_point;//左图像坐标
    cv::Point2f cam1_point;//右图像坐标
  };
  
typedef std::map<int, std::vector<FeatureMetaData> > GridFeatures;

图像中每个格子都有一个索引，在默认4x4个格子的情况下，从左上角到右下角的索引是0~15。对每个格子中的特征点，根据其相应值response排序，取前n个，最终的结果保存在curr_features_ptr变量中。

2 图像跟踪

接收到的图像不是第一帧，进行跟踪。
这部分的执行流程为：trackFeatures() —> addNewFeatures() —> pruneGridFeatures()。

2.1trackFeatures()

（1）在imu数据队列中找到上一帧图像时间对应的imu数据，并用一个迭代器指向它，再在imu数据队列里找到当前图像帧时刻对应的imu数据，再设置一个迭代器指向它。计算这段时间内的imu的平均角速度${}^I\bar{\mathbf{\omega }}$，然后计算这段时间内在相机坐标系下的平均角速度：
$$\begin{gathered} {}^{c_0}\bar{\mathbf{\omega }}={}_{c_0}^I\text{R}{}^I\bar{\mathbf{\omega }} \\ {}^{c_1}\bar{\mathbf{\omega }}={}_{c_1}^I\text{R}{}^I\bar{\mathbf{\omega }} \end{gathered}$$
对应代码：

 // Transform the mean angular velocity from the IMU
  // frame to the cam0 and cam1 frames.
  Vec3f cam0_mean_ang_vel = R_cam0_imu.t() * mean_ang_vel;
  Vec3f cam1_mean_ang_vel = R_cam1_imu.t() * mean_ang_vel;

计算旋转角度，得到当前帧相机位姿关于上一帧的旋转向量，再根据罗德里格斯公式得到旋转矩阵并转置，得到cam0_R_p_c，cam1_R_p_c。最后删除已经经过积分的这一段imu数据，释放内存。

（2）预测经过旋转后的特征点位置。根据上一步计算出来的cam0_R_p_c，cam1_R_p_c，记它们分别为${}_{c_0}^p\text{R}$,${}_{c_1}^p\text{R}$。假设camera0中某特征点i在上一帧的像素坐标的齐次向量为
$${}_i^p\text{P}=\left(\begin{array}{c}{u}_p\\ v_p\\ 1\end{array}\right)$$
则旋转后的像素坐标的齐次向量为：
$${}_i^c\text{P}=\text{K}{}_{c_0}^p\text{R}{\text{K}}^{-1}=\left(\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right)$$
${}_i^c\bar{\text{P}}$除以$z_i$就可以得到旋转后的像素坐标:
$${}_i^c\bar{\text{P}}=(\begin{array}{c}{x}_i/z_i\\ y_i/z_i\end{array}$$