VINS --- 初始化 视觉SFM 部分

brief

把多年前个人网站的资料迁移一下，满满的回忆

(初始化过程中 )光流跟踪（2d-2d点对的匹配关系） -> 对极约束（分解本质矩阵得到两帧之间的RT）-> 再进行 三角化，得到3d坐标 ->进行PnP求解其他帧的位姿->然后利用新帧的位姿又可以进行三角化，再进行PnP

(初始化后的视觉跟踪，与IMU融合过程 ) 光流跟踪（2d-2d点对的匹配关系） -> 再进行 三角化，得到3d坐标 -> 然后使用IMU的积分位姿作为初始值，进行非线性优化求解（Ceres）

（对于没有使用IMU的 tracking ,如双目视觉里程计） 光流跟踪 ->三角化 -> 使用 PnP 构建本质矩阵分解得到初始位姿或者使用重投影方式求解位姿，作为初始值 -> 最后再是基于滑动窗口的位姿优化。

1. 选择参考帧，并计算出当前帧与参考帧之间的位姿

要求这一帧和最后一帧既有足够的共视 > 20对点，也要有足够的视差 （平均视差大于 average_parallax * 460 > 30）
relativePose(relative_R, relative_T, l)

/**
 * @brief 寻找滑窗内一个帧作为枢纽帧，要求和最后一帧既有足够的共视也要有足够的视差
 *        这样其他帧都对齐到这个枢纽帧上
 *        得到T_l_last
 * @param[in] relative_R 
 * @param[in] relative_T 
 * @param[in] l 
 * @return true 
 * @return false 
 */

bool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l)
{
    // find previous frame which contians enough correspondance and parallex with newest frame
    // 优先从最前面开始
    for (int i = 0; i < WINDOW_SIZE; i++)
    {
        vector<pair<Vector3d, Vector3d>> corres;
        corres = f_manager.getCorresponding(i, WINDOW_SIZE);
        // 要求共视的特征点足够多
        if (corres.size() > 20)
        {
            double sum_parallax = 0;
            double average_parallax;
            for (int j = 0; j < int(corres.size()); j++)
            {
                Vector2d pts_0(corres[j].first(0), corres[j].first(1));
                Vector2d pts_1(corres[j].second(0), corres[j].second(1));
                double parallax = (pts_0 - pts_1).norm(); // 计算了视差
                sum_parallax = sum_parallax + parallax;
            }
            // 计算每个特征点的平均视差
            average_parallax = 1.0 * sum_parallax / int(corres.size());
            // 有足够的视差在通过本质矩阵恢复第i帧和最后一帧之间的 R t T_i_last
            if (average_parallax * 460 > 30 && m_estimator.solveRelativeRT(corres, relative_R, relative_T))
            { 
                l = i;
                ROS_DEBUG("average_parallax %f choose l %d and newest frame to triangulate the whole structure", average_parallax * 460, l);
                return true;
            }
        }
    }
    return false;
}

findFundamentalMat 得到基础矩阵，再分别得到位姿 （R和带尺度的t）

/**
 * @brief 根据两帧匹配对求解R和带尺度的t
 * 
 * @param[in] corres 
 * @param[in] Rotation 
 * @param[in] Translation 
 * @return true 
 * @return false 
 */

bool MotionEstimator::solveRelativeRT(const vector<pair<Vector3d, Vector3d>> &corres, Matrix3d &Rotation, Vector3d &Translation)
{
    if (corres.size() >= 15)
    {
        vector<cv::Point2f> ll, rr;
        for (int i = 0; i < int(corres.size()); i++)
        {
            ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));
            rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
        }
        cv::Mat mask;
        // 调用opencv接口求解E矩阵
        cv::Mat E = cv::findFundamentalMat(ll, rr, cv::FM_RANSAC, 0.3 / 460, 0.99, mask);
        // 已经是归一化相机坐标系了，因此内参阵用单位阵
        cv::Mat cameraMatrix = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);
        cv::Mat rot, trans;
        // 用opencv接口恢复R和t
        int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask);
        //cout << "inlier_cnt " << inlier_cnt << endl;

        Eigen::Matrix3d R;
        Eigen::Vector3d T;
        // cv -> eigen
        for (int i = 0; i < 3; i++)
        {   
            T(i) = trans.at<double>(i, 0);
            for (int j = 0; j < 3; j++)
                R(i, j) = rot.at<double>(i, j);
        }
        // opencv得到的是T21,这里换成T12
        Rotation = R.transpose();
        Translation = -R.transpose() * T;
        if(inlier_cnt > 12)
            return true;
        else
            return false;
    }
    return false;
}

2.进行sfm的求解所有帧的位姿与3d坐标点

根据已有的枢纽帧/参考帧和最后一帧的位姿变换，得到各帧位姿和3d点坐标，最后通过ceres进行优化

• 枢纽帧/参考帧设置为单位帧，也可以理解为世界系原点

q[l].w() = 1;
q[l].x() = 0;
q[l].y() = 0;
q[l].z() = 0;
T[l].setZero();

// 求得最后一帧的位姿
q[frame_num - 1] = q[l] * Quaterniond(relative_R);
T[frame_num - 1] = relative_T;

• 使用 PnP 与 三角化 窗口中其他关键帧与特征点坐标 ，都表示为 Tcw
  得到了所有关键帧的位姿，遍历没有被三角化的特征点，进行三角化
• ceres BA 求解
  【固定参考帧位姿（世界坐标系原点），固定最后一帧的（当前帧）的平移，也就是固定这个尺度信息，这两帧之间只优化旋转量】

// 由于是单目视觉slam，有七个自由度不可观，因此，fix一些参数块避免在零空间漂移
// fix设置的世界坐标系第l帧的位姿，同时fix最后一帧的位移用来fix尺度
if (i == l)
{
    problem.SetParameterBlockConstant(c_rotation[i]);
}
if (i == l || i == frame_num - 1)
{
    problem.SetParameterBlockConstant(c_translation[i]);
}

• 最后求解所有的帧，除关键帧外的其他帧 solvePnP（）求解

优化之后 ： 由于尺度未恢复，因此平移暂时不转到imu系 （得到尺度后再更新）

 // Twc -> Twi
        // 由于尺度未恢复，因此平移暂时不转到imu系
        frame_it->second.R = R_pnp * RIC[0].transpose();
        frame_it->second.T = T_pnp;

3. 使用所有关键帧的位姿，即可以与IMU进行对齐

再求解 bg ，g ，s ，V[10]