ORB-SLAM2学习笔记——Initialize函数

1、 Initialize函数

mvIniMatches:帧1特征点在帧2中的匹配
//Rcw,tcw:待求相机位姿
//mvIniP3D:成功匹配点的空间3D坐标
//vbTriangulated:成功三角测量--ture,三角测量失败--false
bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,
                             vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)
{
    // Fill structures with current keypoints and matches with reference frame
    // Reference Frame: 1, Current Frame: 2
    mvKeys2 = CurrentFrame.mvKeysUn;
    //清除内存 分配空间 重新定义尺寸
    mvMatches12.clear();
    mvMatches12.reserve(mvKeys2.size());
    mvbMatched1.resize(mvKeys1.size());
    //把帧1与帧2的对应索引放入mvMatches12，true标志成功， false标志失败
    for(size_t i=0, iend=vMatches12.size();i<iend; i++)
    {
        if(vMatches12[i]>=0)
        {
            mvMatches12.push_back(make_pair(i,vMatches12[i]));
            mvbMatched1[i]=true;
        }
        else
            mvbMatched1[i]=false;
    }

    const int N = mvMatches12.size();

    // Indices for minimum set selection
    //建立盛装序号的容器 预存空间 然后把正确匹配的关键点索引给vAllIndices
    vector<size_t> vAllIndices;
    vAllIndices.reserve(N);
    vector<size_t> vAvailableIndices;

    for(int i=0; i<N; i++)
    {
        vAllIndices.push_back(i);
    }

    // Generate sets of 8 points for each RANSAC iteration
    //每一个RANSAC iteration都生成八对点 总共mMaxIterations组
    mvSets = vector< vector<size_t> >(mMaxIterations,vector<size_t>(8,0));
    //按照一定的规则产生随即种子
    DUtils::Random::SeedRandOnce(0);
    //遍历所有组对
    for(int it=0; it<mMaxIterations; it++)
    {
        //索引赋给中间变量
        vAvailableIndices = vAllIndices;

        // Select a minimum set
        //随机选取 mMaxIterations 组,每组包含8个点,存入 mvSets 中
        for(size_t j=0; j<8; j++)
        {
            //随即组合八个点
            int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);
            //提取索引号
            int idx = vAvailableIndices[randi];
            //索引号赋给mvsets
            mvSets[it][j] = idx;

            //删除中间变量的索引
            vAvailableIndices[randi] = vAvailableIndices.back();
            vAvailableIndices.pop_back();
        }
    }

    // Launch threads to compute in parallel a fundamental matrix and a homography
    //多线程并行计算本质矩阵和单应矩阵
    vector<bool> vbMatchesInliersH, vbMatchesInliersF;
    float SH, SF;
    cv::Mat H, F;
    //std::thread m_thrProcess(&类名::函数名, &类对象, 函数参数);计算F H 得分
    //thread的方法传递引用的时候，必须外层用ref来进行引用传递，否则就是浅拷贝（b=a a的地址赋值给b，a变了b也变）。
    thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
    thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));

    // Wait until both threads have finished
    //两个线程都结束才能开始后面的
    threadH.join();
    threadF.join();

    // Compute ratio of scores
    //计算得分来选择用F还是H计算
    float RH = SH/(SH+SF);

    // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
    //RH>0.4用H计算，否则用F计算
    if(RH>0.40)
        return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
    else //if(pF_HF>0.6)
        return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);

    return false;
}

2、FindHomography函数详解

这边计算一下本质矩阵，并计算出T：
FindHomography函数详解

3、ReconstructH函数

从本质矩阵中恢复出R t等参数

ReconstructH函数