Vins-Fusion之 SFM准备篇（十二）

SFM 初始化需要一对有足够运动的帧作为起点，这个帧要满足：

a.有足够的匹配特征点（>20 个）

b.有足够的视差（parallax > 30 像素）

c.能成功估计相对位姿

所以就引入一个SFM准备篇，来解决起点的问题，这就是本篇的目的。

首先核心代码如下：

cpp 复制代码

bool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l)
{
    for (int i = 0; i < WINDOW_SIZE; i++)
    {
        vector<pair<Vector3d, Vector3d>> corres;
        corres = f_manager.getCorresponding(i, WINDOW_SIZE);//每一帧和最新帧的匹配点对
        if (corres.size() > 20)//匹配点超过 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());//归一化坐标下的平均视差
            //像素视差 > 30 像素表示两帧间运动足够大，有利于三角化和位姿估计
            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;
}

关键步骤说明：

1.遍历候选帧：从第 0 帧到 WINDOW_SIZE-1，依次检查与最新帧的匹配情况

2.匹配点数量检查：corres.size() > 20

3.视差计算：

归一化坐标下的视差：parallax = (pts_0 - pts_1).norm()

转换为像素视差：average_parallax * 460（460 是 FOCAL_LENGTH）

阈值：> 30 像素

4.相对位姿估计：调用 solveRelativeRT 估计相对旋转和平移

5.找到即返回：找到第一个满足条件的帧对就返回，不再继续

solveRelativeRT ：得到最新帧相对参考帧的旋转矩阵、平移向量

cpp 复制代码

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;
        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;
        int inlier_cnt = cv::recoverPose(E, ll, rr, cameraMatrix, rot, trans, mask);//恢复两帧之间的相对旋转和平移
        //cout << "inlier_cnt " << inlier_cnt << endl;
        Eigen::Matrix3d R;
        Eigen::Vector3d T;
        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);
        }

        Rotation = R.transpose();
        Translation = -R.transpose() * T;//想统一用「从较新帧到参考帧」的变换
        if(inlier_cnt > 12)
            return true;
        else
            return false;
    }
    return false;
}

要点：估计基础矩阵E后，再解算出R和T，转换到最新帧相对参考帧的Rotation和Translation，为后续的SFM初始化提供基础数据，这就是本篇的目的。