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 > 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}}=\left(\begin{array}{c}{x}_i/z_i\\ y_i/z_i\\ 1\end{array}\right)$$
但是上述过程中并没有考虑图像的畸变，因此只是一种粗略的预测。

然后根据以预测的像素坐标作为初始值对做图像进行光流跟踪，去除跟踪失败的点和跟踪到图像区域外的点。

（3）去除外点（Outlier removal)。外点的去除分成三步：
a.将当前帧的左右图像的特征点进行立体匹配，去除匹配失败的点。
b.利用2-point RANSAC算法去除cam0的误跟踪点。
c.利用2-point RANSAC算法去除cam1的误跟踪点。

2-Point RANSAC:
a.首先计算2-Point RANSAC的迭代次数，根据公式：
$$N=\frac{\log (1-p)}{\log (1-(1-\epsilon {)}^s)}$$
其中s是数据点的个数，$\epsilon$是外点的比例，p是成功率。S-MSCKF中对应的代码为：

int iter_num = static_cast(
      ceil(log(1-success_probability) / log(1-0.7*0.7)));

b.对前后帧对应的特征点进行去畸变undistortPoints，得到归一化坐标。再将旋转矩阵乘以上一帧特征点的归一化坐标进行旋转补偿。

c.将上一帧经过旋转补偿后的归一化点（取前2维）$\{{\tilde{x}}_{p_i},{\tilde{y}}_{p_i}{\}}_n$跟当前帧的归一化点（前2维）$\{{\bar{x}}_{c_i},{\bar{y}}_{c_i}{\}}_n$进行归一化rescalePoints。归一化系数的计算公式为：
$$s=\frac{2n}{\sqrt{2}({\sum }_i\sqrt{{\tilde{x}}_{p_i}^2+{\tilde{y}}_{p_i}^2}+{\sum }_i\sqrt{{\bar{x}}_{c_i}^2+{\bar{y}}_{c_i}^2})}$$
然后将$\{{\tilde{x}}_{p_i},{\tilde{y}}_{p_i}{\}}_n$和$\{{\bar{x}}_{c_i},{\bar{y}}_{c_i}{\}}_n$乘以该系数，得到${\text{P}}_{s_1},{\text{P}}_{s_2}$。

c.${\text{P}}_{s_1},{\text{P}}_{s_2}$作差，得到$\Delta {\text{P}}_s$。根据阈值distance和$\Delta {\text{P}}_s$，标记内点和外点，并且计算内点的平均距离。如果内点数小于3，则把所有点标记为外点并且返回函数。这种情况在特征点较少且快速旋转的时候可能发生。

d.判断运动退化，即几乎没有平移的情况。此时RANSAC算法不能很好的工作，匹配点之间的距离几乎为0。这里用上一步计算的内点的平均距离是否大于一定值来判断有没有出现运动退化。

e.2-Point Ransac。前面的都是为这一步做准备。令${\text{P}}_{s_1},{\text{P}}_{s_2}$的齐次式为${\bar{\text{P}}}_{s_1},{\bar{\text{P}}}_{s_2}$,则存在对极约束：
$${\bar{\text{P}}}_{s_2}^T\text{E}{\bar{\text{P}}}_{s_1}=0$$
其中$\text{E}={\text{t}}_{\times }\text{R}$。由于${\text{P}}_{s_1}$已经经过了旋转补偿，因此$\text{R}$为单位阵，对极约束可以看成：
$${\bar{\text{P}}}_{s_2}^T{\text{t}}_{\times }{\bar{\text{P}}}_{s_1}=0$$
展开后得到：
$$\left(\begin{array}{ccc}{x}_2& y_2& 1\end{array}\right)\left(\begin{array}{ccc}0& -t_z& t_y\\ t_z& 0& -t_x\\ -t_y& t_x& 0\end{array}\right)\left(\begin{array}{c}{x}_1\\ y_1\\ 1\end{array}\right)=0$$

$$\left(\begin{array}{ccc}{y}_1-y_2& -(x_1-x_2)& x_1{y}_2-x_2{y}_2\end{array}\right)\left(\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right)=0$$

这跟以下代码是符合的：

vector pts_diff(pts1_undistorted.size());
  for (int i = 0; i < pts1_undistorted.size(); ++i)
    pts_diff[i] = pts1_undistorted[i] - pts2_undistorted[i];
    ...
    ...
    MatrixXd coeff_t(pts_diff.size(), 3);
  for (int i = 0; i < pts_diff.size(); ++i) {
    coeff_t(i, 0) = pts_diff[i].y;
    coeff_t(i, 1) = -pts_diff[i].x;
    coeff_t(i, 2) = pts1_undistorted[i].x*pts2_undistorted[i].y -
      pts1_undistorted[i].y*pts2_undistorted[i].x;
  }

2-Point RANSAC就是每次随机提取2对特征点，根据这2对特征点来求解$t_x,t_y,t_z$。在实际求解的过程中，把$t_x,t_y,t_z$其中一个置为1，然后求解剩余2个变量。至于把哪个置为1，则根据对应系数向量的一范数来选择。公式10可以看成：
$$\left(\begin{array}{ccc}a& b& c\end{array}\right)\left(\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right)=0$$
在2对特征点的情况下：
$$\left(\begin{array}{ccc}{a}_1& b_1& c_1\\ a_2& b_2& c_2\end{array}\right)\left(\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right)=0$$
比较左边矩阵各列向量的一范数大小，假设$(c_1,c_2{)}^T$的一范数最小，则令$t_z=1$，只需求解$t_x,t_y$。如果是$(b_1,b_2{)}^T$的一范数最小，则令$t_y=1$。以下推导$t_z=1$的情况，此时公式12可以化成：
$$\left(\begin{array}{cc}{a}_1& b_1\\ a_2& b_2\end{array}\right)\left(\begin{array}{c}{t}_x\\ t_y\end{array}\right)=-\left(\begin{array}{c}{c}_1\\ c_2\end{array}\right)$$
令左边系数矩阵为$\text{A}$,右边向量为$-\text{c}$,则待求结果为：
$$\text{t}=\left(\begin{array}{c}{t}_x\\ t_y\end{array}\right)=-{\text{A}}^{-1}\text{c}$$
这跟代码中的计算过程是一致的。

Vector2d coeff_tx(coeff_t(pair_idx1, 0), coeff_t(pair_idx2, 0));
Vector2d coeff_ty(coeff_t(pair_idx1, 1), coeff_t(pair_idx2, 1));
Vector2d coeff_tz(coeff_t(pair_idx1, 2), coeff_t(pair_idx2, 2));
...
...
A << coeff_tx, coeff_ty;
Vector2d solution = A.inverse() * (-coeff_tz);
model(0) = solution(0);
model(1) = solution(1);
model(2) = 1.0;

得到$\text{t}$之后计算各对特征点的误差：
$$e_i=\left(\begin{array}{ccc}{a}_i& b_i& c_i\end{array}\right)\left(\begin{array}{c}{\hat{t}}_x\\ {\hat{t}}_y\\ {\hat{t}}_z\end{array}\right)$$
理想状态下上式应该为0，然而总会有误差，尤其是参数计算的不是特别准的时候。根据误差阈值划分内点（inliers）和外点（outliers）。根据内点再次求解$\text{t}$，这次求的是$\text{t}$的最小二乘解，同样也是将某一维置一。

循环迭代，保留内点数最多的一次结果并标记内点。

我的想法：这一步根据内点去求$\text{t}$的最小二乘解我认为有点多余，因为这一步只是对函数中的临时变量this_error和best_error进行更新，而整个twoPointRansac函数要赋值的是inlier_markers，而inlier_markers又由best_inlier_set得到，这一路的变量关联为：

inlier_set —> best_inlier_set —> inlier_markers

而inlier_set在第一次求解$\text{t}$的时候（进行求解最下二乘解之前）就已经确定，并且在本次迭代中不会受到之后的代码改变。

d.把经过上述步骤后的跟踪后的特征点按一个个格子保存到curr_features_ptr中，然后根据prev_features_ptr中的特征点数目和curr_features_ptr中的特征点数目计算跟踪成功率。

2.2 addNewFeatures

完成上一步的跟踪后，检测新的特征。

(1)创建mask扣掉curr_features_ptr中已经存在的特征，避免重复检测。
(2)在mask的基础上检测左图像中新的特征，根据特征点分配的格子存储到一个二维vector中，然后每个格子的特征点根据响应值（response）排序，去除响应值较小的特征点，代码里是去掉排在第n个之后的特征点，如果这个格子内的特征点数目不足n，就全数保留。
(3)立体匹配stereoMatch。把匹配成功的点分配到grid_new_features里。然后计算curr_features_ptr中各个格子的vacancy_num，其中vacancy_num为当前格子内的最小特征点数目grid_min_feature_num减去当前格子内的特征点数。然后把grid_new_features中各个格子内的前vacancy_num个特征点分配到curr_features_ptr中并且赋上id。

2.3 pruneGridFeatures

pruneGridFeatures主要是为了去除连续跟踪次数较多的变量。首先将curr_features_ptr中每个格子内的特征根据连续跟踪次数(liftime)排序，如果这个格子内的特征点数目m超过grid_max_feature_num，则去除前(grid_max_feature_num - m)个跟踪次数最多的点。

2.4 发布特征点

将特征点进行去畸变，转化成归一化坐标发布出去；同时发布各阶段的跟踪数目。