【Vins-mono二】vins-mono的后端视觉sfm

vins-mono的视觉sfm作用：初始化all_image_frame中的R、T(T是尺度模糊的)，不涉及f_manager特征点操作
纯视觉的尺度都是模糊的，引入IMU信息后才能确定尺度

vins-mono的视觉sfm主要包括以下五步
注：在重力对齐之前，将枢纽帧l帧当成参考坐标系，即世界坐标系；尺度是模糊的

1、得到枢纽帧l帧和滑窗最后一帧WINDOW_SIZE的相对位姿$R_{cend}^{cl}$ $t_{cend}^{cl}$
已知枢纽帧l帧和滑窗最后一帧WINDOW_SIZE的2d归一化相机坐标，利用本质矩阵求得两帧间的相对姿态和相对平移$R_{cend}^{cl}$ $t_{cend}^{cl}$。
注：本质矩阵求得是$R_{cl}^{cend}$ $t_{cl}^{cend}$，程序中最后转置得到$R_{cend}^{cl}$ $t_{cend}^{cl}$

2、根据$R_{cend}^{cl}$ $t_{cend}^{cl}$，得到一批l帧的3d世界坐标
已知l帧的$R_{cl}^{cl}=I$, $t_{cl}^{cl}=0$和2d归一化相机坐标；滑窗最后一帧WINDOW_SIZE的$R_{cl}^{cend}=(R_{cend}^{cl}{)}^T$,$t_{cl}^{cend}$和2d归一化相机坐标，利用下面公式三角化得到l帧下的3d世界坐标。
注：三角化中转换矩阵的形式必须是$R_w^c$（即$R_l^c$）
$$p^c=R_l^c\ast P^l+t_l^c$$
式中，$p^c$是已知2d归一化坐标，$R_l^c$是已知两帧相对旋转；未知是3d点$P^l$

3、根据l帧下的3d世界坐标和l+1帧、l+2帧…的2d归一化坐标，得到l帧和l+1帧、l+2帧…的相对位姿
已知l帧下的3d世界坐标和l+1帧的2d归一化坐标，利用pnp求得$R_{cl}^{cl+1}$ $t_{cl}^{cl+1}$。重复pnp过程，直至得到所有滑窗内关键帧在l帧下的位姿
$$p^c=R_l^c\ast P^l+t_l^c$$
式中，$P^l$是已知3d点，$p^c$是已知2d归一化坐标；未知是两帧相对旋转$R_l^c$

4、把位姿和空间点放在一块，进行ceres优化
ceres优化，优化变量包括各帧的姿态$R_{cl}^{ck}$、平移$t_{cl}^{ck}$和三角化得到的3d点$P^l$。最后，转置得到最终结果$R_{ck}^{cl}$ $t_{ck}^{cl}$（ck指的是滑窗内的任意某帧）

5、
上面只是针对KF进行sfm，初始化需要all_image_frame中的所有元素的位姿

1. 第一步对应的代码

// 位置 bool Estimator::initialStructure()

Matrix3d relative_R;
Vector3d relative_T;
int l;
//relative_R = R_WINDOW_SIZE^l,l帧为参考坐标系
if (!relativePose(relative_R, relative_T, l)) // relative_R, relative_T是Rclck Tclck 
{
    ROS_INFO("Not enough features or parallax; Move device around");
    return false;
}

2. 第二~四步对应的代码

// 位置 bool Estimator::initialStructure()

// 进行sfm的求解 只针对关键帧
if(!sfm.construct(frame_count + 1, Q, T, l,  
          relative_R, relative_T,
          sfm_f, sfm_tracked_points))   //求解的Q T是Rclck Tclck
{
    ROS_DEBUG("global SFM failed!");
    marginalization_flag = MARGIN_OLD;
    return false;
}

3. 第五步对应的代码

// 位置 bool Estimator::initialStructure()

// Step 3 solve pnp for all frame
// step2只是针对KF进行sfm，初始化需要all_image_frame中的所有元素，因此下面通过KF来求解其他的非KF的位姿
map<double, ImageFrame>::iterator frame_it;
map<int, Vector3d>::iterator it;
frame_it = all_image_frame.begin( );
// i代表跟这个帧最近的KF的索引
for (int i = 0; frame_it != all_image_frame.end( ); frame_it++)
{
    // provide initial guess
    cv::Mat r, rvec, t, D, tmp_r;
    // 这一帧本身就是KF，因此可以直接得到位姿
    // 通过时间戳Headers[i]判断是不是关键帧，每一帧图像只对应一个时间戳
    if((frame_it->first) == Headers[i].stamp.toSec())
    {
        // 此时的Rs=Rclbk，表示枢纽帧cl下bk的姿态
    	// 此时的Ps=Pclck，不是Pclbk，参考坐标系仍是枢纽帧cl，表示枢纽帧测量下ck的位置
        frame_it->second.is_key_frame = true;
        frame_it->second.R = Q[i].toRotationMatrix() * RIC[0].transpose();  // 得到Rwi = Rclbk
        frame_it->second.T = T[i];  // 重要：初始化不估计平移外参 Tclck
        i++;
        continue;
    }//至此，滑窗内。R=Rclck*(Rbc)^T=Rclbk,Tclck    	  参考坐标系是枢纽帧cl


//如果在滑窗内，直接得到该帧的位姿。如果没在，通过下面的程序得到该帧的位姿
    if((frame_it->first) > Headers[i].stamp.toSec())
    {
        i++;
    }
    // 最近的KF提供一个初始值，Twc -> Tcw
    Matrix3d R_inital = (Q[i].inverse()).toRotationMatrix();
    Vector3d P_inital = - R_inital * T[i];
    // eigen -> cv
    cv::eigen2cv(R_inital, tmp_r);
    cv::Rodrigues(tmp_r, rvec);
    cv::eigen2cv(P_inital, t);

    frame_it->second.is_key_frame = false;
    vector<cv::Point3f> pts_3_vector;
    vector<cv::Point2f> pts_2_vector;
    // 遍历这一帧对应的特征点
    for (auto &id_pts : frame_it->second.points)
    {
        int feature_id = id_pts.first;
        // 由于是单目，这里id_pts.second大小就是1
        for (auto &i_p : id_pts.second)
        {
            it = sfm_tracked_points.find(feature_id);
            if(it != sfm_tracked_points.end())  // 有对应的三角化出来的3d点
            {
                Vector3d world_pts = it->second;    // 地图点的世界坐标
                cv::Point3f pts_3(world_pts(0), world_pts(1), world_pts(2));
                pts_3_vector.push_back(pts_3);
                Vector2d img_pts = i_p.second.head<2>();
                cv::Point2f pts_2(img_pts(0), img_pts(1));
                pts_2_vector.push_back(pts_2);
            }
        }
    }
    cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);     
    if(pts_3_vector.size() < 6)
    {
        cout << "pts_3_vector size " << pts_3_vector.size() << endl;
        ROS_DEBUG("Not enough points for solve pnp !");
        return false;
    }
    // 依然是调用opencv求解pnp接口,没有三角化
    if (! cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, 1))
    {
        ROS_DEBUG("solve pnp fail!");
        return false;
    }
    // cv -> eigen,同时Tcw -> Twc
    cv::Rodrigues(rvec, r);
    MatrixXd R_pnp,tmp_R_pnp;
    cv::cv2eigen(r, tmp_R_pnp);
    R_pnp = tmp_R_pnp.transpose();//Rcw -> Rwc
    MatrixXd T_pnp;
    cv::cv2eigen(t, T_pnp);
    T_pnp = R_pnp * (-T_pnp);
    // Twc -> Twi
    // 重要：由于尺度未恢复，因此平移暂时不转到imu系
    // 此时的Rs=Rclbk，表示枢纽帧cl下bk的姿态
    // 此时的Ps=Pclck，不是Pclbk，参考坐标系仍是枢纽帧cl，表示枢纽帧测量下ck的位置
    frame_it->second.R = R_pnp * RIC[0].transpose();
    frame_it->second.T = T_pnp;//注意不是Tclbk
//至此，滑窗外。R=Rclck*(Rbc)^T=Rclbk,Tclck			参考坐标系是枢纽帧cl
}

// 到此就求解出用来做视觉惯性对齐的所有视觉帧的位姿

注：此时的姿态是$R_{bk}^{cl}$，平移是 $t_{ck}^{cl}$，不一样